add parallel commands setArmToAngle and setHandToAngle

diff --git a/src/org/usfirst/frc/team3501/robot/commands/MoveDefenseArmVertical.java b/src/org/usfirst/frc/team3501/robot/commands/MoveDefenseArmVertical.java
index 4028f93e49f4e5ff56ca66345ca96eebca76d48a..72ad433ca812d622278191ce37aa243020b8de2a 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commands/MoveDefenseArmVertical.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/MoveDefenseArmVertical.java
@@ -11,9 +11,16 @@ public class MoveDefenseArmVertical extends CommandGroup {
 
   double horizontalLimit, height;
 
+  public static final double ARM_SPEED = 0.6;
+
   public MoveDefenseArmVertical(double horizontalLimit, double height) {
     this.horizontalLimit = horizontalLimit;
     this.height = height;
+    double targetArmAngle = calculateTargetArmAngle();
+    double targetHandAngle = calculateTargetHandAngle();
+    double handSpeed = ARM_SPEED * targetHandAngle / targetArmAngle;
+    addParallel(new SetArmToAngle(ARM_SPEED, calculateTargetArmAngle()));
+    addParallel(new SetHandToAngle(handSpeed, calculateTargetHandAngle()));
   }
 
   protected double calculateTargetArmAngle() {
@@ -40,24 +47,7 @@ public class MoveDefenseArmVertical extends CommandGroup {
     return handAngle;
   }
 
-  // Make this return true when this Command no longer needs to run execute()
-  @Override
-  protected boolean isFinished() {
-    return false;
-  }
-
-  // Called once after isFinished returns true
-  @Override
-  protected void end() {
-  }
-
   public double square(double num) {
     return num * num;
   }
-
-  // Called when another command which requires one or more of the same
-  // subsystems is scheduled to run
-  @Override
-  protected void interrupted() {
-  }
 }