// Path: src/org/usfirst/frc2832/Robot_2016/RobotMap.java
// public class RobotMap {
// 
//     public static CANTalon frontLeftMotor;
//     public static CANTalon frontRightMotor;
//     public static CANTalon backLeftMotor;
//     public static CANTalon backRightMotor;
//     
//     public static CANTalon winchMotor;
//     
//     public static CANTalon ballIngestLeft;
//     public static CANTalon ballIngestRight;
//     
//     public static CANTalon tail;
//     
//     public static Relay lightRing;
//     
//     public static AnalogInput laser;
//     public static AnalogInput proxSensor;
//     
//     public static Servo kicker; 
//     
//     public static RobotDrive driveTrain;
// 
//     public static IMUAdvanced imu;
// 
//     private static AnalogGyro gyro;
//     public static DeltaPID gyroPID;
//     
//     public static final double ENCODER_PULSE_PER_METER = 2800;
//     
//     
// 
//     public static void init() {
// 
//     	// Doug MacKenzie: Switch from CANTalonLoggable back to CANTalon to remove the logging thread
//     	// Trying to fix very high CPU utilization on the roboRIO that is causing lagging.
// //        frontLeftMotor = new CANTalonLoggable(1);
//     	frontLeftMotor = new CANTalon(1);
//         LiveWindow.addActuator("Drivetrain", "frontLeft", frontLeftMotor);
//         
// //        frontRightMotor = new CANTalonLoggable(3);
//         frontRightMotor = new CANTalon(3);
//         LiveWindow.addActuator("Drivetrain", "frontRight", frontRightMotor);
//         
//         //the following code sets the back motors as slaves/followers to the front
// //        backLeftMotor = new CANTalonLoggable(2);
//         backLeftMotor = new CANTalon(2);
//         backLeftMotor.changeControlMode(CANTalon.TalonControlMode.Follower);
//         backLeftMotor.set(1);
// //        backRightMotor = new CANTalonLoggable(4);
//         backRightMotor = new CANTalon(4);
//         backRightMotor.changeControlMode(CANTalon.TalonControlMode.Follower);
//         backRightMotor.set(3);
//         
//         driveTrain = new RobotDrive(frontLeftMotor, frontRightMotor);
//         
//         driveTrain.setSafetyEnabled(true);
//         driveTrain.setExpiration(0.1);
//         driveTrain.setSensitivity(0.5);
//         driveTrain.setMaxOutput(1.0);
// 
//         frontLeftMotor.enableBrakeMode(true);
//         frontRightMotor.enableBrakeMode(true);
//         backLeftMotor.enableBrakeMode(true);
//         backRightMotor.enableBrakeMode(true);
//         
//         // Doug MacKenzie : Attempt to reproduce the code added by Brendan to reduce current usage by the drive train. 
//         // Removed brownout problems
//         frontLeftMotor.setVoltageRampRate( 41 );
//         frontRightMotor.setVoltageRampRate( 41 );
//         backLeftMotor.setVoltageRampRate( 41 );
//         backRightMotor.setVoltageRampRate( 41 );
//        
//         
//         frontLeftMotor.setInverted(true);
//         frontRightMotor.setInverted(true);
//         
//         winchMotor = new CANTalonCurrentSafety(7);
//         winchMotor.enableBrakeMode(false);
//         winchMotor.configPeakOutputVoltage(12, -12);
//         winchMotor.enableLimitSwitch(true, true);
//         
//         winchMotor.setInverted(false);
//         winchMotor.reverseOutput(false);
//         
//         ballIngestLeft = new CANTalon(5);
//         LiveWindow.addActuator("Ball Handler", "ingestLeft",ballIngestLeft);
//         ballIngestRight = new CANTalon(6);
//         LiveWindow.addActuator("Ball Handler", "ingestRight", ballIngestRight);
//         
//         ballIngestLeft.setInverted(true);
//         ballIngestRight.setInverted(true);
//         
//         //tail = new CANTalon(21); // TODO: Might have to change.
//         //tail.changeControlMode(CANTalon.TalonControlMode.Speed);
//         //tail.enableLimitSwitch(true, true);
//         //tail.setInverted(false);//Use this to switch up and down
// 
// 
//         laser = new AnalogInput(0);
//         proxSensor = new AnalogInput(1);
//         kicker = new Servo(1);
//         
//         kicker.setPosition(Kicker.NEUTRAL_ANGLE);
//         
//         byte update_rate_hz = 50;
//         imu = new IMUAdvanced(new SerialPort(57600,SerialPort.Port.kMXP), update_rate_hz);
//     
//         try
//         {
//         	gyro = new AnalogGyro(2);
//         } 
//         catch (Exception e) 
//         {
//         //e.printStackTrace();
//         }
//         
//         lightRing = new Relay(0);
//         lightRing.set(Relay.Value.kOff);
//     }
// }


import org.usfirst.frc2832.Robot_2016.RobotMap;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
package org.usfirst.frc2832.Robot_2016.commands;



/**
 *
 */
public class SmartRotate extends Command {

