add comments that explain what this commandgroups do

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

package org.usfirst.frc.team3501.robot.commands;

import org.usfirst.frc.team3501.robot.Constants;

import edu.wpi.first.wpilibj.command.CommandGroup;

/**
 * Given input target x and y positions of the tip of the defense arm apparatus:
 * first, calculates what the angle of the arm and hand corresponding to the
 * given x-y coordinate is
 * next, with speed of arm set to a constant, determine what the speed of hand
 * should be if proportional to speed of arm (with the ratio equal to the ratio
 * between arm angle to be adjusted and hand angle to be adjusted
 */
public class MoveDefenseArm extends CommandGroup {

  double horizontalLimit, height;

  public static final double ARM_SPEED = 0.6;

  public MoveDefenseArm(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;
  }
}