Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +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

53
wpilibc/src/main/native/cpp/controller/PIDController.cpp
View File

@@ -12,6 +12,7 @@
#include <hal/FRCUsageReporting.h>
#include "frc/controller/ControllerUtil.h"
#include "frc/smartdashboard/SendableBuilder.h"
#include "frc/smartdashboard/SendableRegistry.h"
@@ -48,13 +49,7 @@ units::second_t PIDController::GetPeriod() const {
return units::second_t(m_period);
}
void PIDController::SetSetpoint(double setpoint) {
if (m_maximumInput > m_minimumInput) {
m_setpoint = std::clamp(setpoint, m_minimumInput, m_maximumInput);
} else {
m_setpoint = setpoint;
}
}
void PIDController::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
double PIDController::GetSetpoint() const { return m_setpoint; }
@@ -66,11 +61,14 @@ bool PIDController::AtSetpoint() const {
void PIDController::EnableContinuousInput(double minimumInput,
double maximumInput) {
m_continuous = true;
SetInputRange(minimumInput, maximumInput);
m_minimumInput = minimumInput;
m_maximumInput = maximumInput;
}
void PIDController::DisableContinuousInput() { m_continuous = false; }
bool PIDController::IsContinuousInputEnabled() const { return m_continuous; }
void PIDController::SetIntegratorRange(double minimumIntegral,
double maximumIntegral) {
m_minimumIntegral = minimumIntegral;
@@ -83,15 +81,20 @@ void PIDController::SetTolerance(double positionTolerance,
m_velocityTolerance = velocityTolerance;
}
double PIDController::GetPositionError() const {
return GetContinuousError(m_positionError);
}
double PIDController::GetPositionError() const { return m_positionError; }
double PIDController::GetVelocityError() const { return m_velocityError; }
double PIDController::Calculate(double measurement) {
m_prevError = m_positionError;
m_positionError = GetContinuousError(m_setpoint - measurement);
if (m_continuous) {
m_positionError = frc::GetModulusError<double>(
m_setpoint, measurement, m_minimumInput, m_maximumInput);
} else {
m_positionError = m_setpoint - measurement;
}
m_velocityError = (m_positionError - m_prevError) / m_period.to<double>();
if (m_Ki != 0) {
@@ -125,29 +128,3 @@ void PIDController::InitSendable(frc::SendableBuilder& builder) {
builder.AddDoubleProperty("setpoint", [this] { return GetSetpoint(); },
[this](double value) { SetSetpoint(value); });
}
double PIDController::GetContinuousError(double error) const {
if (m_continuous && m_inputRange > 0) {
error = std::fmod(error, m_inputRange);
if (std::abs(error) > m_inputRange / 2) {
if (error > 0) {
return error - m_inputRange;
} else {
return error + m_inputRange;
}
}
}
return error;
}
void PIDController::SetInputRange(double minimumInput, double maximumInput) {
m_minimumInput = minimumInput;
m_maximumInput = maximumInput;
m_inputRange = maximumInput - minimumInput;
// Clamp setpoint to new input range
if (m_maximumInput > m_minimumInput) {
m_setpoint = std::clamp(m_setpoint, m_minimumInput, m_maximumInput);
}
}