create new command with math that calculates what two angles arm and hand should...

[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.Command;

/**
 *
 */
public class MoveDefenseArmVertical extends Command {

  double horizontalLimit, height;

  public MoveDefenseArmVertical(double horizontalLimit, double height) {
    // requires();
    this.horizontalLimit = horizontalLimit;
    this.height = height;
  }

  // Called just before this Command runs the first time
  @Override
  protected void initialize() {
  }

  // Called repeatedly when this Command is scheduled to run
  @Override
  protected void execute() {
    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;

    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);
  }

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