diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java index c541c4c7..b465a703 100644 --- 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); } -} \ No newline at end of file +}