Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] PIDController: Add IZone (#5315)

Browse Source 
Co-authored-by: Ryan Blue <ryanzblue@gmail.com>
This commit is contained in:
Gold856
2023-06-20 02:01:01 -04:00
committed by GitHub
parent f5b0d1484b
commit 991f4b0f62
7 changed files with 158 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
wpimath/src/main/native/cpp/controller/PIDController.cpp
View File

@@ -52,6 +52,14 @@ void PIDController::SetD(double Kd) {
m_Kd = Kd;
}
void PIDController::SetIZone(double iZone) {
if (iZone < 0) {
wpi::math::MathSharedStore::ReportError(
"IZone must be a non-negative number, got {}!", iZone);
}
m_iZone = iZone;
}
double PIDController::GetP() const {
return m_Kp;
}
@@ -64,6 +72,10 @@ double PIDController::GetD() const {
return m_Kd;
}
double PIDController::GetIZone() const {
return m_iZone;
}
units::second_t PIDController::GetPeriod() const {
return m_period;
}
@@ -151,7 +163,11 @@ double PIDController::Calculate(double measurement) {
m_velocityError = (m_positionError - m_prevError) / m_period.value();
if (m_Ki != 0) {
// If the absolute value of the position error is outside of IZone, reset the
// total error
if (std::abs(m_positionError) > m_iZone) {
m_totalError = 0;
} else if (m_Ki != 0) {
m_totalError =
std::clamp(m_totalError + m_positionError * m_period.value(),
m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki);
@@ -182,6 +198,9 @@ void PIDController::InitSendable(wpi::SendableBuilder& builder) {
"i", [this] { return GetI(); }, [this](double value) { SetI(value); });
builder.AddDoubleProperty(
"d", [this] { return GetD(); }, [this](double value) { SetD(value); });
builder.AddDoubleProperty(
"izone", [this] { return GetIZone(); },
[this](double value) { SetIZone(value); });
builder.AddDoubleProperty(
"setpoint", [this] { return GetSetpoint(); },
[this](double value) { SetSetpoint(value); });