clean up DriveForTime

[3501/2015-FRC-Spark] / src / org / usfirst / frc3501 / RiceCatRobot / commands / DriveForTime.java

diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/DriveForTime.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/DriveForTime.java
index 9f7d91bf2009f184e2c9a0f6b151014baf25fe17..a268a10704345efa763e0a03f23aefd105971713 100644 (file)
--- a/src/org/usfirst/frc3501/RiceCatRobot/commands/DriveForTime.java
+++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/DriveForTime.java
@@ -14,42 +14,33 @@ import edu.wpi.first.wpilibj.command.Command;
 public class DriveForTime extends Command {
   private double seconds;
   private double speed;
-  private Timer timer;
-  private Direction direction;
-
-  public DriveForTime(double seconds, double speed, Direction direction) {
-    // limit speed to the range [0, MOTOR_MAX_VAL]
-    this.speed = Math.max(speed, -speed);
-    this.speed = Math.min(speed, DriveTrain.MOTOR_MAX_VAL);
 
+  public DriveForTime(double seconds, 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.seconds = seconds;
-    this.direction = direction;
   }
 
   @Override
   protected void initialize() {
-    timer = new Timer();
-    timer.reset();
-    timer.start();
+    this.setTimeout(seconds);
+    Robot.driveTrain.setMotorSpeeds(speed, speed);
   }
 
   @Override
   protected void execute() {
-    if (direction == Direction.FORWARD) {
-      Robot.driveTrain.setMotorSpeeds(-speed, -speed);
-    } else if (direction == Direction.BACKWARD) {
-      Robot.driveTrain.setMotorSpeeds(speed, speed);
-    }
+    // nothing to do since motor speeds already set
   }
 
   @Override
   protected boolean isFinished() {
-    return timer.get() > seconds;
+    return this.isTimedOut();
   }
 
   @Override
   protected void end() {
-    Robot.driveTrain.setMotorSpeeds(0, 0);
+    Robot.driveTrain.stop();
   }
 
   @Override