From 96f6cb509ef73ef11b738edb0b1ad1d6b378f857 Mon Sep 17 00:00:00 2001 From: Brandon Date: Mon, 4 Mar 2024 18:48:26 -0800 Subject: [PATCH] disables pid once it is wihtin tolerance --- src/main/java/org/carlmontrobotics/subsystems/Arm.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java index ab364e9c..39d871af 100644 --- 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 + } }