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 + } }