Fix issues in PIDController

wpilibsuite · Mar 28, 2024 · 709aef8 · 709aef8
1 parent 489e142
commit 709aef8
Show file tree

Hide file tree

Showing 3 changed files with 63 additions and 26 deletions.
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
@@ -127,38 +127,62 @@ public void close() {
    * @param kp The proportional coefficient.
    * @param ki The integral coefficient.
    * @param kd The derivative coefficient.
+   * @throws IllegalArgumentException if kp &lt; 0
+   * @throws IllegalArgumentException if ki &lt; 0
+   * @throws IllegalArgumentException if kd &lt; 0
    */
   public void setPID(double kp, double ki, double kd) {
     m_kp = kp;
     m_ki = ki;
     m_kd = kd;
+    if (kp < 0.0) {
+      throw new IllegalArgumentException("Kp must be a non-negative number!");
+    }
+    if (ki < 0.0) {
+      throw new IllegalArgumentException("Ki must be a non-negative number!");
+    }
+    if (kd < 0.0) {
+      throw new IllegalArgumentException("Kd must be a non-negative number!");
+    }
   }
 
   /**
    * Sets the Proportional coefficient of the PID controller gain.
    *
    * @param kp The proportional coefficient. Must be &gt;= 0.
+   * @throws IllegalArgumentException if kp &lt; 0
    */
   public void setP(double kp) {
     m_kp = kp;
+    if (kp < 0.0) {
+      throw new IllegalArgumentException("Kp must be a non-negative number!");
+    }
   }
 
   /**
    * Sets the Integral coefficient of the PID controller gain.
    *
    * @param ki The integral coefficient. Must be &gt;= 0.
+   * @throws IllegalArgumentException if ki &lt; 0
    */
   public void setI(double ki) {
     m_ki = ki;
+    if (ki < 0.0) {
+      throw new IllegalArgumentException("Ki must be a non-negative number!");
+    }
   }
 
   /**
    * Sets the Differential coefficient of the PID controller gain.
    *
    * @param kd The differential coefficient. Must be &gt;= 0.
+   * @throws IllegalArgumentException if kd &lt; 0
    */
   public void setD(double kd) {
     m_kd = kd;
+    if (kd < 0.0) {
+      throw new IllegalArgumentException("Kd must be a non-negative number!");
+    }
   }
 
   /**
@@ -315,8 +339,9 @@ public boolean isContinuousInputEnabled() {
   /**
    * Sets the minimum and maximum values for the integrator.
    *
-   * <p>When the cap is reached, the integrator value is added to the controller output rather than
-   * the integrator value times the integral gain.
+   * <p>If the integrator value is outside the range [minimumIntegral..maximumIntegral] the integral
+   * gain times the corresponding limit is added to the controller output rather than the integral
+   * gain times the integrator value.
    *
    * @param minimumIntegral The minimum value of the integrator.
    * @param maximumIntegral The maximum value of the integrator.

diff --git a/wpimath/src/main/native/cpp/controller/PIDController.cpp b/wpimath/src/main/native/cpp/controller/PIDController.cpp
@@ -18,28 +18,6 @@ using namespace frc;
 PIDController::PIDController(double Kp, double Ki, double Kd,
                              units::second_t period)
     : m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) {
-  bool invalidGains = false;
-  if (Kp < 0.0) {
-    wpi::math::MathSharedStore::ReportError(
-        "Kp must be a non-negative number, got {}!", Kp);
-    invalidGains = true;
-  }
-  if (Ki < 0.0) {
-    wpi::math::MathSharedStore::ReportError(
-        "Ki must be a non-negative number, got {}!", Ki);
-    invalidGains = true;
-  }
-  if (Kd < 0.0) {
-    wpi::math::MathSharedStore::ReportError(
-        "Kd must be a non-negative number, got {}!", Kd);
-    invalidGains = true;
-  }
-  if (invalidGains) {
-    m_Kp = 0.0;
-    m_Ki = 0.0;
-    m_Kd = 0.0;
-    wpi::math::MathSharedStore::ReportWarning("PID gains defaulted to 0.");
-  }
 
   if (period <= 0_s) {
     wpi::math::MathSharedStore::ReportError(
@@ -60,18 +38,47 @@ void PIDController::SetPID(double Kp, double Ki, double Kd) {
   m_Kp = Kp;
   m_Ki = Ki;
   m_Kd = Kd;
+  CheckGains();
+}
+
+void PIDController::CheckGains() {
+  bool invalidGains = false;
+  if (m_Kp < 0.0) {
+    wpi::math::MathSharedStore::ReportError(
+        "Kp must be a non-negative number, got {}!", m_Kp);
+    invalidGains = true;
+  }
+  if (m_Ki < 0.0) {
+    wpi::math::MathSharedStore::ReportError(
+        "Ki must be a non-negative number, got {}!", m_Ki);
+    invalidGains = true;
+  }
+  if (m_Kd < 0.0) {
+    wpi::math::MathSharedStore::ReportError(
+        "Kd must be a non-negative number, got {}!", m_Kd);
+    invalidGains = true;
+  }
+  if (invalidGains) {
+    m_Kp = 0.0;
+    m_Ki = 0.0;
+    m_Kd = 0.0;
+    wpi::math::MathSharedStore::ReportWarning("PID gains defaulted to 0.");
+  }
 }
 
 void PIDController::SetP(double Kp) {
   m_Kp = Kp;
+  CheckGains();
 }
 
 void PIDController::SetI(double Ki) {
   m_Ki = Ki;
+  CheckGains();
 }
 
 void PIDController::SetD(double Kd) {
   m_Kd = Kd;
+  CheckGains();
 }
 
 void PIDController::SetIZone(double iZone) {

diff --git a/wpimath/src/main/native/include/frc/controller/PIDController.h b/wpimath/src/main/native/include/frc/controller/PIDController.h
@@ -181,8 +181,9 @@ class WPILIB_DLLEXPORT PIDController
   /**
    * Sets the minimum and maximum values for the integrator.
    *
-   * When the cap is reached, the integrator value is added to the controller
-   * output rather than the integrator value times the integral gain.
+   * If the integrator value is outside the range [minimumIntegral..maximumIntegral]
+   * the integral gain times the corresponding limit is added to the controller output
+   * rather than the integral gain times the integrator value.
    *
    * @param minimumIntegral The minimum value of the integrator.
    * @param maximumIntegral The maximum value of the integrator.
@@ -278,6 +279,10 @@ class WPILIB_DLLEXPORT PIDController
 
   bool m_haveSetpoint = false;
   bool m_haveMeasurement = false;
+
+  // Check that Kp, Ki, Kd are all >= 0; if any are not, default
+  // all of them to 0.
+  void CheckGains();
 };
 
 }  // namespace frc