[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 SENSITIVITY_THRESHOLD = 0.1;
        private boolean isDecreasing = false;

        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.setArmSpeed(targetSpeed);
                        isDecreasing = true;
                } else {
                        Robot.intakeArm.setArmSpeed(-targetSpeed);
                        isDecreasing = false;
                }
        }

        @Override
        protected void execute() {

        }

        @Override
        protected boolean isFinished() {
                currentAngle = Robot.intakeArm.getArmAngle();

                if (isDecreasing == true) {
                        return (currentAngle <= targetAngle + SENSITIVITY_THRESHOLD);
                } else {
                        return (currentAngle >= targetAngle + SENSITIVITY_THRESHOLD);
                }
        }

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

        @Override
        protected void interrupted() {
        }

}