From 4875766dd1fe73b596cb6288cfe3ef4779a61cda Mon Sep 17 00:00:00 2001
From: Jonah <47046556+jonahb55@users.noreply.github.com>
Date: Tue, 11 Apr 2023 22:36:34 -0400
Subject: [PATCH] Speed up arm emergency lift

---
 .../org/littletonrobotics/frc2023/subsystems/arm/Arm.java | 8 ++++++--
 1 file changed, 6 insertions(+), 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
index a012d3e..ad7371a 100644
--- 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);