[wpimath] PIDController: Add IZone (#5315)

Co-authored-by: Ryan Blue <ryanzblue@gmail.com>

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);
   }
 }

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);
   }
 }

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); });

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<double>::infinity();
+
   // The period (in seconds) of the control loop running this controller
   units::second_t m_period;
 

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}); });

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

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