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 4028f93e49f4e5ff56ca66345ca96eebca76d48a..61edfcfbdfd9c0d9fb2401b625272034c9d56873 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commands/MoveDefenseArmVertical.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/MoveDefenseArmVertical.java
@@ -1,63 +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 MoveDefenseArmVertical(double horizontalLimit, double height) {
-    this.horizontalLimit = horizontalLimit;
-    this.height = height;
-  }
-
-  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;
-  }
-
-  // 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() {
   }
 }