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

Fix ProfiledPIDController profile direction for continuous input (#2279)

Browse Source 
Previously, it could take the long way around. This recomputes the
profile goal with the shortest error, thus taking the shortest path.

Also removed the setpoint clamping from PIDController::SetSetpoint()
because it's unnecessary to make PIDController behave correctly for
a modular arithmetic input, and it breaks the setpoint calculation in
ProfiledPIDController otherwise.

Fixes #2277.
This commit is contained in:
Tyler Veness
2020-03-14 22:13:57 -07:00
committed by GitHub
parent 8edf9282c3
commit 84e300739c
9 changed files with 233 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -14,6 +14,7 @@
#include <units/units.h>
#include "frc/controller/ControllerUtil.h"
#include "frc/controller/PIDController.h"
#include "frc/smartdashboard/Sendable.h"
#include "frc/smartdashboard/SendableBuilder.h"
@@ -251,6 +252,20 @@ class ProfiledPIDController
* @param measurement The current measurement of the process variable.
*/
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>(
m_goal.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 = Distance_t{error} + measurement;
}
frc::TrapezoidProfile<Distance> profile{m_constraints, m_goal, m_setpoint};
m_setpoint = profile.Calculate(GetPeriod());
return m_controller.Calculate(measurement.template to<double>(),
@@ -337,6 +352,8 @@ class ProfiledPIDController
private:
frc2::PIDController m_controller;
Distance_t m_minimumInput{0};
Distance_t m_maximumInput{0};
typename frc::TrapezoidProfile<Distance>::State m_goal;
typename frc::TrapezoidProfile<Distance>::State m_setpoint;
typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;