Drive with Time command complete

src/main/java/frc/robot/RobotContainer.java

@@ -19,6 +19,7 @@
 import frc.robot.sensors.AnalogSelector;
 import frc.robot.subsystems.*;
 import frc.robot.commands.ArcadeDriveWithJoystick;
+import frc.robot.commands.DriveWithDistance;
 import frc.robot.commands.DriveWithTime;
 
 import java.util.logging.Logger;
@@ -36,8 +37,8 @@ public class RobotContainer {
    * The container for the robot. Contains subsystems, OI devices, and commands.
    */
 
-    // RobotContainer is a singleton class
-    private static RobotContainer currentInstance;
+  // RobotContainer is a singleton class
+  private static RobotContainer currentInstance;
 
   // The robot's subsystems and commands are defined here...    
   private Joystick driverStick;
@@ -48,16 +49,13 @@ public class RobotContainer {
   private Command intakeCommand;
   private Command reverseFeeder;
   private Command moveArmToSetpoint;
-    private Command reverseArmManually;
+  private Command reverseArmManually;
   private Command positionPowercell;
   private Command rampShooterCommand;
   private Command incrementFeeder;
   private Command perfectPosition;
   private Logger logger = Logger.getLogger("RobotContainer");
-  private final double AUTO_DRIVE_TIME = 1.0;
-  private final double AUTO_LEFT_MOTOR_SPEED = 0.2;
-  private final double AUTO_RIGHT_MOTOR_SPEED = 0.2;
-    private Command runFeeder;
+  private Command runFeeder;
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -84,7 +82,7 @@ private void configureButtonBindings() {
         driverStick = new Joystick(0);
         controlStick = new Joystick(1);
 
-//        // Instantiate the intake command and bind it
+        // Instantiate the intake command and bind it
         intakeCommand = new OperatorIntakeCommand();
         new JoystickButton(controlStick, XboxController.Button.kBumperLeft.value).whenHeld(intakeCommand);
 
@@ -119,26 +117,38 @@ private void configureButtonBindings() {
      * @return the command to run in autonomous
      */
     public Command getAutonomousCommand() {
-        int selectorOne = 1, selectorTwo = 1;
 
-        if (analogSelectorOne != null){
+        //Robot has only one selector
+        int selectorOne = 0;
+        if (analogSelectorOne != null)
             selectorOne = analogSelectorOne.getIndex();
-        }
-        logger.info("Selectors: " + selectorOne + " " + selectorTwo);
+        logger.info("Selectors: " + selectorOne);
 
         if (selectorOne == 0) {
             // This is our 'do nothing' selector
             return null;
         } else if (selectorOne == 1) {
             return new SpinUpShooterWithTime(Config.RPM, 7).alongWith(new RunFeederCommandWithTime(-0.7, 7)).andThen(new DrivetrainPIDTurnDelta(45, 0, 5, 3.0));
-           // return new DriveWithTime(AUTO_DRIVE_TIME,  AUTO_LEFT_MOTOR_SPEED,  AUTO_RIGHT_MOTOR_SPEED);
+
         } else if(selectorOne == 2) {
             return new DrivetrainPIDTurnDelta(45, 0, 5, 3.0);
+
+        } else if (selectorOne == 3) {
+         /*
+          * When the selector is set to one, the robot will run for x seconds at y left motor speed and z right motor speed
+          */
+
+              return new DriveWithTime(Config.AUTO_DRIVE_TIME, Config.AUTO_LEFT_MOTOR_SPEED, Config.AUTO_RIGHT_MOTOR_SPEED);
+         }
+
+        else if(selectorOne == 4){
+
+          //List distance, then the drive unit (in option of meters, cm, inches or feet), and then the right and left speed (if not specified, it is 0.5)
+          //Robot drives one meter forward  
+          return new DriveWithDistance(Config.AUTO_DISTANCE, Config.DEFAULT_UNIT, Config.AUTO_RIGHT_MOTOR_SPEED, Config.AUTO_LEFT_MOTOR_SPEED);
+
         }
 
-
-
-
         // Also return null if this ever gets to here because safety
         return null;
     }

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

@@ -0,0 +1,196 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.DriveBase;
+import frc.robot.subsystems.DriveBaseHolder;
+import frc.robot.subsystems.DriveBase.DistanceType;
+
+public class DriveWithDistance extends CommandBase {
+
+  //Initializing private version of drivebase object
+  private final DriveBase driveBase;
+
+  //The speed that the robot must move below to move backwards
+  private final static double ZERO_SPEED = 0;
+
+  private final static double DEFAULT_RIGHT_SPEED = 0.5;
+  private final static double DEFAULT_LEFT_SPEED = 0.5;
+
+  // initialization of the variables
+  public double desiredLeftDistance = 0;
+  public double desiredRightDistance = 0;
+  public double desiredRightEncoderTicks, desiredLeftEncoderTicks;
+  public double leftSpeed = 0;
+  public double rightSpeed = 0;
+  public DistanceType distanceType;
+  public double currentConversion;
+  public boolean commandFinished = false;
+
+  /**
+   * Constructor to be used when the input specifications do not provide the right and left speed (assumed to be 0.5)
+   * 
+   * @param distance The distance to drive
+   * @param distanceType The distance unit type to drive in
+   */
+
+  public DriveWithDistance(double distance, DistanceType distanceType) {
+    this(distance, distanceType, DEFAULT_RIGHT_SPEED, DEFAULT_LEFT_SPEED);
+  }
+
+  /**
+   * Constructor to be used when the input gives the right and left speed
+   * 
+   * @param distance     The distance to drive
+   * @param distanceType The distance unit type to drive in
+   * @param rightSpeed   The right motor speed
+   * @param leftSpeed    The left motor speed
+   */
+  public DriveWithDistance(double distance, DistanceType distanceType, double rightSpeed, double leftSpeed) {
+    this(distance, distance, distanceType, rightSpeed, leftSpeed);
+  }
+
+
+   /**
+   * Constructor to be used when the input gives the right and left distance
+   * 
+   * @param rightDistance The distance to drive for right motor
+   * @param leftDistance The distance to drive for left motor
+   * @param distanceType The distance unit type to drive in
+   * @param rightSpeed The right motor speed
+   * @param leftSpeed The left motor speed
+   */
+  public DriveWithDistance(double rightDistance, double leftDistance, DistanceType distanceType, double rightSpeed, double leftSpeed) {
+    //Initating drivebase
+    this.driveBase = DriveBaseHolder.getInstance();
+
+    //Setting current distance unit
+    this.distanceType = distanceType;
+
+    addRequirements(driveBase);
+
+    assignDistanceType(distanceType);
+
+    //Setting the right speed and left speed according to input specifications
+    this.rightSpeed = rightSpeed;
+    this.leftSpeed = leftSpeed;
+
+    //Determing the amount of desired encoder ticks to drive by getting the distnace and multiplying by the chosen distance unit
+    this.desiredRightEncoderTicks = rightDistance * this.currentConversion;   
+    this.desiredLeftEncoderTicks = leftDistance * this.currentConversion;
+
+  }
+
+  /**
+   * Assigns the current conversion from encoder ticks to distance type
+   *
+   * @param distanceType    The distance unit that the robot is using for driving
+   */
+  private void assignDistanceType(DistanceType distanceType) {
+    //Setting the current conversion type according to the chosen distance unit
+    switch (distanceType) {
+      case FEET:
+        this.currentConversion = DriveBase.DistanceUnit.FEET.encoderTicks;
+
+        break;
+      case METERS:
+        this.currentConversion = DriveBase.DistanceUnit.METERS.encoderTicks;
+
+        break;
+      case CENTIMETERS:
+        this.currentConversion = DriveBase.DistanceUnit.CENTIMETERS.encoderTicks;
+
+        break;
+      case INCHES:
+        this.currentConversion = DriveBase.DistanceUnit.INCHES.encoderTicks;
+
+        break;
+
+      //Default is meters
+      default:
+
+        this.currentConversion = DriveBase.DistanceUnit.METERS.encoderTicks;
+        break;
+
+    }
+  }
+
+  /**
+   * Called  one when the command is initially scheduled and determines the desired distance to travel
+   * 
+   */
+  @Override
+  public void initialize() {
+
+    //Add the desired amount of encoder ticks to the current distance to get the amount of encoder ticks that the robot needs to drive
+    desiredRightDistance = driveBase.getRightDistance() + desiredRightEncoderTicks;
+    desiredLeftDistance = driveBase.getLeftDistance() + desiredLeftEncoderTicks;
+
+    addRequirements(DriveBaseHolder.getInstance());
+  }
+
+  /**
+   * Called every time the scheduler runs while the command is scheduled. (run every 20ms) and determines the current distance
+   * 
+   */
+  @Override
+  public void execute() {
+
+    //Get the current distance of the right encoder and store value in variable
+    double currentRightDistance = DriveBaseHolder.getInstance().getRightDistance();  
+    double currentLeftDistance = DriveBaseHolder.getInstance().getLeftDistance();
+
+    //Run motors according to output of the speeds
+    driveBase.tankDrive(leftSpeed, rightSpeed, false);
+
+    //Dashboard
+    SmartDashboard.putNumber("Left Speed", leftSpeed);
+    SmartDashboard.putNumber("Right Speed", rightSpeed);
+    SmartDashboard.putNumber("Left Error", desiredLeftDistance - currentLeftDistance);
+    SmartDashboard.putNumber("Right Error", desiredRightDistance - currentRightDistance);
+
+    //Code for figuring out when the robot is finished driving and handling user input validation
+
+    // Update to the current right/left distance that the robot has driven
+    currentRightDistance = DriveBaseHolder.getInstance().getRightDistance();
+    currentLeftDistance = DriveBaseHolder.getInstance().getLeftDistance();
+
+    // If the robot has reached or surpassed the desired distance, then stop the
+    // robot. Otherwise, keep moving (moving forward OR moving backwards).
+    if (Math.abs(currentRightDistance) >= desiredRightDistance) {
+      rightSpeed = ZERO_SPEED;
+    }
+
+    // If the robot has reached or surpassed the desired distance, then stop the
+    // robot. Otherwise, keep moving (moving forward).
+    if (Math.abs(currentLeftDistance) >= desiredLeftDistance) {
+      leftSpeed = ZERO_SPEED;
+    }
+
+    //If the left and right speed are zero, just stop the robot
+    if ((leftSpeed == ZERO_SPEED && rightSpeed == ZERO_SPEED) ||
+            (desiredLeftDistance >= currentLeftDistance && desiredRightDistance >= currentRightDistance)) {
+      commandFinished = true;
+    }
+
+    //Taking the changed left and right speed and adding their updated values to the drivebase
+    driveBase.tankDrive(leftSpeed, rightSpeed, false);
+  }
+
+  /**
+   * Determines when the command is finished using the current distnce and desired distance
+   * 
+   */
+
+  @Override
+  public boolean isFinished() {
+    return commandFinished;
+  }
+}

src/main/java/frc/robot/config/Config.java

@@ -134,8 +134,14 @@ private Config() {
     public static double ARM_PID_D = robotSpecific(0.0);
     public static double ARM_PID_F = robotSpecific(0.0);
 
+    public static final double AUTO_LEFT_MOTOR_SPEED = 0.2;
+    public static final double AUTO_RIGHT_MOTOR_SPEED = 0.2;
+    public static final double AUTO_DISTANCE = 1;
+    public static final double AUTO_DRIVE_TIME = 1.0;
+    public static final DriveBase.DistanceType DEFAULT_UNIT= DriveBase.DistanceType.METERS;
+
     public static final int RPM = 1200;
-    
+
     // Define a global constants table for subsystems to use
     public static NetworkTable constantsTable = NetworkTableInstance.getDefault().getTable("constants");
 

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

@@ -153,6 +153,47 @@ public final NeutralMode getNeutralMode() {
      */
     protected void driveModeUpdated(DriveMode driveMode) {
 
+    }
+
+    /**
+     * Get the distance travelled by the left encoder in encoder ticks
+     *
+     * @return The distance of the right encoder (0 before overide)
+     */
+    public double getLeftDistance(){
+        return 0;
+    }
+
+    /**
+     * Get the distance travelled by the right encoder in encoder ticks
+     *
+     * @return The distance of the right encoder (0 before overide)
+     */
+    public double getRightDistance() {
+        return 0;
+    }
+
+    /**
+     * Get the speed of the left encoder in encoder ticks per second
+     *
+     * @return The speed of the left encoder (0 before overide)
+     */
+    public double getLeftSpeed() {
+        return 0;
+    }
+
+    /**
+     * Get the speed of the right encoder in encoder ticks per second
+     *
+     * @return The speed of the right encoder (0 before overide)
+     */
+    public double getRightSpeed() {
+        return 0;
+    }
+
+    //Reset encoder values
+    public void resetEncoders() {
+
     }
 
     /**
@@ -163,4 +204,43 @@ protected void driveModeUpdated(DriveMode driveMode) {
     protected void neutralModeUpdated(NeutralMode neutralMode) {
 
     }
+
+    /**
+     * Measured distance units for converting ticks to units or units to encoder ticks
+     * 
+     */
+    public enum DistanceUnit {
+
+        //Encoder tick is equal to itself
+        ENCODER_TICKS(1d),
+
+        //Measured value of 8583 ticks per meter
+        METERS(8583.1d),
+
+        //1 cm = 0.01 meters. 8583 * 0.01 = 858.3. Rounding to 858 ticks.
+        CENTIMETERS(858.1d),
+
+        //1 foot is equal to 0.3048 meters. 8583 * 0.3048 = 2616.1 (approx). Rounding to 2616 ticks per foot
+        FEET(2616.1d),
+
+        //39.3701 inches is equal to 1 meter. 8583 / 39.3701 = 218.008 (approx). Rounding to 218 ticks per inch
+        INCHES(218.1d);
+
+        //Update encoder ticks with unit
+        public double encoderTicks;
+        DistanceUnit(double encoderTicks) {
+            this.encoderTicks = encoderTicks;
+        }
+    }
+
+    /**
+     * Distance unts to travel 
+     * 
+     */
+    public enum DistanceType {
+
+        //Different distance units
+        CENTIMETERS, METERS, INCHES, FEET; 
+
+    }
 }