Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Fix ProfiledPIDController continuous input (#2652)

Browse Source 
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.)
This commit is contained in:
Tyler Veness
2020-10-15 20:05:23 -07:00
committed by GitHub
parent 67859aea44
commit a112b5e231
9 changed files with 257 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ControllerUtil.java
View File

@@ -12,12 +12,6 @@ public final class ControllerUtil {
* Returns modulus of error where error is the difference between the reference
* and a measurement.
*
* <p>This implements modular subtraction defined as:
*
* <p>e = (r mod m - x mod m) mod m
*
* <p>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() {

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

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