diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java index 1583c8ec65..1d98900beb 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java @@ -24,6 +24,9 @@ public class PIDController implements Sendable, AutoCloseable { // Factor for "derivative" control private double m_kd; + // The error range where "integral" control applies + private double m_iZone = Double.POSITIVE_INFINITY; + // The period (in seconds) of the loop that calls the controller private final double m_period; @@ -141,6 +144,22 @@ public class PIDController implements Sendable, AutoCloseable { m_kd = kd; } + /** + * Sets the IZone range. When the absolute value of the position error is greater than IZone, the + * total accumulated error will reset to zero, disabling integral gain until the absolute value of + * the position error is less than IZone. This is used to prevent integral windup. Must be + * non-negative. Passing a value of zero will effectively disable integral gain. Passing a value + * of {@link Double#POSITIVE_INFINITY} disables IZone functionality. + * + * @param iZone Maximum magnitude of error to allow integral control. + */ + public void setIZone(double iZone) { + if (iZone < 0) { + throw new IllegalArgumentException("IZone must be a non-negative number!"); + } + m_iZone = iZone; + } + /** * Get the Proportional coefficient. * @@ -168,6 +187,15 @@ public class PIDController implements Sendable, AutoCloseable { return m_kd; } + /** + * Get the IZone range. + * + * @return Maximum magnitude of error to allow integral control. + */ + public double getIZone() { + return m_iZone; + } + /** * Returns the period of this controller. * @@ -351,7 +379,10 @@ public class PIDController implements Sendable, AutoCloseable { m_velocityError = (m_positionError - m_prevError) / m_period; - if (m_ki != 0) { + // If the absolute value of the position error is greater than IZone, reset the total error + if (Math.abs(m_positionError) > m_iZone) { + m_totalError = 0; + } else if (m_ki != 0) { m_totalError = MathUtil.clamp( m_totalError + m_positionError * m_period, @@ -377,6 +408,7 @@ public class PIDController implements Sendable, AutoCloseable { builder.addDoubleProperty("p", this::getP, this::setP); builder.addDoubleProperty("i", this::getI, this::setI); builder.addDoubleProperty("d", this::getD, this::setD); + builder.addDoubleProperty("izone", this::getIZone, this::setIZone); builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java index a832472d3f..7637fabe13 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java @@ -98,6 +98,19 @@ public class ProfiledPIDController implements Sendable { m_controller.setD(Kd); } + /** + * Sets the IZone range. When the absolute value of the position error is greater than IZone, the + * total accumulated error will reset to zero, disabling integral gain until the absolute value of + * the position error is less than IZone. This is used to prevent integral windup. Must be + * non-negative. Passing a value of zero will effectively disable integral gain. Passing a value + * of {@link Double#POSITIVE_INFINITY} disables IZone functionality. + * + * @param iZone Maximum magnitude of error to allow integral control. + */ + public void setIZone(double iZone) { + m_controller.setIZone(iZone); + } + /** * Gets the proportional coefficient. * @@ -125,6 +138,15 @@ public class ProfiledPIDController implements Sendable { return m_controller.getD(); } + /** + * Get the IZone range. + * + * @return Maximum magnitude of error to allow integral control. + */ + public double getIZone() { + return m_controller.getIZone(); + } + /** * Gets the period of this controller. * @@ -400,6 +422,7 @@ public class ProfiledPIDController implements Sendable { builder.addDoubleProperty("p", this::getP, this::setP); builder.addDoubleProperty("i", this::getI, this::setI); builder.addDoubleProperty("d", this::getD, this::setD); + builder.addDoubleProperty("izone", this::getIZone, this::setIZone); builder.addDoubleProperty("goal", () -> getGoal().position, this::setGoal); } } diff --git a/wpimath/src/main/native/cpp/controller/PIDController.cpp b/wpimath/src/main/native/cpp/controller/PIDController.cpp index c2e6eb8d8d..2e7227bd7c 100644 --- a/wpimath/src/main/native/cpp/controller/PIDController.cpp +++ b/wpimath/src/main/native/cpp/controller/PIDController.cpp @@ -52,6 +52,14 @@ void PIDController::SetD(double Kd) { m_Kd = Kd; } +void PIDController::SetIZone(double iZone) { + if (iZone < 0) { + wpi::math::MathSharedStore::ReportError( + "IZone must be a non-negative number, got {}!", iZone); + } + m_iZone = iZone; +} + double PIDController::GetP() const { return m_Kp; } @@ -64,6 +72,10 @@ double PIDController::GetD() const { return m_Kd; } +double PIDController::GetIZone() const { + return m_iZone; +} + units::second_t PIDController::GetPeriod() const { return m_period; } @@ -151,7 +163,11 @@ double PIDController::Calculate(double measurement) { m_velocityError = (m_positionError - m_prevError) / m_period.value(); - if (m_Ki != 0) { + // If the absolute value of the position error is outside of IZone, reset the + // total error + if (std::abs(m_positionError) > m_iZone) { + m_totalError = 0; + } else if (m_Ki != 0) { m_totalError = std::clamp(m_totalError + m_positionError * m_period.value(), m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki); @@ -182,6 +198,9 @@ void PIDController::InitSendable(wpi::SendableBuilder& builder) { "i", [this] { return GetI(); }, [this](double value) { SetI(value); }); builder.AddDoubleProperty( "d", [this] { return GetD(); }, [this](double value) { SetD(value); }); + builder.AddDoubleProperty( + "izone", [this] { return GetIZone(); }, + [this](double value) { SetIZone(value); }); builder.AddDoubleProperty( "setpoint", [this] { return GetSetpoint(); }, [this](double value) { SetSetpoint(value); }); diff --git a/wpimath/src/main/native/include/frc/controller/PIDController.h b/wpimath/src/main/native/include/frc/controller/PIDController.h index d053017f8b..94c6056885 100644 --- a/wpimath/src/main/native/include/frc/controller/PIDController.h +++ b/wpimath/src/main/native/include/frc/controller/PIDController.h @@ -73,6 +73,18 @@ class WPILIB_DLLEXPORT PIDController */ void SetD(double Kd); + /** + * Sets the IZone range. When the absolute value of the position error is + * greater than IZone, the total accumulated error will reset to zero, + * disabling integral gain until the absolute value of the position error is + * less than IZone. This is used to prevent integral windup. Must be + * non-negative. Passing a value of zero will effectively disable integral + * gain. Passing a value of infinity disables IZone functionality. + * + * @param iZone Maximum magnitude of error to allow integral control. + */ + void SetIZone(double iZone); + /** * Gets the proportional coefficient. * @@ -94,6 +106,13 @@ class WPILIB_DLLEXPORT PIDController */ double GetD() const; + /** + * Get the IZone range. + * + * @return Maximum magnitude of error to allow integral control. + */ + double GetIZone() const; + /** * Gets the period of this controller. * @@ -221,6 +240,9 @@ class WPILIB_DLLEXPORT PIDController // Factor for "derivative" control double m_Kd; + // The error range where "integral" control applies + double m_iZone = std::numeric_limits::infinity(); + // The period (in seconds) of the control loop running this controller units::second_t m_period; diff --git a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h index 4e32c13c6d..92d2b72f9c 100644 --- a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h +++ b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h @@ -106,6 +106,18 @@ class ProfiledPIDController */ void SetD(double Kd) { m_controller.SetD(Kd); } + /** + * Sets the IZone range. When the absolute value of the position error is + * greater than IZone, the total accumulated error will reset to zero, + * disabling integral gain until the absolute value of the position error is + * less than IZone. This is used to prevent integral windup. Must be + * non-negative. Passing a value of zero will effectively disable integral + * gain. Passing a value of infinity disables IZone functionality. + * + * @param iZone Maximum magnitude of error to allow integral control. + */ + void SetIZone(double iZone) { m_controller.SetIZone(iZone); } + /** * Gets the proportional coefficient. * @@ -127,6 +139,13 @@ class ProfiledPIDController */ double GetD() const { return m_controller.GetD(); } + /** + * Get the IZone range. + * + * @return Maximum magnitude of error to allow integral control. + */ + double GetIZone() const { return m_controller.GetIZone(); } + /** * Gets the period of this controller. * @@ -377,6 +396,9 @@ class ProfiledPIDController "i", [this] { return GetI(); }, [this](double value) { SetI(value); }); builder.AddDoubleProperty( "d", [this] { return GetD(); }, [this](double value) { SetD(value); }); + builder.AddDoubleProperty( + "izone", [this] { return GetIZone(); }, + [this](double value) { SetIZone(value); }); builder.AddDoubleProperty( "goal", [this] { return GetGoal().position.value(); }, [this](double value) { SetGoal(Distance_t{value}); }); diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/PIDInputOutputTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/PIDInputOutputTest.java index 1fe4cb16f3..b25240e85f 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/PIDInputOutputTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/PIDInputOutputTest.java @@ -55,4 +55,24 @@ class PIDInputOutputTest { assertEquals(-0.01 / m_controller.getPeriod(), m_controller.calculate(0.0025, 0), 1e-5); } + + @Test + void iZoneNoOutputTest() { + m_controller.setI(1); + m_controller.setIZone(1); + + double out = m_controller.calculate(2, 0); + + assertEquals(0, out, 1e-5); + } + + @Test + void iZoneOutputTest() { + m_controller.setI(1); + m_controller.setIZone(1); + + double out = m_controller.calculate(1, 0); + + assertEquals(-1 * m_controller.getPeriod(), out, 1e-5); + } } diff --git a/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp b/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp index f1034f5510..a382b79d92 100644 --- a/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp +++ b/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp @@ -49,3 +49,21 @@ TEST_F(PIDInputOutputTest, DerivativeGainOutput) { EXPECT_DOUBLE_EQ(-10_ms / controller->GetPeriod(), controller->Calculate(0.0025, 0)); } + +TEST_F(PIDInputOutputTest, IZoneNoOutput) { + controller->SetI(1); + controller->SetIZone(1); + + double out = controller->Calculate(2, 0); + + EXPECT_DOUBLE_EQ(0, out); +} + +TEST_F(PIDInputOutputTest, IZoneOutput) { + controller->SetI(1); + controller->SetIZone(1); + + double out = controller->Calculate(1, 0); + + EXPECT_DOUBLE_EQ(-1 * controller->GetPeriod().value(), out); +}