Add camera, change to arcade drive, front chooser only, minor driving fixes

[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / commands / driving / TimeDrive.java

diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/TimeDrive.java b/src/org/usfirst/frc/team3501/robot/commands/driving/TimeDrive.java
index e9a8f9f34bb598906cdcb4e2b4d8bb90e4b5e612..4778da85d7cbff1aaa6389d361f85ed4a440448b 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commands/driving/TimeDrive.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/TimeDrive.java
@@ -20,30 +20,24 @@ public class TimeDrive extends Command {
 
   public TimeDrive(double time, double speed) {
     requires(Robot.driveTrain);
+    this.setInterruptible(false);
 
-    timer = new Timer();
-    this.currentTime = 0;
-    this.targetTime = time;
+    this.setTimeout(time);
     this.speed = speed;
   }
 
   @Override
   protected void initialize() {
-    timer.start();
   }
 
   @Override
   protected void execute() {
-    currentTime = timer.get();
-
-    double output = speed * ((targetTime - currentTime) / (targetTime));
-
-    Robot.driveTrain.setMotorSpeeds(output, output);
+    Robot.driveTrain.drive(speed, 0);
   }
 
   @Override
   protected boolean isFinished() {
-    return currentTime >= targetTime;
+    return this.isTimedOut();
   }
 
   @Override