From a112b5e231d0bf6d9e973da10f66a5ba01abe196 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Thu, 15 Oct 2020 20:05:23 -0700 Subject: [PATCH] [wpilib] Fix ProfiledPIDController continuous input (#2652) There were three bugs: 1. The input range variables used in ProfiledPIDController::Calculate() weren't being updated 2. The modulus error calculation was incorrect. 3. The setpoint wasn't being wrapped like the goal, so the invariant that the error remains less than half the input range was violated. (Thanks to @CptJJ for pointing this out and suggesting a fix.) --- .../include/frc/controller/ControllerUtil.h | 32 ++--- .../frc/controller/ProfiledPIDController.h | 9 +- .../cpp/controller/ControllerUtilTest.cpp | 5 + .../cpp/controller/PIDInputOutputTest.cpp | 4 +- .../controller/ProfiledPIDInputOutputTest.cpp | 111 ++++++++++++++++++ .../wpilibj/controller/ControllerUtil.java | 20 ++-- .../controller/ProfiledPIDController.java | 11 +- .../controller/ControllerUtilTest.java | 3 + .../ProfiledPIDInputOutputTest.java | 103 ++++++++++++++++ 9 files changed, 257 insertions(+), 41 deletions(-) create mode 100644 wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp create mode 100644 wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDInputOutputTest.java diff --git a/wpilibc/src/main/native/include/frc/controller/ControllerUtil.h b/wpilibc/src/main/native/include/frc/controller/ControllerUtil.h index 32dced6fb9..7603fde41c 100644 --- a/wpilibc/src/main/native/include/frc/controller/ControllerUtil.h +++ b/wpilibc/src/main/native/include/frc/controller/ControllerUtil.h @@ -18,12 +18,6 @@ namespace frc { * Returns modulus of error where error is the difference between the reference * and a measurement. * - * This implements modular subtraction defined as: - * - * e = (r mod m - x mod m) mod m - * - * with an offset in the modulus range for minimum input. - * * @param reference Reference input of a controller. * @param measurement The current measurement. * @param minimumInput The minimum value expected from the input. @@ -31,28 +25,18 @@ namespace frc { */ template T GetModulusError(T reference, T measurement, T minimumInput, T maximumInput) { + T error = reference - measurement; T modulus = maximumInput - minimumInput; - if constexpr (std::is_same_v) { - T error = std::fmod(reference, modulus) - std::fmod(measurement, modulus); + // Wrap error above maximum input + int numMax = (error + maximumInput) / modulus; + error -= numMax * modulus; - // Moduli on the difference arguments establish a precondition for the - // following modulus. - return std::fmod(error - minimumInput, modulus) + minimumInput; - } else if constexpr (std::is_same_v) { - T error = reference % modulus - measurement % modulus; + // Wrap error below minimum input + int numMin = (error + minimumInput) / modulus; + error -= numMin * modulus; - // Moduli on the difference arguments establish a precondition for the - // following modulus. - return (error - minimumInput) % modulus + minimumInput; - } else { - T error = units::math::fmod(reference, modulus) - - units::math::fmod(measurement, modulus); - - // Moduli on the difference arguments establish a precondition for the - // following modulus. - return units::math::fmod(error - minimumInput, modulus) + minimumInput; - } + return error; } } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h index 3f97d18bd6..fd246cd87a 100644 --- a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h +++ b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h @@ -196,6 +196,8 @@ class ProfiledPIDController void EnableContinuousInput(Distance_t minimumInput, Distance_t maximumInput) { m_controller.EnableContinuousInput(minimumInput.template to(), maximumInput.template to()); + m_minimumInput = minimumInput; + m_maximumInput = maximumInput; } /** @@ -254,8 +256,10 @@ class ProfiledPIDController double Calculate(Distance_t measurement) { if (m_controller.IsContinuousInputEnabled()) { // Get error which is smallest distance between goal and measurement - auto error = frc::GetModulusError( + auto goalMinDistance = frc::GetModulusError( m_goal.position, measurement, m_minimumInput, m_maximumInput); + auto setpointMinDistance = frc::GetModulusError( + m_setpoint.position, measurement, m_minimumInput, m_maximumInput); // Recompute the profile goal with the smallest error, thus giving the // shortest path. The goal may be outside the input range after this @@ -263,7 +267,8 @@ class ProfiledPIDController // report an error of zero. In other words, the setpoint only needs to be // offset from the measurement by the input range modulus; they don't need // to be equal. - m_goal.position = Distance_t{error} + measurement; + m_goal.position = goalMinDistance + measurement; + m_setpoint.position = setpointMinDistance + measurement; } frc::TrapezoidProfile profile{m_constraints, m_goal, m_setpoint}; diff --git a/wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp b/wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp index 9e34af8f6b..00259d1ad7 100644 --- a/wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp +++ b/wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp @@ -17,6 +17,11 @@ TEST(ControllerUtilTest, GetModulusError) { frc::GetModulusError(170.0 + 360.0, -170.0, -180.0, 180.0)); EXPECT_FLOAT_EQ(-20.0, frc::GetModulusError(170.0, -170.0 + 360.0, -180.0, 180.0)); + EXPECT_FLOAT_EQ(20.0, frc::GetModulusError(-170.0, 170.0, -180.0, 180.0)); + EXPECT_FLOAT_EQ(20.0, + frc::GetModulusError(-170.0 + 360.0, 170.0, -180.0, 180.0)); + EXPECT_FLOAT_EQ(20.0, + frc::GetModulusError(-170.0, 170.0 + 360.0, -180.0, 180.0)); // Test range starting at zero EXPECT_FLOAT_EQ(-20.0, frc::GetModulusError(170.0, 190.0, 0.0, 360.0)); diff --git a/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp b/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp index c68cb370e2..3580308e7b 100644 --- a/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp +++ b/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2014-2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2014-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -21,7 +21,7 @@ TEST_F(PIDInputOutputTest, ContinuousInputTest) { controller->SetP(1); controller->EnableContinuousInput(-180, 180); - EXPECT_TRUE(controller->Calculate(-179, 179) < 0); + EXPECT_LT(controller->Calculate(-179, 179), 0); } TEST_F(PIDInputOutputTest, ProportionalGainOutputTest) { diff --git a/wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp b/wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp new file mode 100644 index 0000000000..6f9c36d022 --- /dev/null +++ b/wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp @@ -0,0 +1,111 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include +#include +#include +#include + +#include "frc/controller/ProfiledPIDController.h" +#include "gtest/gtest.h" + +class ProfiledPIDInputOutputTest : public testing::Test { + protected: + frc::ProfiledPIDController* controller; + + void SetUp() override { + controller = new frc::ProfiledPIDController( + 0, 0, 0, {360_deg_per_s, 180_deg_per_s_sq}); + } + + 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); + + static constexpr units::degree_t kSetpoint{-179.0}; + static constexpr units::degree_t kMeasurement{-179.0}; + static constexpr units::degree_t kGoal{179.0}; + + 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), + 180_deg); +} + +TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest2) { + controller->SetP(1); + controller->EnableContinuousInput(-units::radian_t{wpi::math::pi}, + units::radian_t{wpi::math::pi}); + + static constexpr units::radian_t kSetpoint{-3.4826633343199735}; + static constexpr units::radian_t kMeasurement{-3.1352207333939606}; + static constexpr units::radian_t kGoal{-3.534162788601621}; + + 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, ContinuousInputTest3) { + controller->SetP(1); + controller->EnableContinuousInput(-units::radian_t{wpi::math::pi}, + units::radian_t{wpi::math::pi}); + + static constexpr units::radian_t kSetpoint{-3.5176604690006377}; + static constexpr units::radian_t kMeasurement{3.1191729343822456}; + static constexpr units::radian_t kGoal{2.709680418117445}; + + 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); + + EXPECT_DOUBLE_EQ(-0.1, controller->Calculate(0.025_deg, 0_deg)); +} + +TEST_F(ProfiledPIDInputOutputTest, IntegralGainOutputTest) { + controller->SetI(4); + + double out = 0; + + for (int i = 0; i < 5; i++) { + out = controller->Calculate(0.025_deg, 0_deg); + } + + EXPECT_DOUBLE_EQ(-0.5 * controller->GetPeriod().to(), out); +} + +TEST_F(ProfiledPIDInputOutputTest, DerivativeGainOutputTest) { + controller->SetD(4); + + controller->Calculate(0_deg, 0_deg); + + EXPECT_DOUBLE_EQ(-10_ms / controller->GetPeriod(), + controller->Calculate(0.0025_deg, 0_deg)); +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ControllerUtil.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ControllerUtil.java index f1e8716e4b..0896ac6d40 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ControllerUtil.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ControllerUtil.java @@ -12,12 +12,6 @@ public final class ControllerUtil { * Returns modulus of error where error is the difference between the reference * and a measurement. * - *

This implements modular subtraction defined as: - * - *

e = (r mod m - x mod m) mod m - * - *

with an offset in the modulus range for minimum input. - * * @param reference Reference input of a controller. * @param measurement The current measurement. * @param minimumInput The minimum value expected from the input. @@ -25,12 +19,18 @@ public final class ControllerUtil { */ public static double getModulusError(double reference, double measurement, double minimumInput, double maximumInput) { + double error = reference - measurement; double modulus = maximumInput - minimumInput; - double error = reference % modulus - measurement % modulus; - // Moduli on the difference arguments establish a precondition for the - // following modulus. - return (error - minimumInput) % modulus + minimumInput; + // Wrap error above maximum input + int numMax = (int) ((error + maximumInput) / modulus); + error -= numMax * modulus; + + // Wrap error below minimum input + int numMin = (int) ((error + minimumInput) / modulus); + error -= numMin * modulus; + + return error; } private ControllerUtil() { 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 5cbce5dd06..ec7c728233 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 @@ -218,6 +218,8 @@ public class ProfiledPIDController implements Sendable { */ public void enableContinuousInput(double minimumInput, double maximumInput) { m_controller.enableContinuousInput(minimumInput, maximumInput); + m_minimumInput = minimumInput; + m_maximumInput = maximumInput; } /** @@ -283,14 +285,17 @@ 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 error = ControllerUtil.getModulusError(m_goal.position, measurement, m_minimumInput, - m_maximumInput); + double goalMinDistance = ControllerUtil.getModulusError(m_goal.position, measurement, + m_minimumInput, m_maximumInput); + double setpointMinDistance = ControllerUtil.getModulusError(m_setpoint.position, measurement, + m_minimumInput, m_maximumInput); // 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 // will still go there and report an error of zero. In other words, the setpoint only needs to // be offset from the measurement by the input range modulus; they don't need to be equal. - m_goal.position = error + measurement; + m_goal.position = goalMinDistance + measurement; + m_setpoint.position = setpointMinDistance + measurement; } var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint); diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ControllerUtilTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ControllerUtilTest.java index 58e1223d84..307eccd166 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ControllerUtilTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ControllerUtilTest.java @@ -18,6 +18,9 @@ class ControllerUtilTest { assertEquals(-20.0, ControllerUtil.getModulusError(170.0, -170.0, -180.0, 180.0)); assertEquals(-20.0, ControllerUtil.getModulusError(170.0 + 360.0, -170.0, -180.0, 180.0)); assertEquals(-20.0, ControllerUtil.getModulusError(170.0, -170.0 + 360.0, -180.0, 180.0)); + assertEquals(20.0, ControllerUtil.getModulusError(-170.0, 170.0, -180.0, 180.0)); + assertEquals(20.0, ControllerUtil.getModulusError(-170.0 + 360.0, 170.0, -180.0, 180.0)); + assertEquals(20.0, ControllerUtil.getModulusError(-170.0, 170.0 + 360.0, -180.0, 180.0)); // Test range start at zero assertEquals(-20.0, ControllerUtil.getModulusError(170.0, 190.0, 0.0, 360.0)); 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 new file mode 100644 index 0000000000..9fca2a4b48 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDInputOutputTest.java @@ -0,0 +1,103 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.controller; + +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +class ProfiledPIDInputOutputTest { + private ProfiledPIDController m_controller; + + @BeforeEach + void setUp() { + m_controller = new ProfiledPIDController(0, 0, 0, + new TrapezoidProfile.Constraints(360, 180)); + } + + @Test + void continuousInputTest1() { + m_controller.setP(1); + m_controller.enableContinuousInput(-180, 180); + + final double kSetpoint = -179.0; + final double kMeasurement = -179.0; + final double kGoal = 179.0; + + 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) < 180.0); + } + + @Test + void continuousInputTest2() { + m_controller.setP(1); + m_controller.enableContinuousInput(-Math.PI, Math.PI); + + final double kSetpoint = -3.4826633343199735; + final double kMeasurement = -3.1352207333939606; + final double kGoal = -3.534162788601621; + + 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); + } + + @Test + void continuousInputTest3() { + m_controller.setP(1); + m_controller.enableContinuousInput(-Math.PI, Math.PI); + + final double kSetpoint = -3.5176604690006377; + final double kMeasurement = 3.1191729343822456; + final double kGoal = 2.709680418117445; + + 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); + } + + @Test + void proportionalGainOutputTest() { + m_controller.setP(4); + + assertEquals(-0.1, m_controller.calculate(0.025, 0), 1e-5); + } + + @Test + void integralGainOutputTest() { + m_controller.setI(4); + + double out = 0; + + for (int i = 0; i < 5; i++) { + out = m_controller.calculate(0.025, 0); + } + + assertEquals(-0.5 * m_controller.getPeriod(), out, 1e-5); + } + + @Test + void derivativeGainOutputTest() { + m_controller.setD(4); + + m_controller.calculate(0, 0); + + assertEquals(-0.01 / m_controller.getPeriod(), m_controller.calculate(0.0025, 0), 1e-5); + } +}