From fc98a79dbb21b3f53657e76f776fcbdb043c462f Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 14 Aug 2019 22:17:44 -0700 Subject: [PATCH] Clean up PIDController interface in preparation for ProfiledPIDController --- .../native/cpp/controller/PIDController.cpp | 97 ++++++------ .../include/frc/controller/PIDController.h | 101 ++++++------ .../test/native/cpp/PIDInputOutputTest.cpp | 5 +- .../src/test/native/cpp/PIDToleranceTest.cpp | 80 +++------- .../src/main/native/cpp/MotorEncoderTest.cpp | 8 +- .../wpilibj/controller/PIDController.java | 146 ++++++++---------- .../wpi/first/wpilibj/PIDInputOutputTest.java | 3 +- .../wpi/first/wpilibj/PIDToleranceTest.java | 89 +++++------ .../wpi/first/wpilibj/MotorEncoderTest.java | 4 +- .../java/edu/wpi/first/wpilibj/PIDTest.java | 12 +- 10 files changed, 226 insertions(+), 319 deletions(-) diff --git a/wpilibc/src/main/native/cpp/controller/PIDController.cpp b/wpilibc/src/main/native/cpp/controller/PIDController.cpp index c38d2eda3c..0be3e87fc9 100644 --- a/wpilibc/src/main/native/cpp/controller/PIDController.cpp +++ b/wpilibc/src/main/native/cpp/controller/PIDController.cpp @@ -18,11 +18,7 @@ using namespace frc2; PIDController::PIDController(double Kp, double Ki, double Kd, units::second_t period) - : frc::SendableBase(false), - m_Kp(Kp), - m_Ki(Ki), - m_Kd(Kd), - m_period(period.to()) { + : frc::SendableBase(false), m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) { static int instances = 0; instances++; HAL_Report(HALUsageReporting::kResourceType_PIDController, instances); @@ -45,8 +41,6 @@ units::second_t PIDController::GetPeriod() const { return units::second_t(m_period); } -double PIDController::GetOutput() const { return m_output; } - void PIDController::SetSetpoint(double setpoint) { if (m_maximumInput > m_minimumInput) { m_setpoint = std::clamp(setpoint, m_minimumInput, m_maximumInput); @@ -57,25 +51,20 @@ void PIDController::SetSetpoint(double setpoint) { double PIDController::GetSetpoint() const { return m_setpoint; } -bool PIDController::AtSetpoint(double tolerance, double deltaTolerance, +bool PIDController::AtSetpoint(double positionTolerance, + double velocityTolerance, Tolerance toleranceType) const { - double deltaError = GetDeltaError(); - - if (toleranceType == Tolerance::kPercent) { - return std::abs(m_currError) < tolerance / 100 * m_inputRange && - std::abs(deltaError) < deltaTolerance / 100 * m_inputRange; + if (m_toleranceType == Tolerance::kPercent) { + return std::abs(m_positionError) < positionTolerance / 100 * m_inputRange && + std::abs(m_velocityError) < velocityTolerance / 100 * m_inputRange; } else { - return std::abs(m_currError) < tolerance && - std::abs(deltaError) < deltaTolerance; + return std::abs(m_positionError) < positionTolerance && + std::abs(m_velocityError) < velocityTolerance; } } bool PIDController::AtSetpoint() const { - return AtSetpoint(m_tolerance, m_deltaTolerance, m_toleranceType); -} - -void PIDController::SetContinuous(bool continuous) { - m_continuous = continuous; + return AtSetpoint(m_positionTolerance, m_velocityTolerance); } void PIDController::SetInputRange(double minimumInput, double maximumInput) { @@ -89,36 +78,53 @@ void PIDController::SetInputRange(double minimumInput, double maximumInput) { } } +void PIDController::EnableContinuousInput(double minimumInput, + double maximumInput) { + m_continuous = true; + SetInputRange(minimumInput, maximumInput); +} + +void PIDController::DisableContinuousInput() { m_continuous = false; } + void PIDController::SetOutputRange(double minimumOutput, double maximumOutput) { m_minimumOutput = minimumOutput; m_maximumOutput = maximumOutput; } -void PIDController::SetAbsoluteTolerance(double tolerance, - double deltaTolerance) { +void PIDController::SetAbsoluteTolerance(double positionTolerance, + double velocityTolerance) { m_toleranceType = Tolerance::kAbsolute; - m_tolerance = tolerance; - m_deltaTolerance = deltaTolerance; + m_positionTolerance = positionTolerance; + m_velocityTolerance = velocityTolerance; } -void PIDController::SetPercentTolerance(double tolerance, - double deltaTolerance) { +void PIDController::SetPercentTolerance(double positionTolerance, + double velocityTolerance) { m_toleranceType = Tolerance::kPercent; - m_tolerance = tolerance; - m_deltaTolerance = deltaTolerance; + m_positionTolerance = positionTolerance; + m_velocityTolerance = velocityTolerance; } -double PIDController::GetError() const { - return GetContinuousError(m_currError); +double PIDController::GetPositionError() const { + return GetContinuousError(m_positionError); } -/** - * Returns the change in error per second of the PIDController. - * - * @return The change in error per second. - */ -double PIDController::GetDeltaError() const { - return (m_currError - m_prevError) / m_period; +double PIDController::GetVelocityError() const { return m_velocityError; } + +double PIDController::Calculate(double measurement) { + m_prevError = m_positionError; + m_positionError = GetContinuousError(m_setpoint - measurement); + m_velocityError = (m_positionError - m_prevError) / m_period.to(); + + if (m_Ki != 0) { + m_totalError = + std::clamp(m_totalError + m_positionError * m_period.to(), + m_minimumOutput / m_Ki, m_maximumOutput / m_Ki); + } + + return std::clamp( + m_Kp * m_positionError + m_Ki * m_totalError + m_Kd * m_velocityError, + m_minimumOutput, m_maximumOutput); } double PIDController::Calculate(double measurement, double setpoint) { @@ -130,7 +136,6 @@ double PIDController::Calculate(double measurement, double setpoint) { void PIDController::Reset() { m_prevError = 0; m_totalError = 0; - m_output = 0; } void PIDController::InitSendable(frc::SendableBuilder& builder) { @@ -159,19 +164,3 @@ double PIDController::GetContinuousError(double error) const { return error; } - -double PIDController::Calculate(double measurement) { - m_prevError = m_currError; - m_currError = GetContinuousError(m_setpoint - measurement); - - if (m_Ki != 0) { - m_totalError = std::clamp(m_totalError + m_currError * m_period, - m_minimumOutput / m_Ki, m_maximumOutput / m_Ki); - } - - m_output = std::clamp(m_Kp * m_currError + m_Ki * m_totalError + - m_Kd * (m_currError - m_prevError) / m_period, - m_minimumOutput, m_maximumOutput); - - return m_output; -} diff --git a/wpilibc/src/main/native/include/frc/controller/PIDController.h b/wpilibc/src/main/native/include/frc/controller/PIDController.h index 4e57bf57fe..761573dde5 100644 --- a/wpilibc/src/main/native/include/frc/controller/PIDController.h +++ b/wpilibc/src/main/native/include/frc/controller/PIDController.h @@ -30,7 +30,7 @@ class PIDController : public frc::SendableBase { * @param Ki The integral coefficient. * @param Kd The derivative coefficient. * @param period The period between controller updates in seconds. The - * default is 0.02 seconds. + * default is 20 milliseconds. */ PIDController(double Kp, double Ki, double Kd, units::second_t period = 20_ms); @@ -102,16 +102,6 @@ class PIDController : public frc::SendableBase { */ units::second_t GetPeriod() const; - /** - * Returns the current controller output. - * - * This is always centered around zero and constrained to the min and max - * outputs. - * - * @return The latest calculated output. - */ - double GetOutput() const; - /** * Sets the setpoint for the PIDController. * @@ -131,37 +121,22 @@ class PIDController : public frc::SendableBase { * * This will return false until at least one input value has been computed. * - * @param tolerance The maximum allowable error. - * @param deltaTolerance The maximum allowable change in error. - * @param toleranceType The type of tolerances specified. + * @param positionTolerance The maximum allowable position error. + * @param velocityTolerance The maximum allowable velocity error. + * @param toleranceType The type of tolerance specified. */ bool AtSetpoint( - double tolerance, - double deltaTolerance = std::numeric_limits::infinity(), + double positionTolerance, + double velocityTolerance = std::numeric_limits::infinity(), Tolerance toleranceType = Tolerance::kAbsolute) const; /** * Returns true if the error is within the tolerance of the error. * - * Currently this just reports on target as the actual value passes through - * the setpoint. Ideally it should be based on being within the tolerance for - * some period of time. - * * This will return false until at least one input value has been computed. */ bool AtSetpoint() const; - /** - * Sets the PID controller to consider the input to be continuous. - * - * Rather then using the max and min input range as constraints, it considers - * them to be the same point and automatically calculates the shortest route - * to the setpoint. - * - * @param continuous true turns on continuous, false turns off continuous - */ - void SetContinuous(bool continuous = true); - /** * Sets the minimum and maximum values expected from the input. * @@ -170,6 +145,23 @@ class PIDController : public frc::SendableBase { */ void SetInputRange(double minimumInput, double maximumInput); + /** + * Enables continuous input. + * + * Rather then using the max and min input range as constraints, it considers + * them to be the same point and automatically calculates the shortest route + * to the setpoint. + * + * @param minimumInput The minimum value expected from the input. + * @param maximumInput The maximum value expected from the input. + */ + void EnableContinuousInput(double minimumInput, double maximumInput); + + /** + * Disables continuous input. + */ + void DisableContinuousInput(); + /** * Sets the minimum and maximum values to write. * @@ -182,36 +174,33 @@ class PIDController : public frc::SendableBase { * Sets the absolute error which is considered tolerable for use with * AtSetpoint(). * - * @param tolerance Error which is tolerable. - * @param deltaTolerance Change in error per second which is tolerable. + * @param positionTolerance Position error which is tolerable. + * @param velociytTolerance Velocity error which is tolerable. */ void SetAbsoluteTolerance( - double tolerance, - double deltaTolerance = std::numeric_limits::infinity()); + double positionTolerance, + double velocityTolerance = std::numeric_limits::infinity()); /** - * Sets the percentage error which is considered tolerable for use with + * Sets the percent error which is considered tolerable for use with * AtSetpoint(). * - * @param tolerance Percent error which is tolerable. - * @param deltaTolerance Change in percent error per second which is - * tolerable. + * @param positionTolerance Position error which is tolerable. + * @param velociytTolerance Velocity error which is tolerable. */ void SetPercentTolerance( - double tolerance, - double deltaTolerance = std::numeric_limits::infinity()); + double positionTolerance, + double velocityTolerance = std::numeric_limits::infinity()); /** * Returns the difference between the setpoint and the measurement. - * - * @return The error. */ - double GetError() const; + double GetPositionError() const; /** - * Returns the change in error per second. + * Returns the velocity error. */ - double GetDeltaError() const; + double GetVelocityError() const; /** * Returns the next output of the PID controller. @@ -256,7 +245,7 @@ class PIDController : public frc::SendableBase { double m_Kd; // The period (in seconds) of the control loop running this controller - double m_period; + units::second_t m_period; // |maximum output| double m_maximumOutput = 1.0; @@ -270,30 +259,30 @@ class PIDController : public frc::SendableBase { // Minimum input - limit setpoint to this double m_minimumInput = 0; - // input range - difference between maximum and minimum + // Input range - difference between maximum and minimum double m_inputRange = 0; // Do the endpoints wrap around? eg. Absolute encoder bool m_continuous = false; - // The error at the time of the most recent call to calculate() - double m_currError = 0; + // The error at the time of the most recent call to Calculate() + double m_positionError = 0; + double m_velocityError = 0; - // The error at the time of the second-most-recent call to calculate() (used + // The error at the time of the second-most-recent call to Calculate() (used // to compute velocity) - double m_prevError = std::numeric_limits::infinity(); + double m_prevError = 0; // The sum of the errors for use in the integral calc double m_totalError = 0; Tolerance m_toleranceType = Tolerance::kAbsolute; - // The percentage or absolute error that is considered at setpoint. - double m_tolerance = 0.05; - double m_deltaTolerance = std::numeric_limits::infinity(); + // The error that is considered at setpoint. + double m_positionTolerance = 0.05; + double m_velocityTolerance = std::numeric_limits::infinity(); double m_setpoint = 0; - double m_output = 0; }; } // namespace frc2 diff --git a/wpilibc/src/test/native/cpp/PIDInputOutputTest.cpp b/wpilibc/src/test/native/cpp/PIDInputOutputTest.cpp index aad1eca40e..f35b39316c 100644 --- a/wpilibc/src/test/native/cpp/PIDInputOutputTest.cpp +++ b/wpilibc/src/test/native/cpp/PIDInputOutputTest.cpp @@ -36,8 +36,7 @@ TEST_F(PIDInputOutputTest, InputRangeTest) { TEST_F(PIDInputOutputTest, ContinuousInputTest) { controller->SetP(1); - controller->SetInputRange(-180, 180); - controller->SetContinuous(true); + controller->EnableContinuousInput(-180, 180); EXPECT_TRUE(controller->Calculate(-179, 179) < 0); } @@ -65,6 +64,6 @@ TEST_F(PIDInputOutputTest, DerivativeGainOutputTest) { controller->Calculate(0, 0); - EXPECT_DOUBLE_EQ(-.01 / controller->GetPeriod().to(), + EXPECT_DOUBLE_EQ(-.01_s / controller->GetPeriod(), controller->Calculate(.0025, 0)); } diff --git a/wpilibc/src/test/native/cpp/PIDToleranceTest.cpp b/wpilibc/src/test/native/cpp/PIDToleranceTest.cpp index a1be0b9aed..8a9ebe98da 100644 --- a/wpilibc/src/test/native/cpp/PIDToleranceTest.cpp +++ b/wpilibc/src/test/native/cpp/PIDToleranceTest.cpp @@ -5,105 +5,71 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include "frc/Timer.h" #include "frc/controller/PIDController.h" #include "gtest/gtest.h" -using namespace frc; - class PIDToleranceTest : public testing::Test { protected: - const double setpoint = 50.0; - const double range = 200; - const double tolerance = 10.0; + static constexpr double kSetpoint = 50.0; + static constexpr double kRange = 200; + static constexpr double kTolerance = 10.0; - class FakeInput { - public: - double val = 0; - - double Get() { return val; } - }; - - class FakeOutput { - public: - void Set(double output) {} - }; - - FakeInput inp; - FakeOutput out; frc2::PIDController* pidController; void SetUp() override { pidController = new frc2::PIDController(0.5, 0.0, 0.0); - pidController->SetInputRange(-range / 2, range / 2); + pidController->SetInputRange(-kRange / 2, kRange / 2); } void TearDown() override { delete pidController; } - - void Reset() { inp.val = 0; } }; -TEST_F(PIDToleranceTest, Initial) { EXPECT_FALSE(pidController->AtSetpoint()); } +TEST_F(PIDToleranceTest, Initial) { EXPECT_TRUE(pidController->AtSetpoint()); } TEST_F(PIDToleranceTest, Absolute) { - Reset(); + pidController->SetAbsoluteTolerance(kTolerance); + pidController->SetSetpoint(kSetpoint); - pidController->SetAbsoluteTolerance(tolerance); - pidController->SetSetpoint(setpoint); + pidController->Calculate(0.0); EXPECT_FALSE(pidController->AtSetpoint()) << "Error was in tolerance when it should not have been. Error was " - << pidController->GetError(); + << pidController->GetPositionError(); - inp.val = setpoint + tolerance / 2; - for (int i = 0; i <= 50; i++) { - pidController->Calculate(inp.Get()); - } + pidController->Calculate(kSetpoint + kTolerance / 2); EXPECT_TRUE(pidController->AtSetpoint()) << "Error was not in tolerance when it should have been. Error was " - << pidController->GetError(); + << pidController->GetPositionError(); - inp.val = setpoint + 10 * tolerance; - for (int i = 0; i <= 50; i++) { - pidController->Calculate(inp.Get()); - } + pidController->Calculate(kSetpoint + 10 * kTolerance); EXPECT_FALSE(pidController->AtSetpoint()) << "Error was in tolerance when it should not have been. Error was " - << pidController->GetError(); + << pidController->GetPositionError(); } TEST_F(PIDToleranceTest, Percent) { - Reset(); + pidController->SetPercentTolerance(kTolerance); + pidController->SetSetpoint(kSetpoint); - pidController->SetPercentTolerance(tolerance); - pidController->SetSetpoint(setpoint); + pidController->Calculate(0.0); EXPECT_FALSE(pidController->AtSetpoint()) << "Error was in tolerance when it should not have been. Error was " - << pidController->GetError(); + << pidController->GetPositionError(); - inp.val = - setpoint + (tolerance) / 200 * - range; // half of percent tolerance away from setpoint - for (int i = 0; i <= 50; i++) { - pidController->Calculate(inp.Get()); - } + // Half of percent tolerance away from setpoint + pidController->Calculate(kSetpoint + (kTolerance / 2) / 100 * kRange); EXPECT_TRUE(pidController->AtSetpoint()) << "Error was not in tolerance when it should have been. Error was " - << pidController->GetError(); + << pidController->GetPositionError(); - inp.val = - setpoint + - (tolerance) / 50 * range; // double percent tolerance away from setPoint - - for (int i = 0; i <= 50; i++) { - pidController->Calculate(inp.Get()); - } + // Double percent tolerance away from setpoint + pidController->Calculate(kSetpoint + (kTolerance * 2) / 100 * kRange); EXPECT_FALSE(pidController->AtSetpoint()) << "Error was in tolerance when it should not have been. Error was " - << pidController->GetError(); + << pidController->GetPositionError(); } diff --git a/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp b/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp index 36c7027545..9e416c28af 100644 --- a/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp +++ b/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp @@ -152,11 +152,11 @@ TEST_P(MotorEncoderTest, PositionPIDController) { Wait(10.0); pidRunner.Stop(); - RecordProperty("PIDError", pidController.GetError()); + RecordProperty("PIDError", pidController.GetPositionError()); EXPECT_TRUE(pidController.AtSetpoint()) << "PID loop did not converge within 10 seconds. Goal was: " << goal - << " Error was: " << pidController.GetError(); + << " Error was: " << pidController.GetPositionError(); } /** @@ -179,11 +179,11 @@ TEST_P(MotorEncoderTest, VelocityPIDController) { pidRunner.StartPeriodic(pidController.GetPeriod()); Wait(10.0); pidRunner.Stop(); - RecordProperty("PIDError", pidController.GetError()); + RecordProperty("PIDError", pidController.GetPositionError()); EXPECT_TRUE(pidController.AtSetpoint()) << "PID loop did not converge within 10 seconds. Goal was: " << 600 - << " Error was: " << pidController.GetError(); + << " Error was: " << pidController.GetPositionError(); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java index 2613d930bd..6d34d67335 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java @@ -46,17 +46,18 @@ public class PIDController extends SendableBase { // Minimum input - limit setpoint to this private double m_minimumInput; - // input range - difference between maximum and minimum + // Input range - difference between maximum and minimum private double m_inputRange; // Do the endpoints wrap around? eg. Absolute encoder private boolean m_continuous; // The error at the time of the most recent call to calculate() - private double m_currError; + private double m_positionError; + private double m_velocityError; // The error at the time of the second-most-recent call to calculate() (used to compute velocity) - private double m_prevError = Double.POSITIVE_INFINITY; + private double m_prevError; // The sum of the errors for use in the integral calc private double m_totalError; @@ -68,11 +69,10 @@ public class PIDController extends SendableBase { private Tolerance m_toleranceType = Tolerance.kAbsolute; // The percentage or absolute error that is considered at setpoint. - private double m_tolerance = 0.05; - private double m_deltaTolerance = Double.POSITIVE_INFINITY; + private double m_positionTolerance = 0.05; + private double m_velocityTolerance = Double.POSITIVE_INFINITY; private double m_setpoint; - private double m_output; /** * Allocates a PIDController with the given constants for Kp, Ki, and Kd and a default period of @@ -191,17 +191,6 @@ public class PIDController extends SendableBase { return m_period; } - /** - * Returns the current controller output. - * - *

This is always centered around zero and constrained to the min and max outputs. - * - * @return The latest calculated output. - */ - public double getOutput() { - return m_output; - } - /** * Sets the setpoint for the PIDController. * @@ -233,52 +222,28 @@ public class PIDController extends SendableBase { * @return Whether the error is within the acceptable bounds. */ public boolean atSetpoint() { - return atSetpoint(m_tolerance, m_deltaTolerance, m_toleranceType); + return atSetpoint(m_positionTolerance, m_velocityTolerance, m_toleranceType); } /** * Returns true if the error and change in error are below the specified tolerances. * - * @param tolerance The maximum allowable error. - * @param deltaTolerance The maximum allowable change in error from the previous iteration. - * @param toleranceType Whether the given tolerance values are absolute, or percentages of the - * total input range. + * @param positionTolerance The maximum allowable position error. + * @param velocityTolerance The maximum allowable velocity error. + * @param toleranceType The type of tolerance specified. * @return Whether the error is within the acceptable bounds. */ - public boolean atSetpoint(double tolerance, double deltaTolerance, Tolerance toleranceType) { - double error = getError(); - double deltaError = (error - m_prevError) / getPeriod(); - + public boolean atSetpoint(double positionTolerance, double velocityTolerance, + Tolerance toleranceType) { if (toleranceType == Tolerance.kPercent) { - return Math.abs(error) < tolerance / 100 * m_inputRange - && Math.abs(deltaError) < deltaTolerance / 100 * m_inputRange; + return Math.abs(m_positionError) < positionTolerance / 100 * m_inputRange + && Math.abs(m_velocityError) < velocityTolerance / 100 * m_inputRange; } else { - return Math.abs(error) < tolerance && Math.abs(deltaError) < deltaTolerance; + return Math.abs(m_positionError) < positionTolerance + && Math.abs(m_velocityError) < velocityTolerance; } } - /** - * Sets the PID controller to consider the input to be continuous. - * - *

Rather then using the max and min input range as constraints, it considers them to be the - * same point and automatically calculates the shortest route to the setpoint. - */ - public void setContinuous() { - setContinuous(true); - } - - /** - * Sets the PID controller to consider the input to be continuous, - * - *

Rather then using the max and min input range as constraints, it considers them to be the - * same point and automatically calculates the shortest route to the setpoint. - * - * @param continuous true turns on continuous, false turns off continuous - */ - public void setContinuous(boolean continuous) { - m_continuous = continuous; - } - /** * Sets the minimum and maximum values expected from the input. * @@ -296,6 +261,28 @@ public class PIDController extends SendableBase { } } + /** + * Enables continuous input. + * + *

Rather then using the max and min input range as constraints, it considers + * them to be the same point and automatically calculates the shortest route + * to the setpoint. + * + * @param minimumInput The minimum value expected from the input. + * @param maximumInput The maximum value expected from the input. + */ + public void enableContinuousInput(double minimumInput, double maximumInput) { + m_continuous = true; + setInputRange(minimumInput, maximumInput); + } + + /** + * Disables continuous input. + */ + public void disableContinuousInput() { + m_continuous = false; + } + /** * Sets the minimum and maximum values to write. * @@ -310,43 +297,43 @@ public class PIDController extends SendableBase { /** * Sets the absolute error which is considered tolerable for use with atSetpoint(). * - * @param tolerance Absolute error which is tolerable. + * @param positionTolerance Position error which is tolerable. */ - public void setAbsoluteTolerance(double tolerance) { - setAbsoluteTolerance(tolerance, Double.POSITIVE_INFINITY); + public void setAbsoluteTolerance(double positionTolerance) { + setAbsoluteTolerance(positionTolerance, Double.POSITIVE_INFINITY); } /** * Sets the absolute error which is considered tolerable for use with atSetpoint(). * - * @param tolerance Absolute error which is tolerable. - * @param deltaTolerance Change in absolute error per second which is tolerable. + * @param positionTolerance Position error which is tolerable. + * @param velocityTolerance Velocity error which is tolerable. */ - public void setAbsoluteTolerance(double tolerance, double deltaTolerance) { + public void setAbsoluteTolerance(double positionTolerance, double velocityTolerance) { m_toleranceType = Tolerance.kAbsolute; - m_tolerance = tolerance; - m_deltaTolerance = deltaTolerance; + m_positionTolerance = positionTolerance; + m_velocityTolerance = velocityTolerance; } /** * Sets the percent error which is considered tolerable for use with atSetpoint(). * - * @param tolerance Percent error which is tolerable. + * @param positionTolerance Position error which is tolerable. */ - public void setPercentTolerance(double tolerance) { - setPercentTolerance(tolerance, Double.POSITIVE_INFINITY); + public void setPercentTolerance(double positionTolerance) { + setPercentTolerance(positionTolerance, Double.POSITIVE_INFINITY); } /** * Sets the percent error which is considered tolerable for use with atSetpoint(). * - * @param tolerance Percent error which is tolerable. - * @param deltaTolerance Change in percent error per second which is tolerable. + * @param positionTolerance Position error which is tolerable. + * @param velocityTolerance Velocity error which is tolerable. */ - public void setPercentTolerance(double tolerance, double deltaTolerance) { + public void setPercentTolerance(double positionTolerance, double velocityTolerance) { m_toleranceType = Tolerance.kPercent; - m_tolerance = tolerance; - m_deltaTolerance = deltaTolerance; + m_positionTolerance = positionTolerance; + m_velocityTolerance = velocityTolerance; } /** @@ -354,16 +341,15 @@ public class PIDController extends SendableBase { * * @return The error. */ - public double getError() { - return getContinuousError(m_currError); + public double getPositionError() { + return getContinuousError(m_positionError); } /** - * Returns the change in error per second. + * Returns the velocity error. */ - public double getDeltaError() { - double error = getError(); - return (error - m_prevError) / getPeriod(); + public double getVelocityError() { + return m_velocityError; } /** @@ -384,19 +370,18 @@ public class PIDController extends SendableBase { * @param measurement The current measurement of the process variable. */ public double calculate(double measurement) { - m_prevError = m_currError; - m_currError = getContinuousError(m_setpoint - measurement); + m_prevError = m_positionError; + m_positionError = getContinuousError(m_setpoint - measurement); + m_velocityError = (m_positionError - m_prevError) / m_period; if (m_Ki != 0) { - m_totalError = clamp(m_totalError + m_currError * getPeriod(), m_minimumOutput / m_Ki, + m_totalError = clamp(m_totalError + m_positionError * m_period, m_minimumOutput / m_Ki, m_maximumOutput / m_Ki); } - m_output = clamp( - m_Kp * m_currError + m_Ki * m_totalError + m_Kd * (m_currError - m_prevError) / getPeriod(), + return clamp( + m_Kp * m_positionError + m_Ki * m_totalError + m_Kd * m_velocityError, m_minimumOutput, m_maximumOutput); - - return m_output; } /** @@ -405,7 +390,6 @@ public class PIDController extends SendableBase { public void reset() { m_prevError = 0; m_totalError = 0; - m_output = 0; } @Override diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDInputOutputTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDInputOutputTest.java index 6c73553aad..3a744643e4 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDInputOutputTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDInputOutputTest.java @@ -50,8 +50,7 @@ class PIDInputOutputTest { @Test void continuousInputTest() { m_controller.setP(1); - m_controller.setInputRange(-180, 180); - m_controller.setContinuous(true); + m_controller.enableContinuousInput(-180, 180); assertTrue(m_controller.calculate(-179, 179) < 0.0); } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java index 97ab624d1a..d4d63f9692 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java @@ -18,29 +18,14 @@ import static org.junit.jupiter.api.Assertions.assertTrue; class PIDToleranceTest { private PIDController m_pidController; - private static final double m_setpoint = 50.0; - private static final double m_tolerance = 10.0; - private static final double m_range = 200; - - private static class FakeInput { - public double m_val; - - FakeInput() { - m_val = 0; - } - - public double getMeasurement() { - return m_val; - } - } - - private FakeInput m_inp; + private static final double kSetpoint = 50.0; + private static final double kTolerance = 10.0; + private static final double kRange = 200; @BeforeEach void setUp() { - m_inp = new FakeInput(); m_pidController = new PIDController(0.05, 0.0, 0.0); - m_pidController.setInputRange(-m_range / 2, m_range / 2); + m_pidController.setInputRange(-kRange / 2, kRange / 2); } @AfterEach @@ -51,60 +36,56 @@ class PIDToleranceTest { @Test void initialToleranceTest() { - assertFalse(m_pidController.atSetpoint()); + assertTrue(m_pidController.atSetpoint()); } @Test void absoluteToleranceTest() { - m_pidController.setAbsoluteTolerance(m_tolerance); - m_pidController.setSetpoint(m_setpoint); - for (int i = 0; i < 50; i++) { - m_pidController.calculate(m_inp.getMeasurement()); - } + m_pidController.setAbsoluteTolerance(kTolerance); + m_pidController.setSetpoint(kSetpoint); + + m_pidController.calculate(0.0); + assertFalse(m_pidController.atSetpoint(), "Error was in tolerance when it should not have been. Error was " + m_pidController - .getError()); - m_inp.m_val = m_setpoint + m_tolerance / 2; - for (int i = 0; i < 50; i++) { - m_pidController.calculate(m_inp.getMeasurement()); - } + .getPositionError()); + + m_pidController.calculate(kSetpoint + kTolerance / 2); + assertTrue(m_pidController.atSetpoint(), "Error was not in tolerance when it should have been. Error was " + m_pidController - .getError()); - m_inp.m_val = m_setpoint + 10 * m_tolerance; - for (int i = 0; i < 50; i++) { - m_pidController.calculate(m_inp.getMeasurement()); - } + .getPositionError()); + + m_pidController.calculate(kSetpoint + 10 * kTolerance); + assertFalse(m_pidController.atSetpoint(), "Error was in tolerance when it should not have been. Error was " + m_pidController - .getError()); + .getPositionError()); } @Test void percentToleranceTest() { - m_pidController.setPercentTolerance(m_tolerance); - m_pidController.setSetpoint(m_setpoint); - for (int i = 0; i < 50; i++) { - m_pidController.calculate(m_inp.getMeasurement()); - } + m_pidController.setPercentTolerance(kTolerance); + m_pidController.setSetpoint(kSetpoint); + + m_pidController.calculate(0.0); + assertFalse(m_pidController.atSetpoint(), "Error was in tolerance when it should not have been. Error was " + m_pidController - .getError()); - //half of percent tolerance away from setPoint - m_inp.m_val = m_setpoint + m_tolerance / 200 * m_range; - for (int i = 0; i < 50; i++) { - m_pidController.calculate(m_inp.getMeasurement()); - } + .getPositionError()); + + // Half of percent tolerance away from setpoint + m_pidController.calculate(kSetpoint + (kTolerance / 2) / 100 * kRange); + assertTrue(m_pidController.atSetpoint(), "Error was not in tolerance when it should have been. Error was " + m_pidController - .getError()); - //double percent tolerance away from setPoint - m_inp.m_val = m_setpoint + m_tolerance / 50 * m_range; - for (int i = 0; i < 50; i++) { - m_pidController.calculate(m_inp.getMeasurement()); - } + .getPositionError()); + + // Double percent tolerance away from setpoint + m_pidController.calculate(kSetpoint + (kTolerance * 2) / 100 * kRange); + assertFalse(m_pidController.atSetpoint(), "Error was in tolerance when it should not have been. Error was " + m_pidController - .getError()); + .getPositionError()); } } 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 d981e0a845..48045eee75 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java @@ -190,7 +190,7 @@ public class MotorEncoderTest extends AbstractComsSetup { assertTrue( "PID loop did not reach reference within 10 seconds. The current error was" + pidController - .getError(), pidController.atSetpoint()); + .getPositionError(), pidController.atSetpoint()); pidController.close(); } @@ -211,7 +211,7 @@ public class MotorEncoderTest extends AbstractComsSetup { pidRunner.stop(); assertTrue("PID loop did not reach reference within 10 seconds. The error was: " + pidController - .getError(), pidController.atSetpoint()); + .getPositionError(), pidController.atSetpoint()); pidController.close(); } 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 4fed5191cf..24eda490f2 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java @@ -131,8 +131,8 @@ public class PIDTest extends AbstractComsSetup { setupOutputRange(); double reference = 2500.0; m_controller.setSetpoint(reference); - assertEquals("PID.getError() did not start at " + reference, reference, m_controller.getError(), - 0); + assertEquals("PID.getPositionError() did not start at " + reference, + reference, m_controller.getPositionError(), 0); m_builder.updateTable(); assertEquals(k_p, m_table.getEntry("Kp").getDouble(9999999), 0); assertEquals(k_i, m_table.getEntry("Ki").getDouble(9999999), 0); @@ -155,17 +155,17 @@ public class PIDTest extends AbstractComsSetup { setupAbsoluteTolerance(); setupOutputRange(); double reference = 1000.0; - assertEquals(pidData() + "did not start at 0", 0, m_controller.getOutput(), 0); + 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.getError(), 0); + m_controller.getPositionError(), 0); Notifier pidRunner = new Notifier( () -> me.getMotor().set(m_controller.calculate(me.getEncoder().getDistance()))); pidRunner.startPeriodic(m_controller.getPeriod()); Timer.delay(5); pidRunner.stop(); - assertTrue(pidData() + "Was not on Target. Controller Error: " + m_controller.getError(), - m_controller.atSetpoint()); + assertTrue(pidData() + "Was not on Target. Controller Error: " + + m_controller.getPositionError(), m_controller.atSetpoint()); } private String pidData() {