Merge pull request #16 from Team766/bringup-make-nudges-manual

make nudges bypass PID, esp to help with bringup

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

@@ -47,7 +47,9 @@ public void run(Context context) {
 		// context.takeOwnership(Robot.intake);
 		context.takeOwnership(Robot.gyro);
 
-		PlacementPosition placementPosition = null;
+		PlacementPosition placementPosition = PlacementPosition.NONE;
+		boolean elevatorManual = false;
+		boolean wristManual = false;
 
 		while (true) {
 			context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData());
@@ -126,7 +128,9 @@ public void run(Context context) {
 			}
 
 			// look for button presses to queue placement of intake/wrist/elevator superstructure
-			if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_LOW)) {
+			if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_NONE)) {
+				placementPosition = PlacementPosition.NONE;
+			} else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_LOW)) {
 				placementPosition = PlacementPosition.LOW_NODE;
 				// TODO: update lights
 			} else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_MID)) {
@@ -153,6 +157,8 @@ public void run(Context context) {
 			// release to retract
 			if (boxopGamepad.getButtonPressed(InputConstants.BUTTON_EXTEND_WRISTVATOR)) {
 				switch (placementPosition) {
+					case NONE:
+						break;
 					case LOW_NODE:
 						new ExtendWristvatorToLow().run(context);
 						break;
@@ -181,25 +187,25 @@ public void run(Context context) {
 				// look for elevator nudges
 				double elevatorNudgeAxis = boxopGamepad.getAxis(InputConstants.AXIS_ELEVATOR_MOVEMENT);
 				if (Math.abs(elevatorNudgeAxis) > 0.05) {
+					elevatorManual = true;
 					context.takeOwnership(Robot.elevator);
-					if (elevatorNudgeAxis > 0) {
-						Robot.elevator.nudgeUp();
-					} else if (elevatorNudgeAxis < 0) {
-						Robot.elevator.nudgeDown();
-					}
+					Robot.elevator.nudgeNoPID(elevatorNudgeAxis);
 					context.releaseOwnership(Robot.elevator);
+				} else if (elevatorManual) {
+					Robot.elevator.stopElevator();
+					elevatorManual = false;
 				}
 
 				// look for wrist nudges
 				double wristNudgeAxis = boxopGamepad.getAxis(InputConstants.AXIS_WRIST_MOVEMENT);
 				if (Math.abs(wristNudgeAxis) > 0.05) {
+					wristManual = true;
 					context.takeOwnership(Robot.wrist);
-					if (wristNudgeAxis > 0) {
-						Robot.wrist.nudgeUp();
-					} else if (wristNudgeAxis < 0) {
-						Robot.wrist.nudgeDown();
-					}
+					Robot.wrist.nudgeNoPID(wristNudgeAxis);
 					context.releaseOwnership(Robot.wrist);
+				} else if (wristManual) {
+					Robot.wrist.stopWrist();
+					wristManual = true;
 				}
 			}
 		}

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

@@ -1,6 +1,7 @@
 package com.team766.robot;
 
 public enum PlacementPosition {
+	NONE,
 	LOW_NODE,
 	MID_NODE,
 	HIGH_NODE,

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

@@ -41,6 +41,8 @@ public final class InputConstants {
 	public static final int POV_UP = 0;
 	public static final int POV_DOWN = 180;
 
+	// RB
+	public static final int BUTTON_PLACEMENT_NONE = 6;
 	// 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

@@ -10,6 +10,7 @@
 import com.team766.library.RateLimiter;
 import com.team766.library.ValueProvider;
 import com.team766.logging.Severity;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import static com.team766.robot.constants.ConfigConstants.*;
 
@@ -45,6 +46,7 @@ private int getHeight() {
 	}
 
 	private static final double NUDGE_INCREMENT = 5.0;
+	private static final double NUDGE_DAMPENER = 0.25;
 
 	private final CANSparkMax leftMotor;
 	private final CANSparkMax rightMotor;
@@ -100,6 +102,18 @@ public double getHeight() {
 		return EncoderUtils.elevatorRotationsToHeight(leftMotor.getEncoder().getPosition());
 	}
 
+	public void nudgeNoPID(double value) {
+		checkContextOwnership();
+		double clampedValue = MathUtil.clamp(value, -1, 1);
+		clampedValue *= NUDGE_DAMPENER; // make nudges less forceful.  TODO: make this non-linear
+		leftMotor.set(clampedValue);
+	}
+
+	public void stopElevator() {
+		checkContextOwnership();
+		leftMotor.set(0);
+	}
+
 	public void nudgeUp() {
 		double height = getHeight();
 		// NOTE: this could artificially limit nudge range

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

@@ -7,6 +7,7 @@
 import com.team766.library.RateLimiter;
 import com.team766.library.ValueProvider;
 import com.team766.logging.Severity;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.SparkMaxPIDController;
@@ -48,6 +49,7 @@ private double getAngle() {
 	}
 
 	private static final double NUDGE_INCREMENT = 5.0;
+	private static final double NUDGE_DAMPENER = 0.25;
 
 	private final CANSparkMax motor;
 	private final SparkMaxPIDController pidController;
@@ -91,6 +93,18 @@ public double getAngle() {
 		return EncoderUtils.wristRotationsToDegrees(motor.getEncoder().getPosition());
 	}
 
+	public void nudgeNoPID(double value) {
+		checkContextOwnership();
+		double clampedValue = MathUtil.clamp(value, -1, 1);
+		clampedValue *= NUDGE_DAMPENER; // make nudges less forceful. TODO: make this non-linear
+		motor.set(clampedValue);	
+	}
+
+	public void stopWrist() {
+		checkContextOwnership();
+		motor.set(0);
+	}
+
 	public void nudgeUp() {
 		double angle = getAngle();
 		double targetAngle = Math.min(angle + NUDGE_INCREMENT, Position.UP.getAngle());