make MoveToLevel

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

package org.usfirst.frc3501.RiceCatRobot.commands;

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

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

public class MoveToLevel extends Command{

        int targetLevel, currentLevel;
        
        public MoveToLevel(int targetLevel) {
                this.targetLevel = targetLevel;
        }
        
        @Override
        protected void initialize() {
                
        }
        
        @Override
        protected void execute() {
                Robot.arm.setArmSpeeds(RobotMap.ARM_LOW_SPEED);
        }

        @Override
        protected boolean isFinished() {
                if(currentLevel == targetLevel) return true;
                return false;
        }       

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

}