// RobotBuilder Version: 0.0.2
//
// 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 th future.
package org.usfirst.frc330.Beachbot2013Java.commands;
import com.sun.squawk.util.MathUtils;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.buttons.NetworkButton;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.usfirst.frc330.Beachbot2013Java.Robot;
/**
 *
 */
public class  ControlLEDs extends Command {
    
    public ControlLEDs() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
	
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
        requires(Robot.vision);
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
        setRunWhenDisabled(true);
    }
    // Called just before this Command runs the first time
    protected void initialize() {
        Robot.vision.turnOffHighShooterLED();
        Robot.vision.turnOffLowShooterLED();
    }
    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
        if (Robot.vision.getLEDEnable() || DriverStation.getInstance().isFMSAttached())
        {
            double gyroAngle = Robot.chassis.getAngle();
            while (gyroAngle > 180)
            {
                gyroAngle -= 360;  
            }
            while (gyroAngle < -180)
            {
                gyroAngle += 360;
            }
            SmartDashboard.putNumber("VisionGyroAngle: ",gyroAngle);
            if (Robot.vision.getHighLEDState())
            {
                if (gyroAngle < 135 || gyroAngle > -135)
                {
                    Robot.vision.turnOffHighShooterLED();
                }   
            }
            else
            {
                if (gyroAngle > 145 || gyroAngle < - 145)
                {
                    Robot.vision.turnOnHighShooterLED();
                }
            }
            
            if (Robot.vision.getLowLEDState())
            {
                if (gyroAngle > 45 || gyroAngle < -45)
                {
                    Robot.vision.turnOffLowShooterLED();
                }
            }
            else
            {
                if (gyroAngle < 35 && gyroAngle > -35)
                {
                    Robot.vision.turnOnLowShooterLED();
                }
            }
        }
    }
    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return false;
    }
    // Called once after isFinished returns true
    protected void end() {
    }
    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
}

@@ -1494,24 +1494,31 @@
  execute() %7B
+       
 %0A        if 
@@ -1595,32 +1595,46 @@
 hed())%0A        %7B
+              
 %0A            dou
@@ -1939,24 +1939,145 @@
 gyroAngle);%0A
+            //TODO: make a preference for angles. Currently +/- 45 deg for turning off and +/- 35 degrees for turning on%0A
             

add TODO