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

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