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