add clean DriveForTime command

diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/DriveForTime.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/DriveForTime.java
new file mode 100644 (file)
index 0000000..f662e4b
--- /dev/null
+++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/DriveForTime.java
@@ -0,0 +1,59 @@
+package org.usfirst.frc3501.RiceCatRobot.commands;
+
+import org.usfirst.frc3501.RiceCatRobot.Robot;
+import org.usfirst.frc3501.RiceCatRobot.RobotMap.Direction;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * This command takes a time in seconds which is how long it should run
+ *
+ */
+public class DriveForTime extends Command {
+  private double seconds;
+  private Timer timer;
+  private double speed;
+
+  /***
+   * Drive at a fixed speed (speed) for a fixed time (seconds).
+   *
+   * @param seconds
+   *          the number of seconds to drive
+   * @param speed
+   *          a motor value in the range [-1, 1]. Negative numbers are
+   *          interpreted as driving backwards. 0 is stopped.
+   */
+  public DriveForTime(double seconds, double speed) {
+    this.seconds = seconds;
+    this.speed = -speed; // note: setMotorSpeeds(-1, -1) would be
+    // forward full speed, so we take the opposite
+    // of the input to achieve this.
+  }
+
+  @Override
+  protected void initialize() {
+    this.setTimeout(seconds);
+    Robot.driveTrain.setMotorSpeeds(speed, speed);
+  }
+
+  @Override
+  protected void execute() {
+    // nothing here because motors are already set
+  }
+
+  @Override
+  protected boolean isFinished() {
+    return this.isTimedOut();
+  }
+
+  @Override
+  protected void end() {
+    Robot.driveTrain.stop();
+  }
+
+  @Override
+  protected void interrupted() {
+    end();
+  }
+}

diff --git a/src/org/usfirst/frc3501/RiceCatRobot/subsystems/DriveTrain.java b/src/org/usfirst/frc3501/RiceCatRobot/subsystems/DriveTrain.java
index 49ffefa32b0b5fff9c304e0462721ae55c17863c..4117c6a45ec2635608d6aaba91aaa7f4c0c5fe5f 100644 (file)
--- a/src/org/usfirst/frc3501/RiceCatRobot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc3501/RiceCatRobot/subsystems/DriveTrain.java
@@ -104,4 +104,8 @@ public class DriveTrain extends Subsystem {
   @Override
   protected void initDefaultCommand() {
   }
+
+  public void stop() {
+    setMotorSpeeds(0, 0);
+  }
 }