[wpimath] Report error on negative PID gains (#6055)

Defaults PID gains to zero if any are invalid.

wpilibcExamples/src/main/cpp/examples/UltrasonicPID/include/Robot.h

@@ -31,9 +31,7 @@ class Robot : public frc::TimedRobot {
 
  private:
   // proportional speed constant
-  // negative because applying positive voltage will bring us closer to the
-  // target
-  static constexpr double kP = -0.001;
+  static constexpr double kP = 0.001;
   // integral speed constant
   static constexpr double kI = 0.0;
   // derivative speed constant

wpilibcExamples/src/test/cpp/examples/UltrasonicPID/cpp/UltrasonicPIDTest.cpp

@@ -57,7 +57,7 @@ class UltrasonicPIDTest : public testing::TestWithParam<double> {
     m_driveSim.Update(20_ms);
 
     auto startingDistance = units::meter_t{GetParam()};
-    m_distance = startingDistance - m_driveSim.GetLeftPosition();
+    m_distance = m_driveSim.GetLeftPosition() - startingDistance;
 
     m_ultrasonicSim.SetRange(m_distance);
   }
@@ -99,8 +99,7 @@ TEST_P(UltrasonicPIDTest, Auto) {
   }
 
   {
-    // advance 100 timesteps
-    frc::sim::StepTiming(2_s);
+    frc::sim::StepTiming(5_s);
 
     EXPECT_NEAR(Robot::kHoldDistance.value(), m_distance.value(), 10.0);
   }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java

@@ -21,8 +21,7 @@ public class Robot extends TimedRobot {
   static final double kHoldDistanceMillimeters = 1.0e3;
 
   // proportional speed constant
-  // negative because applying positive voltage will bring us closer to the target
-  private static final double kP = -0.001;
+  private static final double kP = 0.001;
   // integral speed constant
   private static final double kI = 0.0;
   // derivative speed constant

wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/UltrasonicPIDTest.java

@@ -80,7 +80,7 @@ class UltrasonicPIDTest {
               m_driveSim.update(0.02);
 
               double startingDistance = m_startToObject;
-              double range = startingDistance - m_driveSim.getLeftPositionMeters();
+              double range = m_driveSim.getLeftPositionMeters() - startingDistance;
 
               m_ultrasonicSim.setRangeMeters(range);
               m_distanceMM = range * 1.0e3;
@@ -126,10 +126,9 @@ class UltrasonicPIDTest {
     }
 
     {
-      // advance 100 timesteps
-      SimHooks.stepTiming(2.0);
+      SimHooks.stepTiming(5.0);
 
-      assertEquals(Robot.kHoldDistanceMillimeters, m_distanceMM, 10);
+      assertEquals(Robot.kHoldDistanceMillimeters, m_distanceMM, 10.0);
     }
   }
 }

wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java

@@ -68,6 +68,9 @@ public class PIDController implements Sendable, AutoCloseable {
    * @param kp The proportional coefficient.
    * @param ki The integral coefficient.
    * @param kd The derivative coefficient.
+   * @throws IllegalArgumentException if kp < 0
+   * @throws IllegalArgumentException if ki < 0
+   * @throws IllegalArgumentException if kd < 0
    */
   public PIDController(double kp, double ki, double kd) {
     this(kp, ki, kd, 0.02);
@@ -79,7 +82,11 @@ public class PIDController implements Sendable, AutoCloseable {
    * @param kp The proportional coefficient.
    * @param ki The integral coefficient.
    * @param kd The derivative coefficient.
-   * @param period The period between controller updates in seconds. Must be non-zero and positive.
+   * @param period The period between controller updates in seconds.
+   * @throws IllegalArgumentException if kp < 0
+   * @throws IllegalArgumentException if ki < 0
+   * @throws IllegalArgumentException if kd < 0
+   * @throws IllegalArgumentException if period <= 0
    */
   @SuppressWarnings("this-escape")
   public PIDController(double kp, double ki, double kd, double period) {
@@ -87,8 +94,17 @@ public class PIDController implements Sendable, AutoCloseable {
     m_ki = ki;
     m_kd = kd;
 
-    if (period <= 0) {
-      throw new IllegalArgumentException("Controller period must be a non-zero positive number!");
+    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!");
+    }
+    if (period <= 0.0) {
+      throw new IllegalArgumentException("Controller period must be a positive number!");
     }
     m_period = period;
 
@@ -121,7 +137,7 @@ public class PIDController implements Sendable, AutoCloseable {
   /**
    * Sets the Proportional coefficient of the PID controller gain.
    *
-   * @param kp proportional coefficient
+   * @param kp The proportional coefficient. Must be &gt;= 0.
    */
   public void setP(double kp) {
     m_kp = kp;
@@ -130,7 +146,7 @@ public class PIDController implements Sendable, AutoCloseable {
   /**
    * Sets the Integral coefficient of the PID controller gain.
    *
-   * @param ki integral coefficient
+   * @param ki The integral coefficient. Must be &gt;= 0.
    */
   public void setI(double ki) {
     m_ki = ki;
@@ -139,7 +155,7 @@ public class PIDController implements Sendable, AutoCloseable {
   /**
    * Sets the Differential coefficient of the PID controller gain.
    *
-   * @param kd differential coefficient
+   * @param kd The differential coefficient. Must be &gt;= 0.
    */
   public void setD(double kd) {
     m_kd = kd;
@@ -153,6 +169,7 @@ public class PIDController implements Sendable, AutoCloseable {
    * of {@link Double#POSITIVE_INFINITY} disables IZone functionality.
    *
    * @param iZone Maximum magnitude of error to allow integral control.
+   * @throws IllegalArgumentException if iZone &lt; 0
    */
   public void setIZone(double iZone) {
     if (iZone < 0) {

wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java

@@ -35,6 +35,9 @@ public class ProfiledPIDController implements Sendable {
    * @param Ki The integral coefficient.
    * @param Kd The derivative coefficient.
    * @param constraints Velocity and acceleration constraints for goal.
+   * @throws IllegalArgumentException if kp < 0
+   * @throws IllegalArgumentException if ki < 0
+   * @throws IllegalArgumentException if kd < 0
    */
   public ProfiledPIDController(
       double Kp, double Ki, double Kd, TrapezoidProfile.Constraints constraints) {
@@ -49,6 +52,10 @@ public class ProfiledPIDController implements Sendable {
    * @param Kd The derivative coefficient.
    * @param constraints Velocity and acceleration constraints for goal.
    * @param period The period between controller updates in seconds. The default is 0.02 seconds.
+   * @throws IllegalArgumentException if kp < 0
+   * @throws IllegalArgumentException if ki < 0
+   * @throws IllegalArgumentException if kd < 0
+   * @throws IllegalArgumentException if period <= 0
    */
   @SuppressWarnings("this-escape")
   public ProfiledPIDController(
@@ -67,9 +74,9 @@ public class ProfiledPIDController implements Sendable {
    *
    * <p>Sets the proportional, integral, and differential coefficients.
    *
-   * @param Kp Proportional coefficient
-   * @param Ki Integral coefficient
-   * @param Kd Differential coefficient
+   * @param Kp The proportional coefficient. Must be &gt;= 0.
+   * @param Ki The integral coefficient. Must be &gt;= 0.
+   * @param Kd The differential coefficient. Must be &gt;= 0.
    */
   public void setPID(double Kp, double Ki, double Kd) {
     m_controller.setPID(Kp, Ki, Kd);
@@ -78,7 +85,7 @@ public class ProfiledPIDController implements Sendable {
   /**
    * Sets the proportional coefficient of the PID controller gain.
    *
-   * @param Kp proportional coefficient
+   * @param Kp The proportional coefficient. Must be &gt;= 0.
    */
   public void setP(double Kp) {
     m_controller.setP(Kp);
@@ -87,7 +94,7 @@ public class ProfiledPIDController implements Sendable {
   /**
    * Sets the integral coefficient of the PID controller gain.
    *
-   * @param Ki integral coefficient
+   * @param Ki The integral coefficient. Must be &gt;= 0.
    */
   public void setI(double Ki) {
     m_controller.setI(Ki);
@@ -96,7 +103,7 @@ public class ProfiledPIDController implements Sendable {
   /**
    * Sets the differential coefficient of the PID controller gain.
    *
-   * @param Kd differential coefficient
+   * @param Kd The differential coefficient. Must be &gt;= 0.
    */
   public void setD(double Kd) {
     m_controller.setD(Kd);
@@ -110,6 +117,7 @@ public class ProfiledPIDController implements Sendable {
    * of {@link Double#POSITIVE_INFINITY} disables IZone functionality.
    *
    * @param iZone Maximum magnitude of error to allow integral control.
+   * @throws IllegalArgumentException if iZone &lt;= 0
    */
   public void setIZone(double iZone) {
     m_controller.setIZone(iZone);

wpimath/src/main/native/cpp/controller/PIDController.cpp

@@ -18,10 +18,32 @@ 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(
-        "Controller period must be a non-zero positive number, got {}!",
-        period.value());
+        "Controller period must be a positive number, got {}!", period.value());
     m_period = 20_ms;
     wpi::math::MathSharedStore::ReportWarning(
         "Controller period defaulted to 20ms.");

wpimath/src/main/native/include/frc/controller/PIDController.h

@@ -25,11 +25,11 @@ class WPILIB_DLLEXPORT PIDController
   /**
    * Allocates a PIDController with the given constants for Kp, Ki, and Kd.
    *
-   * @param Kp     The proportional coefficient.
-   * @param Ki     The integral coefficient.
-   * @param Kd     The derivative coefficient.
+   * @param Kp     The proportional coefficient. Must be >= 0.
+   * @param Ki     The integral coefficient. Must be >= 0.
+   * @param Kd     The derivative coefficient. Must be >= 0.
    * @param period The period between controller updates in seconds. The
-   *               default is 20 milliseconds. Must be non-zero and positive.
+   *               default is 20 milliseconds. Must be positive.
    */
   PIDController(double Kp, double Ki, double Kd,
                 units::second_t period = 20_ms);
@@ -46,30 +46,30 @@ class WPILIB_DLLEXPORT PIDController
    *
    * Sets the proportional, integral, and differential coefficients.
    *
-   * @param Kp Proportional coefficient
-   * @param Ki Integral coefficient
-   * @param Kd Differential coefficient
+   * @param Kp The proportional coefficient. Must be >= 0.
+   * @param Ki The integral coefficient. Must be >= 0.
+   * @param Kd The differential coefficient. Must be >= 0.
    */
   void SetPID(double Kp, double Ki, double Kd);
 
   /**
    * Sets the proportional coefficient of the PID controller gain.
    *
-   * @param Kp proportional coefficient
+   * @param Kp The proportional coefficient. Must be >= 0.
    */
   void SetP(double Kp);
 
   /**
    * Sets the integral coefficient of the PID controller gain.
    *
-   * @param Ki integral coefficient
+   * @param Ki The integral coefficient. Must be >= 0.
    */
   void SetI(double Ki);
 
   /**
    * Sets the differential coefficient of the PID controller gain.
    *
-   * @param Kd differential coefficient
+   * @param Kd The differential coefficient. Must be >= 0.
    */
   void SetD(double Kd);
 
@@ -81,7 +81,8 @@ class WPILIB_DLLEXPORT PIDController
    * 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.
+   * @param iZone Maximum magnitude of error to allow integral control. Must be
+   *   >= 0.
    */
   void SetIZone(double iZone);
 

wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h

@@ -49,12 +49,12 @@ class ProfiledPIDController
    * Kd. Users should call reset() when they first start running the controller
    * to avoid unwanted behavior.
    *
-   * @param Kp          The proportional coefficient.
-   * @param Ki          The integral coefficient.
-   * @param Kd          The derivative coefficient.
+   * @param Kp          The proportional coefficient. Must be >= 0.
+   * @param Ki          The integral coefficient. Must be >= 0.
+   * @param Kd          The derivative coefficient. Must be >= 0.
    * @param constraints Velocity and acceleration constraints for goal.
    * @param period      The period between controller updates in seconds. The
-   *                    default is 20 milliseconds.
+   *                    default is 20 milliseconds. Must be positive.
    */
   ProfiledPIDController(double Kp, double Ki, double Kd,
                         Constraints constraints, units::second_t period = 20_ms)
@@ -79,9 +79,9 @@ class ProfiledPIDController
    *
    * Sets the proportional, integral, and differential coefficients.
    *
-   * @param Kp Proportional coefficient
-   * @param Ki Integral coefficient
-   * @param Kd Differential coefficient
+   * @param Kp The proportional coefficient. Must be >= 0.
+   * @param Ki The integral coefficient. Must be >= 0.
+   * @param Kd The differential coefficient. Must be >= 0.
    */
   void SetPID(double Kp, double Ki, double Kd) {
     m_controller.SetPID(Kp, Ki, Kd);
@@ -90,21 +90,21 @@ class ProfiledPIDController
   /**
    * Sets the proportional coefficient of the PID controller gain.
    *
-   * @param Kp proportional coefficient
+   * @param Kp The proportional coefficient. Must be >= 0.
    */
   void SetP(double Kp) { m_controller.SetP(Kp); }
 
   /**
    * Sets the integral coefficient of the PID controller gain.
    *
-   * @param Ki integral coefficient
+   * @param Ki The integral coefficient. Must be >= 0.
    */
   void SetI(double Ki) { m_controller.SetI(Ki); }
 
   /**
    * Sets the differential coefficient of the PID controller gain.
    *
-   * @param Kd differential coefficient
+   * @param Kd The differential coefficient. Must be >= 0.
    */
   void SetD(double Kd) { m_controller.SetD(Kd); }
 
@@ -116,7 +116,8 @@ class ProfiledPIDController
    * 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.
+   * @param iZone Maximum magnitude of error to allow integral control. Must be
+   *   >= 0.
    */
   void SetIZone(double iZone) { m_controller.SetIZone(iZone); }