correct Izone

DeepBlueRobotics · Mar 6, 2024 · ddf7573 · ddf7573
1 parent 5a37fa4
commit ddf7573
Showing 1 changed file with 2 additions and 8 deletions.
diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -105,7 +105,7 @@ public Arm() {
         armPIDMaster.setPositionPIDWrappingEnabled(true);
         armPIDMaster.setPositionPIDWrappingMinInput(LOWER_ANGLE_LIMIT_RAD);
         armPIDMaster.setPositionPIDWrappingMaxInput(UPPER_ANGLE_LIMIT_RAD);
-        // two PIDs?
+        armPIDMaster.setIZone(IZONE)
 
         SmartDashboard.putData("Arm", this);
 
@@ -176,12 +176,6 @@ private void driveArm() {
             armFeedVolts = armFeed.calculate(getCurrentArmGoal().position, 0);
         }
         armPIDMaster.setReference(setpoint.position, CANSparkBase.ControlType.kPosition, 0, armFeedVolts);
-        if (armAtSetpoint() || getArmPos() > setpoint.position) {
-            armPIDMaster.setIZone(Double.POSITIVE_INFINITY);// turns off pid once it reaches the setpoint
-        } else {
-            armPIDMaster.setIZone(IZONE);
-        }
-
     }
 
     public void setArmTarget(double targetPos) {
@@ -259,4 +253,4 @@ public double getArmClampedGoal(double goal) {
         return MathUtil.clamp(MathUtil.inputModulus(goal, ARM_DISCONT_RAD, ARM_DISCONT_RAD + 2 * Math.PI),
                 LOWER_ANGLE_LIMIT_RAD, UPPER_ANGLE_LIMIT_RAD);
     }
-}
+}