add precondition and Timeout to Expel and Intake commands

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

diff --git a/src/org/usfirst/frc/team3501/robot/commands/RetractDefenseArm.java b/src/org/usfirst/frc/team3501/robot/commands/RetractDefenseArm.java
index a073fa83e7a2c25dd5c9c5273ff7ec7d42076024..95f5f92e44ea2b7c4cfecb091e2c4907d44aa7d0 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/commands/RetractDefenseArm.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/RetractDefenseArm.java
@@ -4,12 +4,21 @@ import org.usfirst.frc.team3501.robot.Robot;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
 
+/***
+ * This command group simultaneously move arm and hand to retracted position.
+ *
+ * @author shaina
+ *
+ */
 public class RetractDefenseArm extends CommandGroup {
+  static final double ARM_FULLY_RETRACTED_ANGLE = 0; // change value once tested
+  static final double HAND_FULLY_RETRACTED_ANGLE = 0; // change value once
+                                                      // tested
 
-  public RetractDefenseArm(double speed, double retractPosition) {
+  public RetractDefenseArm(double speed) {
     requires(Robot.defenseArm);
 
-    addParallel(new SetArmToLevel(speed, retractPosition));
-    addParallel(new SetHandToLevel(speed, retractPosition));
+    addParallel(new SetArmToAngle(speed, ARM_FULLY_RETRACTED_ANGLE));
+    addParallel(new SetHandToAngle(speed, HAND_FULLY_RETRACTED_ANGLE));
   }
 }