Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +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

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