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

32
wpilibc/src/main/native/include/frc/controller/ControllerUtil.h
View File

@@ -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 <typename T>
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, double>) {
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, int>) {
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