	private static double MAX_OUTPUT=0.8;
	private static double MIN_OUTPUT=0.5;
// Path: src/org/usfirst/frc2832/Robot_2016/RobotMap.java
// public class RobotMap {
// 
//     public static CANTalon frontLeftMotor;
//     public static CANTalon frontRightMotor;
//     public static CANTalon backLeftMotor;
//     public static CANTalon backRightMotor;
//     
//     public static CANTalon winchMotor;
//     
//     public static CANTalon ballIngestLeft;
//     public static CANTalon ballIngestRight;
//     
//     public static CANTalon tail;
//     
//     public static Relay lightRing;
//     
//     public static AnalogInput laser;
//     public static AnalogInput proxSensor;
//     
//     public static Servo kicker; 
//     
//     public static RobotDrive driveTrain;
// 
//     public static IMUAdvanced imu;
// 
//     private static AnalogGyro gyro;
//     public static DeltaPID gyroPID;
//     
//     public static final double ENCODER_PULSE_PER_METER = 2800;
//     
//     
// 
//     public static void init() {
// 
//     	// Doug MacKenzie: Switch from CANTalonLoggable back to CANTalon to remove the logging thread
//     	// Trying to fix very high CPU utilization on the roboRIO that is causing lagging.
// //        frontLeftMotor = new CANTalonLoggable(1);
//     	frontLeftMotor = new CANTalon(1);
//         LiveWindow.addActuator("Drivetrain", "frontLeft", frontLeftMotor);
//         
// //        frontRightMotor = new CANTalonLoggable(3);
//         frontRightMotor = new CANTalon(3);
//         LiveWindow.addActuator("Drivetrain", "frontRight", frontRightMotor);
//         
//         //the following code sets the back motors as slaves/followers to the front
// //        backLeftMotor = new CANTalonLoggable(2);
//         backLeftMotor = new CANTalon(2);
//         backLeftMotor.changeControlMode(CANTalon.TalonControlMode.Follower);
//         backLeftMotor.set(1);
// //        backRightMotor = new CANTalonLoggable(4);
//         backRightMotor = new CANTalon(4);
//         backRightMotor.changeControlMode(CANTalon.TalonControlMode.Follower);
//         backRightMotor.set(3);
//         
//         driveTrain = new RobotDrive(frontLeftMotor, frontRightMotor);
//         
//         driveTrain.setSafetyEnabled(true);
//         driveTrain.setExpiration(0.1);
//         driveTrain.setSensitivity(0.5);
//         driveTrain.setMaxOutput(1.0);
// 
//         frontLeftMotor.enableBrakeMode(true);
//         frontRightMotor.enableBrakeMode(true);
//         backLeftMotor.enableBrakeMode(true);
//         backRightMotor.enableBrakeMode(true);
//         
//         // Doug MacKenzie : Attempt to reproduce the code added by Brendan to reduce current usage by the drive train. 
//         // Removed brownout problems
//         frontLeftMotor.setVoltageRampRate( 41 );
//         frontRightMotor.setVoltageRampRate( 41 );
//         backLeftMotor.setVoltageRampRate( 41 );
//         backRightMotor.setVoltageRampRate( 41 );
//        
//         
//         frontLeftMotor.setInverted(true);
//         frontRightMotor.setInverted(true);
//         
//         winchMotor = new CANTalonCurrentSafety(7);
//         winchMotor.enableBrakeMode(false);
//         winchMotor.configPeakOutputVoltage(12, -12);
//         winchMotor.enableLimitSwitch(true, true);
//         
//         winchMotor.setInverted(false);
//         winchMotor.reverseOutput(false);
//         
//         ballIngestLeft = new CANTalon(5);
//         LiveWindow.addActuator("Ball Handler", "ingestLeft",ballIngestLeft);
//         ballIngestRight = new CANTalon(6);
//         LiveWindow.addActuator("Ball Handler", "ingestRight", ballIngestRight);
//         
//         ballIngestLeft.setInverted(true);
//         ballIngestRight.setInverted(true);
//         
//         //tail = new CANTalon(21); // TODO: Might have to change.
//         //tail.changeControlMode(CANTalon.TalonControlMode.Speed);
//         //tail.enableLimitSwitch(true, true);
//         //tail.setInverted(false);//Use this to switch up and down
// 
// 
//         laser = new AnalogInput(0);
//         proxSensor = new AnalogInput(1);
//         kicker = new Servo(1);
//         
//         kicker.setPosition(Kicker.NEUTRAL_ANGLE);
//         
//         byte update_rate_hz = 50;
//         imu = new IMUAdvanced(new SerialPort(57600,SerialPort.Port.kMXP), update_rate_hz);
//     
//         try
//         {
//         	gyro = new AnalogGyro(2);
//         } 
//         catch (Exception e) 
//         {
//         //e.printStackTrace();
//         }
//         
//         lightRing = new Relay(0);
//         lightRing.set(Relay.Value.kOff);
//     }
// }


// Path: src/org/usfirst/frc2832/Robot_2016/commands/SmartRotate.java
import org.usfirst.frc2832.Robot_2016.RobotMap;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

package org.usfirst.frc2832.Robot_2016.commands;



/**
 *
 */
public class SmartRotate extends Command {

	private static double MAX_OUTPUT=0.8;
	private static double MIN_OUTPUT=0.5;
	private static double MAX_INPUT=30;