Pre-Bag Fixes

- Finished PID initial tuning (still oscillates occasionally)
- Has option to disable PID (Needs teleop override)
- Tuned TwoToteOneContainer
- Moved the claw up button

src/org/team708/robot/OI.java

@@ -59,7 +59,7 @@ public class OI {
 	public static final int toggleWristPositionButton = Gamepad.button_L_Shoulder;
 
 	// Claw Elevator Buttons
-	public static final int clawHeightIncrementButton = Gamepad.button_Y;
+	public static final int clawHeightIncrementButton = Gamepad.button_B;
 	public static final int clawHeightDecrementButton = Gamepad.button_A;
 
 	/*

src/org/team708/robot/Robot.java

@@ -80,7 +80,7 @@ public void robotInit() {
 
 		autonomousMode = new SendableChooser();		// Initializes the Autonomous selection box
 		queueAutonomousModes();						// Adds autonomous modes to the selection box
-		setPIDPreferences();						// Adds PID gain constants to the Robot Preferences
+//		setPIDPreferences();						// Adds PID gain constants to the Robot Preferences
     }
 
     /**
@@ -202,9 +202,9 @@ private void sendDashboardSubsystems() {
     private void setPIDPreferences() {
     	// Attempts to get robot preferences to set for PID, but throws an exception if the SmartDashboard is not found
     	try {
-    		drivetrain.setPIDGain('p', Preferences.getInstance().getDouble("P", drivetrain.getPIDGain('p')));
-    		drivetrain.setPIDGain('i', Preferences.getInstance().getDouble("I", drivetrain.getPIDGain('i')));
-    		drivetrain.setPIDGain('d', Preferences.getInstance().getDouble("D", drivetrain.getPIDGain('d')));
+    		drivetrain.setPIDGain('p', Preferences.getInstance().getDouble("P", 0.2));
+    		drivetrain.setPIDGain('i', Preferences.getInstance().getDouble("I", 0.0));
+    		drivetrain.setPIDGain('d', Preferences.getInstance().getDouble("D", 0.1));
     	} catch (NullPointerException e) {
     		e.printStackTrace();
     	}

src/org/team708/robot/commands/autonomous/TwoToteOneContainer.java

@@ -9,6 +9,7 @@
 import org.team708.robot.commands.indexer.IndexerUp;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
 
 /**
  * An autonomous mode where the robot starts at a container,
@@ -20,9 +21,9 @@ public class TwoToteOneContainer extends CommandGroup {
 	private static final double ENCODER_SPEED = 0.45;
 	private static final double IR_TOLERANCE = 0.0625;
 
-	private static final double TO_FIRST_TOTE_DISTANCE = 15.0;
-	private static final double TURN_DEGREES_TO_SECOND_TOTE = 169.0;
-	private static final double TO_SECOND_TOTE_DISTANCE = 45.0;
+	private static final double TO_FIRST_TOTE_DISTANCE = 21.0;
+	private static final double TURN_DEGREES_TO_SECOND_TOTE = 172.0;
+	private static final double TO_SECOND_TOTE_DISTANCE = 58.0;
 
 	private static final double TURN_DEGREES_TO_AUTO_ZONE = -85.0;
 	private static final double TO_AUTO_ZONE_DISTANCE = 115.0;
@@ -39,6 +40,7 @@ public  TwoToteOneContainer() {
 
     	// Gets the second tote
     	addSequential(new TurnToDegrees(TURN_SPEED, TURN_DEGREES_TO_SECOND_TOTE));
+    	addSequential(new WaitCommand(0.1));
     	addSequential(new DriveStraightToEncoderDistance(TO_SECOND_TOTE_DISTANCE, ENCODER_SPEED));
 //    	addSequential(new DriveToIRDistance(Constants.IR_HAS_TOTE_DISTANCE, IR_TOLERANCE));
     	addSequential(new IndexerUp(false));

src/org/team708/robot/commands/drivetrain/DriveStraightForTime.java

@@ -27,7 +27,7 @@ protected void initialize() {
 
     // Called repeatedly when this Command is scheduled to run
     protected void execute() {
-    	Robot.drivetrain.haloDrive(moveSpeed, 0.0);
+    	Robot.drivetrain.haloDrive(moveSpeed, 0.0, true);
     }
 
     // Make this return true when this Command no longer needs to run execute()

src/org/team708/robot/commands/drivetrain/DriveStraightToEncoderDistance.java

@@ -39,7 +39,7 @@ protected void initialize() {
 
     // Called repeatedly when this Command is scheduled to run
     protected void execute() {
-    	Robot.drivetrain.haloDrive(speed, rotate);
+    	Robot.drivetrain.haloDrive(speed, rotate, true);
 //    	Robot.drivetrain.haloDrive(Math708.getPercentError
 //    			(Robot.drivetrain.getEncoderDistance(), targetDistance), rotate);    	
     }

src/org/team708/robot/commands/drivetrain/DriveToIRDistance.java

@@ -28,7 +28,7 @@ protected void initialize() {
     // Called repeatedly when this Command is scheduled to run
     protected void execute() {
     	moveSpeed = Robot.drivetrain.moveByIR(targetDistance, tolerance);
-    	Robot.drivetrain.haloDrive(moveSpeed * Constants.DRIVE_MOTOR_MAX_SPEED, 0.0);
+    	Robot.drivetrain.haloDrive(moveSpeed * Constants.DRIVE_MOTOR_MAX_SPEED, 0.0, true);
     }
 
     // Make this return true when this Command no longer needs to run execute()

src/org/team708/robot/commands/drivetrain/JoystickDrive.java

@@ -23,7 +23,7 @@ protected void initialize() {
 
     // Called repeatedly when this Command is scheduled to run
     protected void execute() {
-    	Robot.drivetrain.haloDrive(OI.driverGamepad.getAxis(Gamepad.leftStick_Y), -OI.driverGamepad.getAxis(Gamepad.rightStick_X));
+    	Robot.drivetrain.haloDrive(OI.driverGamepad.getAxis(Gamepad.leftStick_Y), -OI.driverGamepad.getAxis(Gamepad.rightStick_X), true);
     }
 
     // Make this return true when this Command no longer needs to run execute()

src/org/team708/robot/commands/drivetrain/TurnToDegrees.java

@@ -28,9 +28,9 @@ protected void initialize() {
     // Called repeatedly when this Command is scheduled to run
     protected void execute() {
     	if (goalDegrees >= 0) {
-    		Robot.drivetrain.haloDrive(0.0, -rotationSpeed);
+    		Robot.drivetrain.haloDrive(0.0, -rotationSpeed, true);
     	} else {
-    		Robot.drivetrain.haloDrive(0.0, rotationSpeed);
+    		Robot.drivetrain.haloDrive(0.0, rotationSpeed, true);
     	}
     }
 

src/org/team708/robot/subsystems/Drivetrain.java

@@ -22,11 +22,19 @@
 public class Drivetrain extends PIDSubsystem {
 
 	// PID Tuning parameters
-	private static double Kp = 0.20;		// Proportional gain  
-	private static double Ki = 0.0;		// Integral gain		 
-	private static double Kd = 0.1;		// Derivative gain
+	private static double Kp = 0.0;		// Proportional gain  Was 0.1
+	private static double Ki = 0.0;		// Integral gain		  Was 0.02
+	private static double Kd = 0.0;		// Derivative gain	  Was 0.005
+
+	private static double KpForward = 0.1;
+	private static double KiForward = 0.01;
+	private static double KdForward = 0.005;
+
+	private static double KpBackward = 0.1;
+	private static double KiBackward = 0.02;
+	private static double KdBackward = 0.005;
 
-	private static double tolerance = 5;
+	private static double tolerance = 1;
 
 	// Variables specific for drivetrain PID loop
 	private double moveSpeed = 0.0;
@@ -74,9 +82,9 @@ public Drivetrain() {
 		encoder.reset();								// Resets the encoder so that it starts with a 0.0 value
 		setEncoderReading();							// Sets the encoder to read positive when moving forward
 		irSensor = new IRSensor(RobotMap.drivetrainIRSensor, IRSensor.GP2Y0A21YK0F);
-
+
+		setInputRange(-25.0, 25.0);
 		setAbsoluteTolerance(tolerance);
-		setInputRange(-360.0, 360.0);
         setSetpoint(0.0);
 //		enable();
         disable();
@@ -95,29 +103,44 @@ public void initDefaultCommand() {
      * @param move
      * @param rotate
      */
-    public void haloDrive(double move, double rotate) {
+    public void haloDrive(double move, double rotate, boolean usePID) {
     	// Checks whether drift correction is needed
 
     	//sets multiplier for max drive speed
     	move = move * Constants.DRIVE_MOTOR_MAX_SPEED;
     	rotate = rotate * Constants.ROTATE_MOTOR_MAX_SPEED;
 
-
-    	if (rotate == 0.0 && move != 0.0) {
-    		// Enables the PID controller if it is not already
-    		if (!getPIDController().isEnable()) {
-    			gyro.reset();
-    			getPIDController().reset();
-//    			enable();
-    			disable();
-    		}
-    		// Sets the forward move speed to the move parameter
-    		moveSpeed = move;
+    	if (usePID) {
+	    	if (rotate == 0.0 && move > 0.0) {
+	    		// Enables the PID controller if it is not already
+	    		if (!getPIDController().isEnable()) {
+	    			getPIDController().setPID(KpForward, KiForward, KdForward);
+	    			getPIDController().reset();
+	    			gyro.reset();
+	    			enable();
+	    			gyro.reset();
+	    		}
+	    		// Sets the forward move speed to the move parameter
+	    		moveSpeed = move;
+	    	} else if (rotate == 0.0 && move < 0.0){
+	    		// Enables the PID controller if it is not already
+	    		if (!getPIDController().isEnable()) {
+	    			getPIDController().setPID(KpBackward, KiBackward, KdBackward);
+	    			getPIDController().reset();
+	    			gyro.reset();
+	    			enable();
+	    			gyro.reset();
+	    		}
+	    		// Sets the forward move speed to the move parameter
+	    		moveSpeed = move;
+	    	} else {
+	    		// Disables the PID controller if it enabled so the drivetrain can move freely
+	    		if (getPIDController().isEnable()) {
+	    			disable();
+	    		}
+	    		drivetrain.arcadeDrive(move, rotate);
+	    	}
     	} else {
-    		// Disables the PID controller if it enabled so the drivetrain can move freely
-    		if (getPIDController().isEnable()) {
-    			disable();
-    		}
     		drivetrain.arcadeDrive(move, rotate);
     	}
     }
@@ -316,6 +339,9 @@ public void sendToDashboard() {
     	// PID Info
     	SmartDashboard.putNumber("PID Output", pidOutput);
     	SmartDashboard.putNumber("DT IR Distance", getIRDistance());
+    	SmartDashboard.putNumber("P Actual", getPIDGain('p'));
+    	SmartDashboard.putNumber("I Actual", getPIDGain('i'));
+    	SmartDashboard.putNumber("D Actual", getPIDGain('d'));
 
     	//Encoder Info
     	SmartDashboard.putNumber("DT Encoder Raw", encoder.get());