add DriveDistance command

diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/DriveDistance.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/DriveDistance.java
new file mode 100644 (file)
index 0000000..75ae227
--- /dev/null
+++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/DriveDistance.java
@@ -0,0 +1,51 @@
+package org.usfirst.frc3501.RiceCatRobot.commands;
+
+import org.usfirst.frc3501.RiceCatRobot.Robot;
+import org.usfirst.frc3501.RiceCatRobot.RobotMap.Direction;
+import org.usfirst.frc3501.RiceCatRobot.subsystems.DriveTrain;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * This command will drive the specified distance at specified speed.
+ *
+ */
+public class DriveDistance extends Command {
+  private double distance; // in units of cm
+  private double speed;
+
+  public DriveDistance(double distance, double speed) {
+    // Since negative values in setMotorSpeeds(...) are forwards, we reverse
+    // speed here so positive input values will move the robot forwards.
+    this.speed = -speed;
+    this.distance = distance;
+  }
+
+  @Override
+  protected void initialize() {
+    Robot.driveTrain.resetEncoders();
+    Robot.driveTrain.setMotorSpeeds(speed, speed);
+  }
+
+  @Override
+  protected void execute() {
+    // nothing to do since motor speeds already set
+  }
+
+  @Override
+  protected boolean isFinished() {
+    return Robot.driveTrain.getLeftDistance() >= distance
+        && Robot.driveTrain.getRightDistance() >= distance;
+  }
+
+  @Override
+  protected void end() {
+    Robot.driveTrain.stop();
+  }
+
+  @Override
+  protected void interrupted() {
+    end();
+  }
+}
\ No newline at end of file