add parallel commands setArmToAngle and setHandToAngle

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

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

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

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

/**
 *
 */
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() {
    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;
  }
}