package org.usfirst.frc2832.Robot_2016;

import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.Preferences;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class Aimer extends Subsystem {
	public static int currentLevel = 0;
	public static int[] levelPositions = {0,3000,4500,5500};
	//public static final int MOVE_SPEED_UP = 200;
	public static String mode = "position";
	//public static final int MOVE_SPEED_DOWN = -175;
	public static final double UP_PID_P = Preferences.getInstance().getDouble("Aimer Up kP", 5.0),
							   UP_PID_I = Preferences.getInstance().getDouble("Aimer Up kI", 0.001),
							   UP_PID_D = Preferences.getInstance().getDouble("Aimer Up kD", 0.0),
							   DOWN_PID_P = Preferences.getInstance().getDouble("Aimer Down kP", 2.0),
							   DOWN_PID_I = Preferences.getInstance().getDouble("Aimer Down kI", 0.02),
							   DOWN_PID_D = Preferences.getInstance().getDouble("Aimer Down kD", 0.0);
	public static final int	MOVE_SPEED_UP = Preferences.getInstance().getInt("Aimer Up Speed", 200),
							MOVE_SPEED_DOWN = Preferences.getInstance().getInt("Aimer Down Speed", -175);
			
	
	@Override
	protected void initDefaultCommand() {
		// TODO Auto-generated method stub
		
	}

	public static void upSpeedMode()
	{
		RobotMap.winchMotor.changeControlMode(CANTalon.TalonControlMode.Speed);
		RobotMap.winchMotor.setPID(UP_PID_P, UP_PID_I, UP_PID_D);
		RobotMap.winchMotor.setAllowableClosedLoopErr(0);
	}
	public static void downSpeedMode()
	{
		RobotMap.winchMotor.changeControlMode(CANTalon.TalonControlMode.Speed);
		RobotMap.winchMotor.setPID(DOWN_PID_P, DOWN_PID_I, DOWN_PID_D);
		RobotMap.winchMotor.setAllowableClosedLoopErr(0);
	}
	public static void toPositionMode()
	{
		RobotMap.winchMotor.changeControlMode(CANTalon.TalonControlMode.Position);
		RobotMap.winchMotor.setPID(20, 0, 0);
		RobotMap.winchMotor.setAllowableClosedLoopErr(0);
	}
	/**
	 * Position mode ONLY
	 * "freezes" the winch in its current position
	 */
	public static void stop()
	{
		toPositionMode();
		RobotMap.winchMotor.set(RobotMap.winchMotor.getEncPosition());
	}
}

@@ -517,21 +517,190 @@
 tic 
-final 
 double 
+UP_PID_P, UP_PID_I, UP_PID_D,%0A%09%09%09%09%09%09%09   DOWN_PID_P, DOWN_PID_I, DOWN_PID_D;%0A%09public static int MOVE_SPEED_UP, MOVE_SPEED_DOWN;%0A%09%09%09%0A%09%0A%09public static void loadPreferences() %7B%0A%09%09
 UP_P
@@ -761,28 +761,20 @@
 P%22, 5.0)
-,
+;
 %0A%09%09
-%09%09%09%09%09   
 UP_PID_I
@@ -833,28 +833,20 @@
 , 0.001)
-,
+;
 %0A%09%09
-%09%09%09%09%09   
 UP_PID_D
@@ -903,28 +903,20 @@
 D%22, 0.0)
-,
+;
 %0A%09%09
-%09%09%09%09%09   
 DOWN_PID
@@ -977,28 +977,20 @@
 P%22, 2.0)
-,
+;
 %0A%09%09
-%09%09%09%09%09   
 DOWN_PID
@@ -1052,28 +1052,20 @@
 %22, 0.02)
-,
+;
 %0A%09%09
-%09%09%09%09%09   
 DOWN_PID
@@ -1129,39 +1129,19 @@
  0.0);%0A%09
-public static final int
+%09%0A%09
 %09MOVE_SP
@@ -1204,23 +1204,18 @@
 d%22, 200)
-,%0A%09%09%09%09%09
+;%0A
 %09%09MOVE_S
@@ -1287,18 +1287,17 @@
 -175);%0A%09
-%09%09
+%7D
 %0A%09%0A%09@Ove

Fix issue with loading preferences before robot is ready