add precondition and Timeout to Expel and Intake commands
[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / commands / ClampBar.java
index 44648a07f71c3fdf0df40eaf1e31e5a04ef7cd7c..90f9c7e6ed6d399e0df01533a7ad7eb40494f189 100755 (executable)
@@ -1,36 +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() {
-        // Use requires() here to declare subsystem dependencies
-        // eg. requires(chassis);
-    }
-
-    // Called just before this Command runs the first time
-    protected void initialize() {
-    }
-
-    // Called repeatedly when this Command is scheduled to run
-    protected void execute() {
-    }
-
-    // Make this return true when this Command no longer needs to run execute()
-    protected boolean isFinished() {
-        return false;
-    }
-
-    // Called once after isFinished returns true
-    protected void end() {
-    }
-
-    // Called when another command which requires one or more of the same
-    // subsystems is scheduled to run
-    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();
+  }
 }