diff --git a/wpilibc/src/main/native/cpp/controller/PIDController.cpp b/wpilibc/src/main/native/cpp/controller/PIDController.cpp index 7c67519e61..aaa7461d69 100644 --- a/wpilibc/src/main/native/cpp/controller/PIDController.cpp +++ b/wpilibc/src/main/native/cpp/controller/PIDController.cpp @@ -76,8 +76,9 @@ double PIDController::GetSetpoint() const { bool PIDController::AtSetpoint() const { double positionError; if (m_continuous) { - positionError = frc::InputModulus(m_setpoint - m_measurement, - m_minimumInput, m_maximumInput); + double errorBound = (m_maximumInput - m_minimumInput) / 2.0; + positionError = + frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound); } else { positionError = m_setpoint - m_measurement; } @@ -128,8 +129,9 @@ double PIDController::Calculate(double measurement) { m_prevError = m_positionError; if (m_continuous) { - m_positionError = frc::InputModulus(m_setpoint - measurement, - m_minimumInput, m_maximumInput); + double errorBound = (m_maximumInput - m_minimumInput) / 2.0; + m_positionError = + frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound); } else { m_positionError = m_setpoint - measurement; } diff --git a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h index e6733e37a4..1afbba2694 100644 --- a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h +++ b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h @@ -253,10 +253,11 @@ class ProfiledPIDController double Calculate(Distance_t measurement) { if (m_controller.IsContinuousInputEnabled()) { // Get error which is smallest distance between goal and measurement + auto errorBound = (m_maximumInput - m_minimumInput) / 2.0; auto goalMinDistance = frc::InputModulus( - m_goal.position - measurement, m_minimumInput, m_maximumInput); + m_goal.position - measurement, -errorBound, errorBound); auto setpointMinDistance = frc::InputModulus( - m_setpoint.position - measurement, m_minimumInput, m_maximumInput); + m_setpoint.position - measurement, -errorBound, errorBound); // Recompute the profile goal with the smallest error, thus giving the // shortest path. The goal may be outside the input range after this diff --git a/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp b/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp index e0994b46ba..cc4507f34b 100644 --- a/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp +++ b/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp @@ -17,8 +17,10 @@ class PIDInputOutputTest : public testing::Test { TEST_F(PIDInputOutputTest, ContinuousInputTest) { controller->SetP(1); controller->EnableContinuousInput(-180, 180); + EXPECT_DOUBLE_EQ(controller->Calculate(-179, 179), -2); - EXPECT_LT(controller->Calculate(-179, 179), 0); + controller->EnableContinuousInput(0, 360); + EXPECT_DOUBLE_EQ(controller->Calculate(1, 359), -2); } TEST_F(PIDInputOutputTest, ProportionalGainOutputTest) { diff --git a/wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp b/wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp index 0f5e37e0be..10de3b9274 100644 --- a/wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp +++ b/wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp @@ -22,14 +22,6 @@ class ProfiledPIDInputOutputTest : public testing::Test { void TearDown() override { delete controller; } }; -TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest) { - controller->SetP(1); - controller->EnableContinuousInput(-180_deg, 180_deg); - - controller->Reset(-179_deg); - EXPECT_LT(controller->Calculate(-179_deg, 179_deg), 0); -} - TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest1) { controller->SetP(1); controller->EnableContinuousInput(-180_deg, 180_deg); @@ -80,6 +72,23 @@ TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest3) { units::radian_t{wpi::math::pi}); } +TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest4) { + controller->SetP(1); + controller->EnableContinuousInput(0_rad, + units::radian_t{2.0 * wpi::math::pi}); + + static constexpr units::radian_t kSetpoint{2.78}; + static constexpr units::radian_t kMeasurement{3.12}; + static constexpr units::radian_t kGoal{2.71}; + + controller->Reset(kSetpoint); + EXPECT_LT(controller->Calculate(kMeasurement, kGoal), 0.0); + + // Error must be less than half the input range at all times + EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement), + units::radian_t{wpi::math::pi}); +} + TEST_F(ProfiledPIDInputOutputTest, ProportionalGainOutputTest) { controller->SetP(4); 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 68e648da60..6050aa9f86 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 @@ -203,8 +203,8 @@ public class PIDController implements Sendable, AutoCloseable { public boolean atSetpoint() { double positionError; if (m_continuous) { - positionError = - MathUtil.inputModulus(m_setpoint - m_measurement, m_minimumInput, m_maximumInput); + double errorBound = (m_maximumInput - m_minimumInput) / 2.0; + positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound); } else { positionError = m_setpoint - m_measurement; } @@ -310,8 +310,8 @@ public class PIDController implements Sendable, AutoCloseable { m_prevError = m_positionError; if (m_continuous) { - m_positionError = - MathUtil.inputModulus(m_setpoint - measurement, m_minimumInput, m_maximumInput); + double errorBound = (m_maximumInput - m_minimumInput) / 2.0; + m_positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound); } else { m_positionError = m_setpoint - measurement; } 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 514bd31dd3..bd81d16605 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 @@ -270,10 +270,11 @@ public class ProfiledPIDController implements Sendable { public double calculate(double measurement) { if (m_controller.isContinuousInputEnabled()) { // Get error which is smallest distance between goal and measurement + double errorBound = (m_maximumInput - m_minimumInput) / 2.0; double goalMinDistance = - MathUtil.inputModulus(m_goal.position - measurement, m_minimumInput, m_maximumInput); + MathUtil.inputModulus(m_goal.position - measurement, -errorBound, errorBound); double setpointMinDistance = - MathUtil.inputModulus(m_setpoint.position - measurement, m_minimumInput, m_maximumInput); + MathUtil.inputModulus(m_setpoint.position - measurement, -errorBound, errorBound); // Recompute the profile goal with the smallest error, thus giving the shortest path. The goal // may be outside the input range after this operation, but that's OK because the controller 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 e8db694dd8..49bb4593a6 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 @@ -5,7 +5,6 @@ package edu.wpi.first.wpilibj.controller; import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertTrue; import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; @@ -22,8 +21,10 @@ class PIDInputOutputTest { void continuousInputTest() { m_controller.setP(1); m_controller.enableContinuousInput(-180, 180); + assertEquals(m_controller.calculate(-179, 179), -2, 1e-5); - assertTrue(m_controller.calculate(-179, 179) < 0.0); + m_controller.enableContinuousInput(0, 360); + assertEquals(m_controller.calculate(1, 359), -2, 1e-5); } @Test diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDInputOutputTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDInputOutputTest.java index 37265d494a..12f6c0b888 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDInputOutputTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDInputOutputTest.java @@ -67,6 +67,22 @@ class ProfiledPIDInputOutputTest { assertTrue(Math.abs(m_controller.getSetpoint().position - kMeasurement) < Math.PI); } + @Test + void continuousInputTest4() { + m_controller.setP(1); + m_controller.enableContinuousInput(0, 2.0 * Math.PI); + + final double kSetpoint = 2.78; + final double kMeasurement = 3.12; + final double kGoal = 2.71; + + m_controller.reset(kSetpoint); + assertTrue(m_controller.calculate(kMeasurement, kGoal) < 0.0); + + // Error must be less than half the input range at all times + assertTrue(Math.abs(m_controller.getSetpoint().position - kMeasurement) < Math.PI / 2.0); + } + @Test void proportionalGainOutputTest() { m_controller.setP(4);