add OI for PIDless nudges

src/main/java/com/team766/robot/OI.java

@@ -133,10 +133,7 @@ public void run(Context context) {
 			}
 
 			// look for button presses to queue placement of intake/wrist/elevator superstructure
-			if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_NONE)) {
-				placementPosition = PlacementPosition.NONE;
-				setLightsForPlacement();
-			} else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_LOW)) {
+			if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_LOW)) {
 				placementPosition = PlacementPosition.LOW_NODE;
 				setLightsForPlacement();
 			} else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_MID)) {
@@ -155,8 +152,16 @@ public void run(Context context) {
 				new IntakeIn().run(context);
 			} else if (boxopGamepad.getButtonReleased(InputConstants.BUTTON_INTAKE_IN)) {
 				new IntakeIdle().run(context);
-			} else if (boxopGamepad.getButton(InputConstants.BUTTON_INTAKE_STOP)) {
+			}
+
+			// see if we should reset the button states
+			if (boxopGamepad.getButton(InputConstants.BUTTON_RESET_STATE)) {
+				// stop the intake
 				new IntakeStop().run(context);
+				// reset the placement position
+				placementPosition = PlacementPosition.NONE;
+				// reset the cone/cube selection to cones
+				new GoForCones().run(context);
 			}
 
 			// look for button hold to extend intake/wrist/elevator superstructure,
@@ -186,42 +191,68 @@ public void run(Context context) {
 				context.startAsync(new RetractWristvator());
 			}
 
+			// TODO: refactor this code.  it's getting gnarly.
 			// look for manual nudges
 			// we only allow these if the extend elevator trigger is extended
 			if (boxopGamepad.getButton(InputConstants.BUTTON_EXTEND_WRISTVATOR)) {
-
-				// look for elevator nudges
+				// the y axis is flipped from what we expect.  invert so up is positive, down is negative.
 				double elevatorNudgeAxis = -1 * boxopGamepad.getAxis(InputConstants.AXIS_ELEVATOR_MOVEMENT);
-				if (Math.abs(elevatorNudgeAxis) > 0.05) {
-					// elevatorManual = true;
-					context.takeOwnership(Robot.elevator);
-					// Robot.elevator.nudgeNoPID(elevatorNudgeAxis);
-					if (elevatorNudgeAxis > 0) {
-						Robot.elevator.nudgeUp();
-					} else {
-						Robot.elevator.nudgeDown();
+				double wristNudgeAxis = -1 * boxopGamepad.getAxis(InputConstants.AXIS_WRIST_MOVEMENT);
+
+				if (boxopGamepad.getButtonPressed(InputConstants.BUTTON_PLACEMENT_RESET_WRISTVATOR)) {
+					// bypass PID
+					if (Math.abs(elevatorNudgeAxis) > 0.05) {
+						elevatorManual = true;
+						context.takeOwnership(Robot.elevator);
+						Robot.elevator.nudgeNoPID(elevatorNudgeAxis);
+						context.releaseOwnership(Robot.elevator);
+					} else if (elevatorManual) {
+						context.takeOwnership(Robot.elevator);
+						Robot.elevator.stopElevator();
+						context.releaseOwnership(Robot.elevator);
+						elevatorManual = false;
 					}
-					context.releaseOwnership(Robot.elevator);
-				} else if (false && elevatorManual) {
-					Robot.elevator.stopElevator();
-					elevatorManual = false;
-				}
 
-				// look for wrist nudges
-				double wristNudgeAxis = -1 * boxopGamepad.getAxis(InputConstants.AXIS_WRIST_MOVEMENT);
-				if (Math.abs(wristNudgeAxis) > 0.05) {
-					// wristManual = true;
-					context.takeOwnership(Robot.wrist);
-					// Robot.wrist.nudgeNoPID(wristNudgeAxis);
-					if (wristNudgeAxis > 0) {
-						Robot.wrist.nudgeUp();
-					} else {
-						Robot.wrist.nudgeDown();
+					if ((Math.abs(wristNudgeAxis) > 0.05)) {
+						wristManual = true;
+						context.takeOwnership(Robot.wrist);
+						Robot.wrist.nudgeNoPID(wristNudgeAxis);
+						context.releaseOwnership(Robot.wrist);
+					} else if (wristManual) {
+						context.takeOwnership(Robot.wrist);
+						Robot.wrist.stopWrist();
+						context.releaseOwnership(Robot.wrist);
+						elevatorManual = false;
 					}
+				} else if (boxopGamepad.getButtonReleased(InputConstants.BUTTON_PLACEMENT_RESET_WRISTVATOR)) {
+					context.takeOwnership(Robot.wrist);
+					context.takeOwnership(Robot.elevator);
+					Robot.wrist.resetEncoder();
+					Robot.elevator.resetEncoder();
 					context.releaseOwnership(Robot.wrist);
-				} else if (false && wristManual) {
-					Robot.wrist.stopWrist();
-					wristManual = true;
+					context.releaseOwnership(Robot.elevator);
+				} else {
+					// look for elevator nudges
+					if (Math.abs(elevatorNudgeAxis) > 0.05) {
+						context.takeOwnership(Robot.elevator);
+						if (elevatorNudgeAxis > 0) {
+							Robot.elevator.nudgeUp();
+						} else {
+							Robot.elevator.nudgeDown();
+						}
+						context.releaseOwnership(Robot.elevator);
+					}
+
+					// look for wrist nudges
+					if (Math.abs(wristNudgeAxis) > 0.05) {
+						context.takeOwnership(Robot.wrist);
+						if (wristNudgeAxis > 0) {
+							Robot.wrist.nudgeUp();
+						} else {
+							Robot.wrist.nudgeDown();
+						}
+						context.releaseOwnership(Robot.wrist);
+					}
 				}
 			}
 

src/main/java/com/team766/robot/constants/InputConstants.java

@@ -29,20 +29,19 @@ public final class InputConstants {
 	// RT
 	public static final int BUTTON_EXTEND_WRISTVATOR = 8;
 	// Start
-	public static final int BUTTON_INTAKE_STOP = 10; // used for development
+	public static final int BUTTON_RESET_STATE = 10;
 
 	// left axis
 	public static final int AXIS_WRIST_MOVEMENT = 1;
 	// right axis
 	public static final int AXIS_ELEVATOR_MOVEMENT = 3;
 
-
 	// pov
 	public static final int POV_UP = 0;
 	public static final int POV_DOWN = 180;
 
-	// RB
-	public static final int BUTTON_PLACEMENT_NONE = 6;
+	// LB
+	public static final int BUTTON_PLACEMENT_RESET_WRISTVATOR = 5;
 	// X/A/B/Y
 	public static final int BUTTON_PLACEMENT_HUMAN_PLAYER = 1; // X
 	public static final int BUTTON_PLACEMENT_HIGH = 4; // Y

src/main/java/com/team766/robot/mechanisms/Elevator.java

@@ -81,7 +81,7 @@ public Elevator() {
 
 		rightMotor.follow(leftMotor, true);
 
-		leftMotor.getEncoder().setPosition(EncoderUtils.elevatorHeightToRotations(Position.RETRACTED.getHeight()));
+		resetEncoder();
 
 		pidController = leftMotor.getPIDController();
 		pidController.setFeedbackDevice(leftMotor.getEncoder());
@@ -95,6 +95,11 @@ public Elevator() {
 		maxAccel = ConfigFileReader.getInstance().getDouble(ELEVATOR_MAX_ACCEL);
 	}
 
+	public void resetEncoder() {
+		leftMotor.getEncoder().setPosition(
+				EncoderUtils.elevatorHeightToRotations(Position.RETRACTED.getHeight()));
+	}
+
 	public double getRotations() {
 		return leftMotor.getEncoder().getPosition();
 	}

src/main/java/com/team766/robot/mechanisms/Wrist.java

@@ -76,7 +76,7 @@ public Wrist() {
 		}
 		motor = (CANSparkMax) halMotor;
 
-		motor.getEncoder().setPosition(EncoderUtils.wristDegreesToRotations(Position.TOP.getAngle()));
+		resetEncoder();
 
 		// stash the PIDController for convenience.  will update the PID values to the latest from the config
 		// file each time we use the motor.
@@ -90,6 +90,11 @@ public Wrist() {
 		ffGain = ConfigFileReader.getInstance().getDouble(WRIST_FFGAIN);
 	}
 
+	public void resetEncoder() {
+		motor.getEncoder()
+				.setPosition(EncoderUtils.wristDegreesToRotations(Position.TOP.getAngle()));
+	}
+
 	public double getRotations() {
 		return motor.getEncoder().getPosition();
 	}