From: Cindy Zhang <cindyzyx9@gmail.com>
Date: Sat, 13 Feb 2016 20:53:42 +0000 (-0800)
Subject: add parallel commands setArmToAngle and setHandToAngle
X-Git-Url: http://challenge-bot.com/repos/?a=commitdiff_plain;h=d3fd4be06f292d3322da33d648630df0ae2b684c;p=3501%2Fstronghold-2016

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 4028f93e..72ad433c 100644
--- 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() {
-  }
 }