disables pid once it is wihtin tolerance

DeepBlueRobotics · Mar 5, 2024 · 96f6cb5 · 96f6cb5
1 parent ca60b94
commit 96f6cb5
Showing 1 changed file with 4 additions and 0 deletions.
diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -5,6 +5,7 @@
 import org.carlmontrobotics.commands.ArmTeleop;
 import org.carlmontrobotics.lib199.MotorConfig;
 import org.carlmontrobotics.lib199.MotorControllerFactory;
+import org.opencv.core.Mat;
 
 import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.CANSparkBase;
@@ -165,6 +166,9 @@ private void driveArm() {
             armFeedVolts = armFeed.calculate(getCurrentArmGoal().position, 0);
         }
         armPIDMaster.setReference(setPoint.position, CANSparkBase.ControlType.kVelocity, 0, armFeedVolts);
+        if (armAtSetpoint()){
+            armPIDMaster.setIZone(Double.POSITIVE_INFINITY);//turns off pid once it reaches the setpoint
+        }
 
     }