[3501/2015-FRC-Spark] / src / org / usfirst / frc3501 / RiceCatRobot / commands / MoveArmFor.java

package org.usfirst.frc3501.RiceCatRobot.commands;

import org.usfirst.frc3501.RiceCatRobot.Robot;
import org.usfirst.frc3501.RiceCatRobot.RobotMap;
import org.usfirst.frc3501.RiceCatRobot.RobotMap.Direction;

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

/**
 * This command takes a time in seconds which is how long it should run to move
 * arm up or down
 *
 */
public class MoveArmFor extends Command {
    private double seconds;
    private Timer timer;
    private Direction direction;

    /*
     * @param Direction must be up or down
     */
    public MoveArmFor(double seconds, Direction direction) {
        this.seconds = seconds;
        this.direction = direction;
    }

    @Override
    protected void initialize() {
        timer = new Timer();
        timer.start();
    }

    @Override
    protected void execute() {
        if (direction == Direction.UP) {
            Robot.arm.setArmSpeeds(-RobotMap.ARM_LOW_SPEED);
        } else if (direction == Direction.DOWN) {
            Robot.arm.setArmSpeeds(RobotMap.ARM_LOW_SPEED);
        }
    }

    @Override
    protected boolean isFinished() {
        if (timer.get() > seconds) {
            Robot.arm.setArmSpeeds(0);
        }
        return timer.get() > seconds;
    }

    @Override
    protected void end() {
        Robot.arm.setArmSpeeds(0);
    }

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