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

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

@@ -196,6 +196,8 @@ class ProfiledPIDController
void EnableContinuousInput(Distance_t minimumInput, Distance_t maximumInput) {
m_controller.EnableContinuousInput(minimumInput.template to<double>(),
maximumInput.template to<double>());
m_minimumInput = minimumInput;
m_maximumInput = maximumInput;
}
/**
@@ -254,8 +256,10 @@ class ProfiledPIDController
double Calculate(Distance_t measurement) {
if (m_controller.IsContinuousInputEnabled()) {
// Get error which is smallest distance between goal and measurement
auto error = frc::GetModulusError<Distance_t>(
auto goalMinDistance = frc::GetModulusError<Distance_t>(
m_goal.position, measurement, m_minimumInput, m_maximumInput);
auto setpointMinDistance = frc::GetModulusError<Distance_t>(
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
@@ -263,7 +267,8 @@ class ProfiledPIDController
// 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 = Distance_t{error} + measurement;
m_goal.position = goalMinDistance + measurement;
m_setpoint.position = setpointMinDistance + measurement;
}
frc::TrapezoidProfile<Distance> profile{m_constraints, m_goal, m_setpoint};