add comments that explain what these commandgroups do

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

diff --git a/src/org/usfirst/frc/team3501/robot/commands/MoveDefenseArmVertical.java b/src/org/usfirst/frc/team3501/robot/commands/MoveDefenseArmVertical.java
index 72ad433ca812d622278191ce37aa243020b8de2a..61edfcfbdfd9c0d9fb2401b625272034c9d56873 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commands/MoveDefenseArmVertical.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/MoveDefenseArmVertical.java
@@ -1,53 +1,22 @@
 package org.usfirst.frc.team3501.robot.commands;
 
-import org.usfirst.frc.team3501.robot.Constants;
+import org.usfirst.frc.team3501.robot.Robot;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
 
 /**
- *
+ * Given input verticalDisplacement, which represents how far (and in what
+ * direction, depending on the sign) the user wants the arm to be moved linearly
+ * & vertically, this commandGroup calls the MoveDefenseArm command, which
+ * will then move the tip of the arm to the target position based on input
+ * verticalDisplacement
  */
 public class MoveDefenseArmVertical extends CommandGroup {
 
-  double horizontalLimit, height;
+  public MoveDefenseArmVertical(double verticalDisplacement) {
+    double x = Robot.defenseArm.getArmHorizontalDisplacement();
+    double y = Robot.defenseArm.getArmVerticalDisplacement();
+    addSequential(new MoveDefenseArm(x, y + verticalDisplacement));
 
-  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() {
-    double armAngle;
-    armAngle = square(horizontalLimit) + square(height)
-        + square(Constants.DefenseArm.ARM_LENGTH)
-        - square(Constants.DefenseArm.HAND_LENGTH);
-    armAngle /= 2 * Math.sqrt(square(horizontalLimit) + square(height))
-        * Constants.DefenseArm.ARM_LENGTH;
-    armAngle = Math.acos(armAngle);
-    armAngle = Math.atan(height / horizontalLimit) - armAngle;
-    return armAngle;
-  }
-
-  protected double calculateTargetHandAngle() {
-    double handAngle;
-    handAngle = square(horizontalLimit) + square(height)
-        + square(Constants.DefenseArm.HAND_LENGTH)
-        - square(Constants.DefenseArm.ARM_LENGTH);
-    handAngle /= 2 * Math.sqrt(square(horizontalLimit) + square(height))
-        * Constants.DefenseArm.HAND_LENGTH;
-    handAngle = Math.acos(handAngle);
-    handAngle = handAngle + 90 - Math.atan(horizontalLimit / height);
-    return handAngle;
-  }
-
-  public double square(double num) {
-    return num * num;
   }
 }