add precondition and Timeout to Expel and Intake commands

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

diff --git a/src/org/usfirst/frc/team3501/robot/commands/ClampBar.java b/src/org/usfirst/frc/team3501/robot/commands/ClampBar.java
index f571619cdeeedaf79b05fdde26d10d301db25604..90f9c7e6ed6d399e0df01533a7ad7eb40494f189 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/commands/ClampBar.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/ClampBar.java
@@ -1,30 +1,39 @@
 package org.usfirst.frc.team3501.robot.commands;
 
+import org.usfirst.frc.team3501.robot.Robot;
+
 import edu.wpi.first.wpilibj.command.Command;
 
 public class ClampBar extends Command {
-
-       public ClampBar() {
-       }
-
-       @Override
-       protected void initialize() {
-       }
-
-       @Override
-       protected void execute() {
-       }
-
-       @Override
-       protected boolean isFinished() {
-               return false;
-       }
-
-       @Override
-       protected void end() {
-       }
-
-       @Override
-       protected void interrupted() {
-       }
+  private double secondsToClamp = 2.0; // seconds for the winch to run in order
+                                       // for it to clamp(requires testing)
+  private double winchSpeed = 0.5; // requires testing
+
+  public ClampBar() {
+  }
+
+  @Override
+  protected void initialize() {
+    setTimeout(secondsToClamp);
+    Robot.scaler.runWinch(winchSpeed);
+  }
+
+  @Override
+  protected void execute() {
+  }
+
+  @Override
+  protected boolean isFinished() {
+    return isTimedOut();
+  }
+
+  @Override
+  protected void end() {
+    Robot.scaler.runWinch(0);
+  }
+
+  @Override
+  protected void interrupted() {
+    end();
+  }
 }