Add command MoveIntakeArmToAngle

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

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

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

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

public class MoveIntakeArmToLevel extends Command {

	private double currentAngle;
	private double targetAngle;
	private double targetSpeed;
	private double SENSITIVITY_THRESHOLD = 0.1;

	public MoveIntakeArmToLevel(double level, double speed) {
		requires(Robot.intakeArm);
		targetAngle = Robot.intakeArm.getAngleForLevel(targetAngle);
		targetSpeed = speed;

	}

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

	@Override
	protected void execute() {

	}

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

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

	@Override
	protected void interrupted() {
	}
}