delete anything to do with pots and motors in IntakeArm and Constants, add the left...

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

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

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

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

public class MoveIntakeArmToAngle extends Command {
  private double currentAngle;
  private double targetAngle;
  private double targetSpeed;
  private double calculatedSpeed;
  private double SENSITIVITY_THRESHOLD = 0.1;
  private double MIN_SPEED = .3;

  public MoveIntakeArmToAngle(double angle, double speed) {
    requires(Robot.intakeArm);
    targetAngle = angle;
    targetSpeed = speed;

  }

  @Override
  protected void initialize() {

    // set the arm speed to the calculated angle
    double speed = getCalculatedSpeed();

    if (speed < MIN_SPEED && speed > 0)
      speed = Math.signum(getCalculatedSpeed()) * MIN_SPEED;
    Robot.intakeArm.setArmSpeed(speed);
  }

  @Override
  protected void execute() {
    // set the arm speed to the calculated angle
    Robot.intakeArm.setArmSpeed(calculatedSpeed);
  }

  private double getError() {
    // targetAngle - currentAngle = error
    return Robot.intakeArm.getArmAngle() - targetAngle;
  }

  private double getCalculatedSpeed() {
    // if the arm has to move up -- the speed will be +
    // if the arm has to move down -- the speed will be -

    return (getError() / targetAngle) * targetSpeed;
  }

  @Override
  protected boolean isFinished() {
    // if the arm's angle is close enough to the target value return true
    // else return false
    return (Math.abs(getError()) <= SENSITIVITY_THRESHOLD);
  }

  @Override
  protected void end() {
    Robot.intakeArm.stop();
  }

  @Override
  protected void interrupted() {
    end();
  }

}