Speed up arm emergency lift

Mechanical-Advantage · Apr 12, 2023 · 4875766 · 4875766
1 parent 3e011e6
commit 4875766
Showing 1 changed file with 6 additions and 2 deletions.
diff --git a/src/main/java/org/littletonrobotics/frc2023/subsystems/arm/Arm.java b/src/main/java/org/littletonrobotics/frc2023/subsystems/arm/Arm.java
@@ -438,8 +438,12 @@ public void periodic() {
       elbowFeedback.reset();
 
     } else if (emergencyLiftDirection != LiftDirection.NONE) {
-      shoulderAngleSetpoint = emergencyLiftDirection == LiftDirection.FRONT ? 0.0 : Math.PI;
-      shoulderVoltageFeedback = shoulderFeedback.calculate(shoulderAngle, shoulderAngleSetpoint);
+      boolean runShoulder =
+          emergencyLiftDirection == LiftDirection.FRONT
+              ? shoulderAngle > Units.degreesToRadians(10.0)
+              : shoulderAngle < Units.degreesToRadians(170.0);
+      shoulderVoltageFeedback =
+          (runShoulder ? 6.0 : 0.0) * (emergencyLiftDirection == LiftDirection.FRONT ? -1.0 : 1.0);
       elbowAngleSetpoint = Math.PI;
       elbowVoltageFeedback = elbowFeedback.calculate(elbowAngle, elbowAngleSetpoint);
       io.setShoulderVoltage(shoulderVoltageFeedback);