TankDrive code

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2019.1.1"
+    id "edu.wpi.first.GradleRIO" version "2019.3.2"
 }
 
 def ROBOT_MAIN_CLASS = "frc.robot.Main"

src/main/java/frc/robot/OI.java

@@ -7,6 +7,7 @@
 
 package frc.robot;
 
+import edu.wpi.first.wpilibj.Joystick;
 /**
  * This class is the glue that binds the controls on the physical operator
  * interface to the commands and command groups that allow control of the robot.
@@ -20,12 +21,16 @@ public class OI {
   // Joystick stick = new Joystick(port);
   // Button button = new JoystickButton(stick, buttonNumber);
 
+  public static Joystick joyLeft = new Joystick(0); // port subject to change
+  public static Joystick joyRight = new Joystick(1);
+
+
   // There are a few additional built in buttons you can use. Additionally,
   // by subclassing Button you can create custom triggers and bind those to
   // commands the same as any other Button.
 
   //// TRIGGERING COMMANDS WITH BUTTONS
-  // Once you have a button, it's trivial to bind it to a button in one of
+  // Once you have a button, it's trivial to bind it to a button in oneP of
   // three ways:
 
   // Start the command when the button is pressed and let it run the command
@@ -34,9 +39,7 @@ public class OI {
 
   // Run the command while the button is being held down and interrupt it once
   // the button is released.
-  // button.whileHeld(new ExampleCommand());
-
-  // Start the command when the button is released and let it run the command
-  // until it is finished as determined by it's isFinished method.
-  // button.whenReleased(new ExampleCommand());
+  public OI() {
+
+  }
 }

src/main/java/frc/robot/Robot.java

@@ -12,8 +12,10 @@
 import edu.wpi.first.wpilibj.command.Scheduler;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc.robot.commands.ExampleCommand;
-import frc.robot.subsystems.ExampleSubsystem;
+import frc.robot.subsystems.EncoderTest;
+import frc.robot.subsystems.Gyroscope;
+import frc.robot.subsystems.PIDControl;
+import frc.robot.subsystems.TankDrive;
 
 /**
  * The VM is configured to automatically run this class, and to call the
@@ -23,22 +25,24 @@
  * project.
  */
 public class Robot extends TimedRobot {
-  public static ExampleSubsystem m_subsystem = new ExampleSubsystem();
-  public static OI m_oi;
+  public static TankDrive tankDriveSubsystem = new TankDrive();
+  public static EncoderTest encoderSub = new EncoderTest();
+  public static Gyroscope gyroSub = new Gyroscope();
+  public static PIDControl pidSub = new PIDControl(10);//place holder for distance
+  public static OI oi;
 
-  Command m_autonomousCommand;
-  SendableChooser<Command> m_chooser = new SendableChooser<>();
+  Command autonomousCommand;
+  SendableChooser<Command> chooser = new SendableChooser<>();
 
   /**
    * This function is run when the robot is first started up and should be
    * used for any initialization code.
    */
   @Override
   public void robotInit() {
-    m_oi = new OI();
-    m_chooser.setDefaultOption("Default Auto", new ExampleCommand());
+    oi = new OI();
     // chooser.addOption("My Auto", new MyAutoCommand());
-    SmartDashboard.putData("Auto mode", m_chooser);
+    SmartDashboard.putData("Auto mode", chooser);
   }
 
   /**
@@ -80,7 +84,7 @@ public void disabledPeriodic() {
    */
   @Override
   public void autonomousInit() {
-    m_autonomousCommand = m_chooser.getSelected();
+    autonomousCommand = chooser.getSelected();
 
     /*
      * String autoSelected = SmartDashboard.getString("Auto Selector",
@@ -90,8 +94,8 @@ public void autonomousInit() {
      */
 
     // schedule the autonomous command (example)
-    if (m_autonomousCommand != null) {
-      m_autonomousCommand.start();
+    if (autonomousCommand != null) {
+      autonomousCommand.start();
     }
   }
 
@@ -109,8 +113,8 @@ public void teleopInit() {
     // teleop starts running. If you want the autonomous to
     // continue until interrupted by another command, remove
     // this line or comment it out.
-    if (m_autonomousCommand != null) {
-      m_autonomousCommand.cancel();
+    if (autonomousCommand != null) {
+      autonomousCommand.cancel();
     }
   }
 

src/main/java/frc/robot/RobotMap.java

@@ -7,6 +7,8 @@
 
 package frc.robot;
 
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
 /**
  * The RobotMap is a mapping from the ports sensors and actuators are wired into
  * to a variable name. This provides flexibility changing wiring, makes checking
@@ -23,4 +25,6 @@ public class RobotMap {
   // number and the module. For example you with a rangefinder:
   // public static int rangefinderPort = 1;
   // public static int rangefinderModule = 1;
+
+  public static WPI_TalonSRX TANK_DRIVE_TOP_LEFT = new WPI_TalonSRX(15); //port # not yet known? to be attached with encoder
 }

src/main/java/frc/robot/commands/ExampleCommand.java → src/main/java/frc/robot/commands/AutonDriveCommand.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,13 +10,10 @@
 import edu.wpi.first.wpilibj.command.Command;
 import frc.robot.Robot;
 
-/**
- * An example command.  You can replace me with your own command.
- */
-public class ExampleCommand extends Command {
-  public ExampleCommand() {
+public class AutonDriveCommand extends Command { // yet to be tested
+  public AutonDriveCommand() {
     // Use requires() here to declare subsystem dependencies
-    requires(Robot.m_subsystem);
+    requires(Robot.pidSub);
   }
 
   // Called just before this Command runs the first time
@@ -32,7 +29,7 @@ protected void execute() {
   // Make this return true when this Command no longer needs to run execute()
   @Override
   protected boolean isFinished() {
-    return false;
+    return Robot.pidSub.encoderPID.onTarget(); // stop when PID reads that we've met designated position. onTarget() will == true
   }
 
   // Called once after isFinished returns true

src/main/java/frc/robot/commands/DriveCommand.java

@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.Robot;
+
+public class DriveCommand extends Command { // yet to be tested
+  public DriveCommand() {
+    // Use requires() here to declare subsystem dependencies
+    // eg. requires(chassis);
+    requires(Robot.tankDriveSubsystem);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+    Robot.tankDriveSubsystem.init();
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  protected void execute() {
+    Robot.tankDriveSubsystem.driveWithJoystick();
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  protected boolean isFinished() {
+    return false;
+  }
+
+  // Called once after isFinished returns true
+  @Override
+  protected void end() {
+  }
+
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  @Override
+  protected void interrupted() {
+    Robot.tankDriveSubsystem.stop();
+  }
+}

src/main/java/frc/robot/commands/EncoderCommand.java

@@ -0,0 +1,41 @@
+package frc.robot.commands;
+
+import frc.robot.Robot;
+import frc.robot.RobotMap;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class EncoderCommand extends Command { // yet to be tested
+    public EncoderCommand() {
+        requires(Robot.encoderSub);
+    }
+    // Called just before this Command runs the first time
+    @Override
+    protected void initialize() { 
+        RobotMap.TANK_DRIVE_TOP_LEFT.setSelectedSensorPosition(0); 
+    }
+
+    // Called repeatedly when this Command is scheduled to run
+    @Override
+    protected void execute() {
+        SmartDashboard.putNumber("Position", Robot.encoderSub.getPosition());
+        Robot.encoderSub.goStraight(10); //Parameter is a placeholder = 10 inch to move
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return false;
+    }
+
+    // Called once after isFinished returns true
+    @Override
+    protected void end() {
+
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    @Override
+    protected void interrupted() {
+    }
+}

src/main/java/frc/robot/commands/GoToAngle.java

@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.Robot;
+
+public class GoToAngle extends Command { //yet to be tested
+
+  public double finalAngle;
+  public double finalAngle1;
+  public double specifiedAngle;
+
+  public GoToAngle(double pfinalAngle1, double pfinalAngle2, double pSpecifiedAngle) {
+    requires(Robot.gyroSub);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+    Robot.gyroSub.gyro.reset();
+    Robot.tankDriveSubsystem.driveToAngle(specifiedAngle); //autonomous?
+    //Robot.gyroSub.motor.set(0.5);
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  protected void execute() {
+    SmartDashboard.putNumber("Current angle ", Robot.gyroSub.gyro.getAngle());
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  protected boolean isFinished() {
+    SmartDashboard.putBoolean("finished", (Robot.gyroSub.gyro.getAngle() >= finalAngle  && Robot.gyroSub.gyro.getAngle() <= finalAngle1));
+    return (Robot.gyroSub.gyro.getAngle() >= finalAngle && Robot.gyroSub.gyro.getAngle() <= finalAngle1); 
+  }
+
+  // Called once after isFinished returns true
+  @Override
+  protected void end() {
+    Robot.tankDriveSubsystem.stop();
+    Robot.gyroSub.stop();
+
+  }
+
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  @Override
+  protected void interrupted() {
+    Robot.tankDriveSubsystem.stop();
+    Robot.gyroSub.stop();
+  }
+}

src/main/java/frc/robot/subsystems/EncoderTest.java

@@ -0,0 +1,49 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+import frc.robot.Robot;
+import frc.robot.RobotMap;
+
+public class EncoderTest extends Subsystem { // yet to be tested
+
+    public EncoderTest() {
+        RobotMap.TANK_DRIVE_TOP_LEFT.setSensorPhase(true); 
+        RobotMap.TANK_DRIVE_TOP_LEFT.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 1, 10);
+        RobotMap.TANK_DRIVE_TOP_LEFT.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 10);
+    }
+
+    public double getPosition() {
+        return (double)RobotMap.TANK_DRIVE_TOP_LEFT.getSelectedSensorPosition(); // where the robot is 
+    }
+
+    public void goStraight(double distance) { //where 6 is the circumference of the wheels and distance is in inches
+        double rotations = distance / (6 * Math.PI); //gets the number of times the wheel needs to rotate to reach a specified distance (inch) (rotations = distance/circumference)
+        double units = 400 * rotations; // gets the total number of raw sensor units correlated with specified distance
+        Robot.tankDriveSubsystem.driveForward(); //Drives with 0.5 speed
+        if (getPosition() >= units) //determines if the raw sensor units thus far equates to that of specified distance. if so, stop moving
+            Robot.tankDriveSubsystem.stop(); //stops all motors
+    }
+
+    public void turnToAngle(double distance, double angle) {
+        double rotations = distance / (6 * Math.PI); //gets the number of times the wheel needs to rotate to reach a specified distance (in) (rotations = distance/circumference)
+        double units = 400 * rotations; // gets the total number of raw sensor units correlated with specified distance
+        Robot.tankDriveSubsystem.driveToAngle(angle); //Drives with 0.5 speed
+        if (getPosition() >= units) //determines if the raw sensor units thus far equates to that of specified distance. if so, stop moving
+            Robot.tankDriveSubsystem.stop(); //stops all motors
+    }
+
+    public void turnToAngle(double angleOfTurn) { 
+        double circumferenceOfRobot = Math.PI * 22.74; //units are in inches, where 22.74 is a measure of the distance between each wheel
+        double distance = (double)(circumferenceOfRobot * (angleOfTurn/360)); //gets the distance needed to turn a specified angle
+        turnToAngle(distance, angleOfTurn); // calls on auton tank drive for turning to angles
+    }
+
+    @Override
+    protected void initDefaultCommand() {
+        //setDefaultCommand(new EncoderCommand());
+    }
+
+}