Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +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);

8
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
View File

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

5
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java
View File

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

5
wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDInputOutputTest.java
View File

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

16
wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDInputOutputTest.java
View File

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