Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/Robot…

…Code2024 into sofie-arm
DeepBlueRobotics · Mar 6, 2024 · 0b5d292 · 0b5d292
2 parents 43acf02 + ddf7573
commit 0b5d292
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
@@ -103,7 +103,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);
 
@@ -174,12 +174,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) {
@@ -257,4 +251,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);
     }
-}
+}