Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-05 03:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Fix PIDController continuous range error calculations (#3170)

Browse Source 
The inputs should all be errors, so the range should be symmetric.

Fixes #3168.
Fixes #3304.
This commit is contained in:
Tyler Veness
2021-05-21 23:52:30 -07:00
committed by GitHub
parent 04dae799a2
commit 87384ea684
8 changed files with 55 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
wpilibc/src/main/native/cpp/controller/PIDController.cpp
View File

@@ -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;
}

5
wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
View File

@@ -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<Distance_t>(
m_goal.position - measurement, m_minimumInput, m_maximumInput);
m_goal.position - measurement, -errorBound, errorBound);
auto setpointMinDistance = frc::InputModulus<Distance_t>(
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

4
wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp
View File

@@ -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) {

25
wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
View File

@@ -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);