From de4f822fbfd21dd7421f56b749240e7b11d9bc5c Mon Sep 17 00:00:00 2001
From: David <david.dobervich@gmail.com>
Date: Fri, 20 Nov 2015 07:43:30 -0800
Subject: [PATCH] add clean DriveForTime command

---
 .../RiceCatRobot/commands/DriveForTime.java   | 59 +++++++++++++++++++
 .../RiceCatRobot/subsystems/DriveTrain.java   |  4 ++
 2 files changed, 63 insertions(+)
 create mode 100644 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
new file mode 100644
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 49ffefa..4117c6a 100644
--- 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);
+  }
 }
-- 
2.30.2