// RobotBuilder Version: 2.0
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// Java from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in the future.


package org.usfirst.frc1891.FirstSteamworks.commands;
import edu.wpi.first.wpilibj.command.Command;
import org.usfirst.frc1891.FirstSteamworks.Robot;

/**
 *
 */
public class Turn extends Command {
	
	private double m_angle;
	private boolean reinitialized = false;

	
    /**
     * turns the robot
     * @param angle	angle to turn, positive is right, negative is left
     */
    public Turn(double angle) {
        requires(Robot.driveSystem);
        m_angle = angle;
        // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    	
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	//order 66
    	if (!reinitialized) {
    		Robot.driveSystem.turn(m_angle);
    		reinitialized = true;
    	}
    	
    	Robot.driveSystem.turn(0);
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
		return Robot.driveSystem.isTurnFinished();
    }

    // Called once after isFinished returns true
    protected void end() {
    	reinitialized = false;
    	System.out.println("turnFinished");
    	Robot.driveSystem.stopMotors();
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	end();
    }
}
