turns off pid if arm overshoots

DeepBlueRobotics · Mar 5, 2024 · 0157dcd · 0157dcd
1 parent 9d00521
commit 0157dcd
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -166,7 +166,7 @@ private void driveArm() {
             armFeedVolts = armFeed.calculate(getCurrentArmGoal().position, 0);
         }
         armPIDMaster.setReference(setPoint.position, CANSparkBase.ControlType.kVelocity, 0, armFeedVolts);
-        if (armAtSetpoint()){
+        if (armAtSetpoint() || getArmPos() > setPoint.position){
             armPIDMaster.setIZone(Double.POSITIVE_INFINITY);//turns off pid once it reaches the setpoint
         }
         else {