From 6281ec0810a3c6c196209eb7d1f385ad05d55589 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Mon, 23 Sep 2024 13:57:20 -0400 Subject: [PATCH] [wpimath] PIDController: Update field and method names for error and errorDerivative (#7088) --- .../wpi/first/wpilibj/MotorEncoderTest.java | 4 +- .../java/edu/wpi/first/wpilibj/PIDTest.java | 9 +- .../first/math/controller/PIDController.java | 109 +++++++++++++----- .../controller/ProfiledPIDController.java | 8 +- .../native/cpp/controller/PIDController.cpp | 71 ++++++++---- .../include/frc/controller/PIDController.h | 50 ++++++-- .../frc/controller/ProfiledPIDController.h | 8 +- .../math/controller/PIDToleranceTest.java | 8 +- .../cpp/controller/PIDToleranceTest.cpp | 8 +- 9 files changed, 188 insertions(+), 87 deletions(-) diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java index 078ef5aa693..84c0d152f55 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java @@ -208,7 +208,7 @@ public void testPositionPIDController() { assertTrue( "PID loop did not reach reference within 10 seconds. The current error was" - + pidController.getPositionError(), + + pidController.getError(), pidController.atSetpoint()); } } @@ -233,7 +233,7 @@ public void testVelocityPIDController() { assertTrue( "PID loop did not reach reference within 10 seconds. The error was: " - + pidController.getPositionError(), + + pidController.getError(), pidController.atSetpoint()); } } diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java index f2cd12fd0ad..909194556c8 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java @@ -133,7 +133,7 @@ public void testInitialSettings() { assertEquals( "PID.getPositionError() did not start at " + reference, reference, - m_controller.getPositionError(), + m_controller.getError(), 0); m_builder.update(); assertEquals(m_p, m_table.getEntry("Kp").getDouble(9999999), 0); @@ -160,10 +160,7 @@ public void testRotateToTarget() { assertEquals(pidData() + "did not start at 0", 0, me.getMotor().get(), 0); m_controller.setSetpoint(reference); assertEquals( - pidData() + "did not have an error of " + reference, - reference, - m_controller.getPositionError(), - 0); + pidData() + "did not have an error of " + reference, reference, m_controller.getError(), 0); Notifier pidRunner = new Notifier( () -> me.getMotor().set(m_controller.calculate(me.getEncoder().getDistance()))); @@ -171,7 +168,7 @@ public void testRotateToTarget() { Timer.delay(5); pidRunner.stop(); assertTrue( - pidData() + "Was not on Target. Controller Error: " + m_controller.getPositionError(), + pidData() + "Was not on Target. Controller Error: " + m_controller.getError(), m_controller.atSetpoint()); pidRunner.close(); 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 b55813bfa1d..044b332be07 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 @@ -42,8 +42,8 @@ public class PIDController implements Sendable, AutoCloseable { private boolean m_continuous; // The error at the time of the most recent call to calculate() - private double m_positionError; - private double m_velocityError; + private double m_error; + private double m_errorDerivative; // The error at the time of the second-most-recent call to calculate() (used to compute velocity) private double m_prevError; @@ -52,8 +52,8 @@ public class PIDController implements Sendable, AutoCloseable { private double m_totalError; // The error that is considered at setpoint. - private double m_positionTolerance = 0.05; - private double m_velocityTolerance = Double.POSITIVE_INFINITY; + private double m_errorTolerance = 0.05; + private double m_errorDerivativeTolerance = Double.POSITIVE_INFINITY; private double m_setpoint; private double m_measurement; @@ -227,18 +227,40 @@ public double getPeriod() { * Returns the position tolerance of this controller. * * @return the position tolerance of the controller. + * @deprecated Use getErrorTolerance() instead. */ + @Deprecated(forRemoval = true, since = "2025") public double getPositionTolerance() { - return m_positionTolerance; + return m_errorTolerance; } /** * Returns the velocity tolerance of this controller. * * @return the velocity tolerance of the controller. + * @deprecated Use getErrorDerivativeTolerance() instead. */ + @Deprecated(forRemoval = true, since = "2025") public double getVelocityTolerance() { - return m_velocityTolerance; + return m_errorDerivativeTolerance; + } + + /** + * Returns the error tolerance of this controller. + * + * @return the error tolerance of the controller. + */ + public double getErrorTolerance() { + return m_errorTolerance; + } + + /** + * Returns the error derivative tolerance of this controller. + * + * @return the error derivative tolerance of the controller. + */ + public double getErrorDerivativeTolerance() { + return m_errorDerivativeTolerance; } /** @@ -261,12 +283,12 @@ public void setSetpoint(double setpoint) { if (m_continuous) { double errorBound = (m_maximumInput - m_minimumInput) / 2.0; - m_positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound); + m_error = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound); } else { - m_positionError = m_setpoint - m_measurement; + m_error = m_setpoint - m_measurement; } - m_velocityError = (m_positionError - m_prevError) / m_period; + m_errorDerivative = (m_error - m_prevError) / m_period; } /** @@ -288,8 +310,8 @@ public double getSetpoint() { public boolean atSetpoint() { return m_haveMeasurement && m_haveSetpoint - && Math.abs(m_positionError) < m_positionTolerance - && Math.abs(m_velocityError) < m_velocityTolerance; + && Math.abs(m_error) < m_errorTolerance + && Math.abs(m_errorDerivative) < m_errorDerivativeTolerance; } /** @@ -338,39 +360,61 @@ public void setIntegratorRange(double minimumIntegral, double maximumIntegral) { /** * Sets the error which is considered tolerable for use with atSetpoint(). * - * @param positionTolerance Position error which is tolerable. + * @param errorTolerance Error which is tolerable. */ - public void setTolerance(double positionTolerance) { - setTolerance(positionTolerance, Double.POSITIVE_INFINITY); + public void setTolerance(double errorTolerance) { + setTolerance(errorTolerance, Double.POSITIVE_INFINITY); } /** * Sets the error which is considered tolerable for use with atSetpoint(). * - * @param positionTolerance Position error which is tolerable. - * @param velocityTolerance Velocity error which is tolerable. + * @param errorTolerance Error which is tolerable. + * @param errorDerivativeTolerance Error derivative which is tolerable. */ - public void setTolerance(double positionTolerance, double velocityTolerance) { - m_positionTolerance = positionTolerance; - m_velocityTolerance = velocityTolerance; + public void setTolerance(double errorTolerance, double errorDerivativeTolerance) { + m_errorTolerance = errorTolerance; + m_errorDerivativeTolerance = errorDerivativeTolerance; } /** * Returns the difference between the setpoint and the measurement. * * @return The error. + * @deprecated Use getError() instead. */ + @Deprecated(forRemoval = true, since = "2025") public double getPositionError() { - return m_positionError; + return m_error; } /** * Returns the velocity error. * * @return The velocity error. + * @deprecated Use getErrorDerivative() instead. */ + @Deprecated(forRemoval = true, since = "2025") public double getVelocityError() { - return m_velocityError; + return m_errorDerivative; + } + + /** + * Returns the difference between the setpoint and the measurement. + * + * @return The error. + */ + public double getError() { + return m_error; + } + + /** + * Returns the error derivative. + * + * @return The error derivative. + */ + public double getErrorDerivative() { + return m_errorDerivative; } /** @@ -394,38 +438,38 @@ public double calculate(double measurement, double setpoint) { */ public double calculate(double measurement) { m_measurement = measurement; - m_prevError = m_positionError; + m_prevError = m_error; m_haveMeasurement = true; if (m_continuous) { double errorBound = (m_maximumInput - m_minimumInput) / 2.0; - m_positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound); + m_error = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound); } else { - m_positionError = m_setpoint - m_measurement; + m_error = m_setpoint - m_measurement; } - m_velocityError = (m_positionError - m_prevError) / m_period; + m_errorDerivative = (m_error - m_prevError) / m_period; // If the absolute value of the position error is greater than IZone, reset the total error - if (Math.abs(m_positionError) > m_iZone) { + if (Math.abs(m_error) > m_iZone) { m_totalError = 0; } else if (m_ki != 0) { m_totalError = MathUtil.clamp( - m_totalError + m_positionError * m_period, + m_totalError + m_error * m_period, m_minimumIntegral / m_ki, m_maximumIntegral / m_ki); } - return m_kp * m_positionError + m_ki * m_totalError + m_kd * m_velocityError; + return m_kp * m_error + m_ki * m_totalError + m_kd * m_errorDerivative; } /** Resets the previous error and the integral term. */ public void reset() { - m_positionError = 0; + m_error = 0; m_prevError = 0; m_totalError = 0; - m_velocityError = 0; + m_errorDerivative = 0; m_haveMeasurement = false; } @@ -446,5 +490,10 @@ public void initSendable(SendableBuilder builder) { } }); builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint); + builder.addDoubleProperty("measurement", () -> m_measurement, null); + builder.addDoubleProperty("error", this::getError, null); + builder.addDoubleProperty("error derivative", this::getErrorDerivative, null); + builder.addDoubleProperty("previous error", () -> this.m_prevError, null); + builder.addDoubleProperty("total error", this::getAccumulatedError, null); } } 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 4f6c3da1f5f..6c8cd6ea208 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 @@ -174,7 +174,7 @@ public double getPeriod() { * @return the position tolerance of the controller. */ public double getPositionTolerance() { - return m_controller.getPositionTolerance(); + return m_controller.getErrorTolerance(); } /** @@ -183,7 +183,7 @@ public double getPositionTolerance() { * @return the velocity tolerance of the controller. */ public double getVelocityTolerance() { - return m_controller.getVelocityTolerance(); + return m_controller.getErrorDerivativeTolerance(); } /** @@ -330,7 +330,7 @@ public void setTolerance(double positionTolerance, double velocityTolerance) { * @return The error. */ public double getPositionError() { - return m_controller.getPositionError(); + return m_controller.getError(); } /** @@ -339,7 +339,7 @@ public double getPositionError() { * @return The change in error per second. */ public double getVelocityError() { - return m_controller.getVelocityError(); + return m_controller.getErrorDerivative(); } /** diff --git a/wpimath/src/main/native/cpp/controller/PIDController.cpp b/wpimath/src/main/native/cpp/controller/PIDController.cpp index 1e8bfc2aec4..93fa6d5d121 100644 --- a/wpimath/src/main/native/cpp/controller/PIDController.cpp +++ b/wpimath/src/main/native/cpp/controller/PIDController.cpp @@ -103,11 +103,11 @@ units::second_t PIDController::GetPeriod() const { } double PIDController::GetPositionTolerance() const { - return m_positionTolerance; + return m_error; } double PIDController::GetVelocityTolerance() const { - return m_velocityTolerance; + return m_errorDerivative; } double PIDController::GetAccumulatedError() const { @@ -120,13 +120,12 @@ void PIDController::SetSetpoint(double setpoint) { if (m_continuous) { double errorBound = (m_maximumInput - m_minimumInput) / 2.0; - m_positionError = - InputModulus(m_setpoint - m_measurement, -errorBound, errorBound); + m_error = InputModulus(m_setpoint - m_measurement, -errorBound, errorBound); } else { - m_positionError = m_setpoint - m_measurement; + m_error = m_setpoint - m_measurement; } - m_velocityError = (m_positionError - m_prevError) / m_period.value(); + m_errorDerivative = (m_error - m_prevError) / m_period.value(); } double PIDController::GetSetpoint() const { @@ -135,8 +134,8 @@ double PIDController::GetSetpoint() const { bool PIDController::AtSetpoint() const { return m_haveMeasurement && m_haveSetpoint && - std::abs(m_positionError) < m_positionTolerance && - std::abs(m_velocityError) < m_velocityTolerance; + std::abs(m_error) < m_errorTolerance && + std::abs(m_errorDerivative) < m_errorDerivativeTolerance; } void PIDController::EnableContinuousInput(double minimumInput, @@ -160,46 +159,61 @@ void PIDController::SetIntegratorRange(double minimumIntegral, m_maximumIntegral = maximumIntegral; } -void PIDController::SetTolerance(double positionTolerance, - double velocityTolerance) { - m_positionTolerance = positionTolerance; - m_velocityTolerance = velocityTolerance; +void PIDController::SetTolerance(double errorTolerance, + double errorDerivativeTolerance) { + m_errorTolerance = errorTolerance; + m_errorDerivativeTolerance = errorDerivativeTolerance; +} + +double PIDController::GetErrorTolerance() const { + return m_error; +} + +double PIDController::GetErrorDerivativeTolerance() const { + return m_errorDerivativeTolerance; +} + +double PIDController::GetError() const { + return m_error; +} + +double PIDController::GetErrorDerivative() const { + return m_errorDerivative; } double PIDController::GetPositionError() const { - return m_positionError; + return m_error; } double PIDController::GetVelocityError() const { - return m_velocityError; + return m_errorDerivative; } double PIDController::Calculate(double measurement) { m_measurement = measurement; - m_prevError = m_positionError; + m_prevError = m_error; m_haveMeasurement = true; if (m_continuous) { double errorBound = (m_maximumInput - m_minimumInput) / 2.0; - m_positionError = - InputModulus(m_setpoint - m_measurement, -errorBound, errorBound); + m_error = InputModulus(m_setpoint - m_measurement, -errorBound, errorBound); } else { - m_positionError = m_setpoint - m_measurement; + m_error = m_setpoint - m_measurement; } - m_velocityError = (m_positionError - m_prevError) / m_period.value(); + m_errorDerivative = (m_error - m_prevError) / m_period.value(); // If the absolute value of the position error is outside of IZone, reset the // total error - if (std::abs(m_positionError) > m_iZone) { + if (std::abs(m_error) > m_iZone) { m_totalError = 0; } else if (m_Ki != 0) { m_totalError = - std::clamp(m_totalError + m_positionError * m_period.value(), + std::clamp(m_totalError + m_error * m_period.value(), m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki); } - return m_Kp * m_positionError + m_Ki * m_totalError + m_Kd * m_velocityError; + return m_Kp * m_error + m_Ki * m_totalError + m_Kd * m_errorDerivative; } double PIDController::Calculate(double measurement, double setpoint) { @@ -209,10 +223,10 @@ double PIDController::Calculate(double measurement, double setpoint) { } void PIDController::Reset() { - m_positionError = 0; + m_error = 0; m_prevError = 0; m_totalError = 0; - m_velocityError = 0; + m_errorDerivative = 0; m_haveMeasurement = false; } @@ -230,4 +244,13 @@ void PIDController::InitSendable(wpi::SendableBuilder& builder) { builder.AddDoubleProperty( "setpoint", [this] { return GetSetpoint(); }, [this](double value) { SetSetpoint(value); }); + builder.AddDoubleProperty( + "measurement", [this] { return m_measurement; }, nullptr); + builder.AddDoubleProperty("error", [this] { return GetError(); }, nullptr); + builder.AddDoubleProperty( + "error derivative", [this] { return GetErrorDerivative(); }, nullptr); + builder.AddDoubleProperty( + "previous error", [this] { return m_prevError; }, nullptr); + builder.AddDoubleProperty( + "total error", [this] { return GetAccumulatedError(); }, nullptr); } diff --git a/wpimath/src/main/native/include/frc/controller/PIDController.h b/wpimath/src/main/native/include/frc/controller/PIDController.h index 90687e93446..c202462c973 100644 --- a/wpimath/src/main/native/include/frc/controller/PIDController.h +++ b/wpimath/src/main/native/include/frc/controller/PIDController.h @@ -121,18 +121,36 @@ class WPILIB_DLLEXPORT PIDController */ units::second_t GetPeriod() const; + /** + * Gets the error tolerance of this controller. + * + * @return The error tolerance of the controller. + */ + double GetErrorTolerance() const; + + /** + * Gets the error derivative tolerance of this controller. + * + * @return The error derivative tolerance of the controller. + */ + double GetErrorDerivativeTolerance() const; + /** * Gets the position tolerance of this controller. * * @return The position tolerance of the controller. + * @deprecated Use GetErrorTolerance() instead. */ + [[deprecated("Use the GetErrorTolerance method instead.")]] double GetPositionTolerance() const; /** * Gets the velocity tolerance of this controller. * * @return The velocity tolerance of the controller. + * @deprecated Use GetErrorDerivativeTolerance() instead. */ + [[deprecated("Use the GetErrorDerivativeTolerance method instead.")]] double GetVelocityTolerance() const; /** @@ -201,21 +219,35 @@ class WPILIB_DLLEXPORT PIDController /** * Sets the error which is considered tolerable for use with AtSetpoint(). * - * @param positionTolerance Position error which is tolerable. - * @param velocityTolerance Velocity error which is tolerable. + * @param errorTolerance error which is tolerable. + * @param errorDerivativeTolerance error derivative which is tolerable. + */ + void SetTolerance(double errorTolerance, + double errorDerivativeTolerance = + std::numeric_limits::infinity()); + + /** + * Returns the difference between the setpoint and the measurement. + */ + double GetError() const; + + /** + * Returns the error derivative. */ - void SetTolerance( - double positionTolerance, - double velocityTolerance = std::numeric_limits::infinity()); + double GetErrorDerivative() const; /** * Returns the difference between the setpoint and the measurement. + * @deprecated Use GetError() instead. */ + [[deprecated("Use GetError method instead.")]] double GetPositionError() const; /** * Returns the velocity error. + * @deprecated Use GetErrorDerivative() instead. */ + [[deprecated("Use GetErrorDerivative method instead.")]] double GetVelocityError() const; /** @@ -268,8 +300,8 @@ class WPILIB_DLLEXPORT PIDController bool m_continuous = false; // The error at the time of the most recent call to Calculate() - double m_positionError = 0; - double m_velocityError = 0; + double m_error = 0; + double m_errorDerivative = 0; // The error at the time of the second-most-recent call to Calculate() (used // to compute velocity) @@ -279,8 +311,8 @@ class WPILIB_DLLEXPORT PIDController double m_totalError = 0; // The error that is considered at setpoint. - double m_positionTolerance = 0.05; - double m_velocityTolerance = std::numeric_limits::infinity(); + double m_errorTolerance = 0.05; + double m_errorDerivativeTolerance = std::numeric_limits::infinity(); double m_setpoint = 0; double m_measurement = 0; diff --git a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h index bd5866f133f..a74991ea1aa 100644 --- a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h +++ b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h @@ -162,7 +162,7 @@ class ProfiledPIDController * @return The position tolerance of the controller. */ double GetPositionTolerance() const { - return m_controller.GetPositionTolerance(); + return m_controller.GetErrorTolerance(); } /** @@ -171,7 +171,7 @@ class ProfiledPIDController * @return The velocity tolerance of the controller. */ double GetVelocityTolerance() const { - return m_controller.GetVelocityTolerance(); + return m_controller.GetErrorDerivativeTolerance(); } /** @@ -300,14 +300,14 @@ class ProfiledPIDController * @return The error. */ Distance_t GetPositionError() const { - return Distance_t{m_controller.GetPositionError()}; + return Distance_t{m_controller.GetError()}; } /** * Returns the change in error per second. */ Velocity_t GetVelocityError() const { - return Velocity_t{m_controller.GetVelocityError()}; + return Velocity_t{m_controller.GetErrorDerivative()}; } /** diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/PIDToleranceTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/PIDToleranceTest.java index 4fdb8670b39..73bd8c7a0fc 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/PIDToleranceTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/PIDToleranceTest.java @@ -36,28 +36,28 @@ void absoluteToleranceTest() { assertFalse( controller.atSetpoint(), "Error was in tolerance when it should not have been. Error was " - + controller.getPositionError()); + + controller.getError()); controller.calculate(0.0); assertFalse( controller.atSetpoint(), "Error was in tolerance when it should not have been. Error was " - + controller.getPositionError()); + + controller.getError()); controller.calculate(kSetpoint + kTolerance / 2); assertTrue( controller.atSetpoint(), "Error was not in tolerance when it should have been. Error was " - + controller.getPositionError()); + + controller.getError()); controller.calculate(kSetpoint + 10 * kTolerance); assertFalse( controller.atSetpoint(), "Error was in tolerance when it should not have been. Error was " - + controller.getPositionError()); + + controller.getError()); } } } diff --git a/wpimath/src/test/native/cpp/controller/PIDToleranceTest.cpp b/wpimath/src/test/native/cpp/controller/PIDToleranceTest.cpp index f37be89861f..5ea4112d6e6 100644 --- a/wpimath/src/test/native/cpp/controller/PIDToleranceTest.cpp +++ b/wpimath/src/test/native/cpp/controller/PIDToleranceTest.cpp @@ -28,23 +28,23 @@ TEST(PIDToleranceTest, AbsoluteTolerance) { EXPECT_FALSE(controller.AtSetpoint()) << "Error was in tolerance when it should not have been. Error was " - << controller.GetPositionError(); + << controller.GetError(); controller.Calculate(0.0); EXPECT_FALSE(controller.AtSetpoint()) << "Error was in tolerance when it should not have been. Error was " - << controller.GetPositionError(); + << controller.GetError(); controller.Calculate(kSetpoint + kTolerance / 2); EXPECT_TRUE(controller.AtSetpoint()) << "Error was not in tolerance when it should have been. Error was " - << controller.GetPositionError(); + << controller.GetError(); controller.Calculate(kSetpoint + 10 * kTolerance); EXPECT_FALSE(controller.AtSetpoint()) << "Error was in tolerance when it should not have been. Error was " - << controller.GetPositionError(); + << controller.GetError(); }