wh1ter0se / PowerUp-2018

import edu.wpi.first.wpilibj.DigitalInput;

import edu.wpi.first.wpilibj.DriverStation;

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

import org.usfirst.frc.team3695.robot.enumeration.Position;

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

/**

 * toggles the state of the clamp

 */

public class CyborgCommandGoToMid extends Command {

    public CyborgCommandGoToMid() {

        requires(Robot.SUB_MAST);

    }

    protected void initialize() {

    	DriverStation.reportWarning("MOVING TO MID POSITION", false);

    	isFinished = true;

    }

    protected void execute() {

    	if (!isFinished) {

    		isFinished = Robot.SUB_MAST.goToMiddle();

    	}

    }

    protected void end() {}

    protected void interrupted() {

    	end();

    }

}