From 9b6ffc201cf6d5936cd32a3596a62c0b378b5ab7 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Mon, 26 Aug 2019 21:40:30 -0700 Subject: [PATCH] Replace SetOutputRange() with SetIntegratorRange() If users are attempting to use the output range to limit the controller action, they should use ProfiledPIDController instead. If they actually intended to clamp the output, they should use std::clamp(). --- .../native/cpp/controller/PIDController.cpp | 13 +++++---- .../cpp/controller/ProfiledPIDController.cpp | 6 ++--- .../include/frc/controller/PIDController.h | 17 ++++++------ .../frc/controller/ProfiledPIDController.h | 11 +++++--- .../cpp/controller/PIDInputOutputTest.cpp | 8 ------ .../src/main/native/cpp/MotorEncoderTest.cpp | 3 +-- .../wpilibj/controller/PIDController.java | 27 +++++++++---------- .../controller/ProfiledPIDController.java | 13 +++++---- .../controller/PIDInputOutputTest.java | 12 --------- .../wpi/first/wpilibj/MotorEncoderTest.java | 3 +-- .../java/edu/wpi/first/wpilibj/PIDTest.java | 14 +++++----- 11 files changed, 55 insertions(+), 72 deletions(-) diff --git a/wpilibc/src/main/native/cpp/controller/PIDController.cpp b/wpilibc/src/main/native/cpp/controller/PIDController.cpp index 6fb38c4716..374fa74f46 100644 --- a/wpilibc/src/main/native/cpp/controller/PIDController.cpp +++ b/wpilibc/src/main/native/cpp/controller/PIDController.cpp @@ -64,9 +64,10 @@ void PIDController::EnableContinuousInput(double minimumInput, void PIDController::DisableContinuousInput() { m_continuous = false; } -void PIDController::SetOutputRange(double minimumOutput, double maximumOutput) { - m_minimumOutput = minimumOutput; - m_maximumOutput = maximumOutput; +void PIDController::SetIntegratorRange(double minimumIntegral, + double maximumIntegral) { + m_minimumIntegral = minimumIntegral; + m_maximumIntegral = maximumIntegral; } void PIDController::SetTolerance(double positionTolerance, @@ -89,12 +90,10 @@ double PIDController::Calculate(double measurement) { if (m_Ki != 0) { m_totalError = std::clamp(m_totalError + m_positionError * m_period.to(), - m_minimumOutput / m_Ki, m_maximumOutput / m_Ki); + m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki); } - return std::clamp( - m_Kp * m_positionError + m_Ki * m_totalError + m_Kd * m_velocityError, - m_minimumOutput, m_maximumOutput); + return m_Kp * m_positionError + m_Ki * m_totalError + m_Kd * m_velocityError; } double PIDController::Calculate(double measurement, double setpoint) { diff --git a/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp b/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp index 28dabe2dee..ca0ad00134 100644 --- a/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp +++ b/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp @@ -69,9 +69,9 @@ void ProfiledPIDController::DisableContinuousInput() { m_controller.DisableContinuousInput(); } -void ProfiledPIDController::SetOutputRange(double minimumOutput, - double maximumOutput) { - m_controller.SetOutputRange(minimumOutput, maximumOutput); +void ProfiledPIDController::SetIntegratorRange(double minimumIntegral, + double maximumIntegral) { + m_controller.SetIntegratorRange(minimumIntegral, maximumIntegral); } void ProfiledPIDController::SetTolerance(double positionTolerance, diff --git a/wpilibc/src/main/native/include/frc/controller/PIDController.h b/wpilibc/src/main/native/include/frc/controller/PIDController.h index e47d3caa4a..c1000d7f94 100644 --- a/wpilibc/src/main/native/include/frc/controller/PIDController.h +++ b/wpilibc/src/main/native/include/frc/controller/PIDController.h @@ -139,12 +139,15 @@ class PIDController : public frc::SendableBase { void DisableContinuousInput(); /** - * Sets the minimum and maximum values to write. + * Sets the minimum and maximum values for the integrator. * - * @param minimumOutput the minimum value to write to the output - * @param maximumOutput the maximum value to write to the output + * When the cap is reached, the integrator value is added to the controller + * output rather than the integrator value times the integral gain. + * + * @param minimumIntegral The minimum value of the integrator. + * @param maximumIntegral The maximum value of the integrator. */ - void SetOutputRange(double minimumOutput, double maximumOutput); + void SetIntegratorRange(double minimumIntegral, double maximumIntegral); /** * Sets the error which is considered tolerable for use with AtSetpoint(). @@ -211,11 +214,9 @@ class PIDController : public frc::SendableBase { // The period (in seconds) of the control loop running this controller units::second_t m_period; - // |maximum output| - double m_maximumOutput = 1.0; + double m_maximumIntegral = 1.0; - // |minimum output| - double m_minimumOutput = -1.0; + double m_minimumIntegral = -1.0; // Maximum input - limit setpoint to this double m_maximumInput = 0; diff --git a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h index a02d76d116..a39f76b0a6 100644 --- a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h +++ b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h @@ -168,12 +168,15 @@ class ProfiledPIDController : public SendableBase { void DisableContinuousInput(); /** - * Sets the minimum and maximum values to write. + * Sets the minimum and maximum values for the integrator. * - * @param minimumOutput the minimum value to write to the output - * @param maximumOutput the maximum value to write to the output + * When the cap is reached, the integrator value is added to the controller + * output rather than the integrator value times the integral gain. + * + * @param minimumIntegral The minimum value of the integrator. + * @param maximumIntegral The maximum value of the integrator. */ - void SetOutputRange(double minimumOutput, double maximumOutput); + void SetIntegratorRange(double minimumIntegral, double maximumIntegral); /** * Sets the error which is considered tolerable for use with diff --git a/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp b/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp index da60c1300c..7bdac2e4b0 100644 --- a/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp +++ b/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp @@ -17,14 +17,6 @@ class PIDInputOutputTest : public testing::Test { void TearDown() override { delete controller; } }; -TEST_F(PIDInputOutputTest, OutputRangeTest) { - controller->SetP(1); - controller->SetOutputRange(-50, 50); - - EXPECT_DOUBLE_EQ(-50, controller->Calculate(100, 0)); - EXPECT_DOUBLE_EQ(50, controller->Calculate(0, 100)); -} - TEST_F(PIDInputOutputTest, ContinuousInputTest) { controller->SetP(1); controller->EnableContinuousInput(-180, 180); diff --git a/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp b/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp index 227ee107a2..74242df745 100644 --- a/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp +++ b/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp @@ -141,7 +141,7 @@ TEST_P(MotorEncoderTest, PositionPIDController) { double goal = 1000; frc2::PIDController pidController(0.001, 0.01, 0.0); pidController.SetTolerance(50.0); - pidController.SetOutputRange(-0.2, 0.2); + pidController.SetIntegratorRange(-0.2, 0.2); pidController.SetSetpoint(goal); /* 10 seconds should be plenty time to get to the reference */ @@ -167,7 +167,6 @@ TEST_P(MotorEncoderTest, VelocityPIDController) { frc2::PIDController pidController(1e-5, 0.0, 0.0006); pidController.SetTolerance(200.0); - pidController.SetOutputRange(-0.3, 0.3); pidController.SetSetpoint(600); /* 10 seconds should be plenty time to get to the reference */ 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 d6c8e590cf..d5c677d54f 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 @@ -35,11 +35,9 @@ public class PIDController extends SendableBase { // The period (in seconds) of the loop that calls the controller private final double m_period; - // |maximum output| - private double m_maximumOutput = 1.0; + private double m_maximumIntegral = 1.0; - // |minimum output| - private double m_minimumOutput = -1.0; + private double m_minimumIntegral = -1.0; // Maximum input - limit setpoint to this private double m_maximumInput; @@ -244,14 +242,17 @@ public class PIDController extends SendableBase { } /** - * Sets the minimum and maximum values to write. + * Sets the minimum and maximum values for the integrator. * - * @param minimumOutput the minimum value to write to the output - * @param maximumOutput the maximum value to write to the output + *

When the cap is reached, the integrator value is added to the controller + * output rather than the integrator value times the integral gain. + * + * @param minimumIntegral The minimum value of the integrator. + * @param maximumIntegral The maximum value of the integrator. */ - public void setOutputRange(double minimumOutput, double maximumOutput) { - m_minimumOutput = minimumOutput; - m_maximumOutput = maximumOutput; + public void setIntegratorRange(double minimumIntegral, double maximumIntegral) { + m_minimumIntegral = minimumIntegral; + m_maximumIntegral = maximumIntegral; } /** @@ -314,12 +315,10 @@ public class PIDController extends SendableBase { if (m_Ki != 0) { m_totalError = MathUtils.clamp(m_totalError + m_positionError * m_period, - m_minimumOutput / m_Ki, m_maximumOutput / m_Ki); + m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki); } - return MathUtils.clamp( - m_Kp * m_positionError + m_Ki * m_totalError + m_Kd * m_velocityError, - m_minimumOutput, m_maximumOutput); + return m_Kp * m_positionError + m_Ki * m_totalError + m_Kd * m_velocityError; } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java index 25e839eb64..60e06fa64b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java @@ -210,13 +210,16 @@ public class ProfiledPIDController extends SendableBase { } /** - * Sets the minimum and maximum values to write. + * Sets the minimum and maximum values for the integrator. * - * @param minimumOutput the minimum value to write to the output - * @param maximumOutput the maximum value to write to the output + *

When the cap is reached, the integrator value is added to the controller + * output rather than the integrator value times the integral gain. + * + * @param minimumIntegral The minimum value of the integrator. + * @param maximumIntegral The maximum value of the integrator. */ - public void setOutputRange(double minimumOutput, double maximumOutput) { - m_controller.setOutputRange(minimumOutput, maximumOutput); + public void setIntegratorRange(double minimumIntegral, double maximumIntegral) { + m_controller.setIntegratorRange(minimumIntegral, maximumIntegral); } /** diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDInputOutputTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDInputOutputTest.java index a120b07ac6..67222d3e65 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDInputOutputTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDInputOutputTest.java @@ -10,7 +10,6 @@ package edu.wpi.first.wpilibj.controller; import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; -import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertTrue; @@ -22,17 +21,6 @@ class PIDInputOutputTest { m_controller = new PIDController(0, 0, 0); } - @Test - void outputRangeTest() { - m_controller.setP(1); - m_controller.setOutputRange(-50, 50); - - assertAll( - () -> assertEquals(-50, m_controller.calculate(100, 0), 1e-5), - () -> assertEquals(50, m_controller.calculate(0, 100), 1e-5) - ); - } - @Test void continuousInputTest() { m_controller.setP(1); 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 139eba3e5d..b5af375310 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java @@ -178,7 +178,7 @@ public class MotorEncoderTest extends AbstractComsSetup { public void testPositionPIDController() { PIDController pidController = new PIDController(0.001, 0.0005, 0); pidController.setTolerance(50.0); - pidController.setOutputRange(-0.2, 0.2); + pidController.setIntegratorRange(-0.2, 0.2); pidController.setSetpoint(1000); Notifier pidRunner = new Notifier( @@ -201,7 +201,6 @@ public class MotorEncoderTest extends AbstractComsSetup { LinearFilter filter = LinearFilter.movingAverage(50); PIDController pidController = new PIDController(1e-5, 0.0, 0.0006); pidController.setTolerance(200); - pidController.setOutputRange(-0.3, 0.3); pidController.setSetpoint(600); Notifier pidRunner = 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 3a62e18921..d366e218d8 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java @@ -45,7 +45,7 @@ public class PIDTest extends AbstractComsSetup { private SendableBuilderImpl m_builder; private static final double absoluteTolerance = 50; - private static final double outputRange = 0.25; + private static final double integratorRange = 0.25; private PIDController m_controller = null; private static MotorEncoderFixture me = null; @@ -121,14 +121,14 @@ public class PIDTest extends AbstractComsSetup { m_controller.setTolerance(absoluteTolerance); } - private void setupOutputRange() { - m_controller.setOutputRange(-outputRange, outputRange); + private void setupIntegratorRange() { + m_controller.setIntegratorRange(-integratorRange, integratorRange); } @Test public void testInitialSettings() { setupTolerance(); - setupOutputRange(); + setupIntegratorRange(); double reference = 2500.0; m_controller.setSetpoint(reference); assertEquals("PID.getPositionError() did not start at " + reference, @@ -144,7 +144,7 @@ public class PIDTest extends AbstractComsSetup { @Test public void testSetSetpoint() { setupTolerance(); - setupOutputRange(); + setupIntegratorRange(); double reference = 2500.0; m_controller.setSetpoint(reference); assertEquals("Did not correctly set reference", reference, m_controller.getSetpoint(), 1e-3); @@ -153,7 +153,7 @@ public class PIDTest extends AbstractComsSetup { @Test(timeout = 10000) public void testRotateToTarget() { setupTolerance(); - setupOutputRange(); + setupIntegratorRange(); double reference = 1000.0; assertEquals(pidData() + "did not start at 0", 0, me.getMotor().get(), 0); m_controller.setSetpoint(reference); @@ -178,7 +178,7 @@ public class PIDTest extends AbstractComsSetup { @Test(expected = RuntimeException.class) public void testOnTargetNoToleranceSet() { - setupOutputRange(); + setupIntegratorRange(); m_controller.atSetpoint(); } }