Rename MoveIntakeArm to MoveIntakeArmToAngle

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

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

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

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

public class MoveIntakeArmToAngle extends Command {
	private double targetAngle;
	private double currentAngle;
	private double targetSpeed;
	private double SENSITIVITY_THRESHOLD = 0.1;
	private double NO_MOVEMENT_SPEED = 0;

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

	@Override
	protected void initialize() {
		currentAngle = Robot.intakeArm.getArmAngle();
		double difference = targetAngle - currentAngle;
		if (difference > 0) {
			Robot.intakeArm.setArmVoltage(targetSpeed);
		} else {
			Robot.intakeArm.setArmVoltage(-targetSpeed);
		}
	}

	@Override
	protected void execute() {

	}

	@Override
	protected boolean isFinished() {
		currentAngle = Robot.intakeArm.getArmAngle();
		double distance = Math.abs(currentAngle - targetAngle);
		if (distance <= SENSITIVITY_THRESHOLD) {
			return true;
		} else {
			return false;
		}

	}

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

	@Override
	protected void interrupted() {
	}
}