diff --git a/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/include/Robot.h index 4ff2700d6a..4d225764f0 100644 --- a/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/include/Robot.h +++ b/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 diff --git a/wpilibcExamples/src/test/cpp/examples/UltrasonicPID/cpp/UltrasonicPIDTest.cpp b/wpilibcExamples/src/test/cpp/examples/UltrasonicPID/cpp/UltrasonicPIDTest.cpp index a970a3a1bc..5cfcbe8dbc 100644 --- a/wpilibcExamples/src/test/cpp/examples/UltrasonicPID/cpp/UltrasonicPIDTest.cpp +++ b/wpilibcExamples/src/test/cpp/examples/UltrasonicPID/cpp/UltrasonicPIDTest.cpp @@ -57,7 +57,7 @@ class UltrasonicPIDTest : public testing::TestWithParam { 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); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java index 532e9ac448..8c22e423f0 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java +++ b/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 diff --git a/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/UltrasonicPIDTest.java b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/UltrasonicPIDTest.java index 342daa2922..f0a27b8f9c 100644 --- a/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/UltrasonicPIDTest.java +++ b/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); } } } 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 97c9551668..f7246848c2 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 @@ -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 >= 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 >= 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 >= 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 < 0 */ public void setIZone(double iZone) { if (iZone < 0) { 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 e0557b1bad..e69c73f12c 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 @@ -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 { * *

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. */ 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 >= 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 >= 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 >= 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 <= 0 */ public void setIZone(double iZone) { m_controller.setIZone(iZone); diff --git a/wpimath/src/main/native/cpp/controller/PIDController.cpp b/wpimath/src/main/native/cpp/controller/PIDController.cpp index 2638f9ec09..a67d56d0ea 100644 --- a/wpimath/src/main/native/cpp/controller/PIDController.cpp +++ b/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."); diff --git a/wpimath/src/main/native/include/frc/controller/PIDController.h b/wpimath/src/main/native/include/frc/controller/PIDController.h index 0d5b0a3f75..30ab8f07a9 100644 --- a/wpimath/src/main/native/include/frc/controller/PIDController.h +++ b/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); diff --git a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h index 39dc3add2c..72278affca 100644 --- a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h +++ b/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); }