Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-01 02:41:48 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] PIDController: Recompute the error in AtSetpoint() (#2822)

Browse Source 
This makes AtSetpoint() return false after the setpoint is changed with
SetSetpoint().

Closes #2821.

Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
This commit is contained in:
Tyler Veness
2020-11-28 14:33:17 -08:00
committed by GitHub
parent 2056f0ce09
commit 3413bfc06a
5 changed files with 45 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -54,8 +54,18 @@ void PIDController::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
double PIDController::GetSetpoint() const { return m_setpoint; }
bool PIDController::AtSetpoint() const {
return std::abs(m_positionError) < m_positionTolerance &&
std::abs(m_velocityError) < m_velocityTolerance;
double positionError;
if (m_continuous) {
positionError = frc::GetModulusError<double>(
m_setpoint, m_measurement, m_minimumInput, m_maximumInput);
} else {
positionError = m_setpoint - m_measurement;
}
double velocityError = (positionError - m_prevError) / m_period.to<double>();
return std::abs(positionError) < m_positionTolerance &&
std::abs(velocityError) < m_velocityTolerance;
}
void PIDController::EnableContinuousInput(double minimumInput,
@@ -86,6 +96,7 @@ double PIDController::GetPositionError() const { return m_positionError; }
double PIDController::GetVelocityError() const { return m_velocityError; }
double PIDController::Calculate(double measurement) {
m_measurement = measurement;
m_prevError = m_positionError;
if (m_continuous) {

1
wpilibc/src/main/native/include/frc/controller/PIDController.h
View File

@@ -238,6 +238,7 @@ class PIDController : public frc::Sendable,
double m_velocityTolerance = std::numeric_limits<double>::infinity();
double m_setpoint = 0;
double m_measurement = 0;
};
} // namespace frc2

10
wpilibc/src/test/native/cpp/controller/PIDToleranceTest.cpp
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2014-2019 FIRST. All Rights Reserved. */
/* Copyright (c) 2014-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -23,9 +23,17 @@ TEST(PIDToleranceTest, AbsoluteTolerance) {
frc2::PIDController controller{0.5, 0.0, 0.0};
controller.EnableContinuousInput(-kRange / 2, kRange / 2);
EXPECT_TRUE(controller.AtSetpoint())
<< "Error was not in tolerance when it should have been. Error was "
<< controller.GetPositionError();
controller.SetTolerance(kTolerance);
controller.SetSetpoint(kSetpoint);
EXPECT_FALSE(controller.AtSetpoint())
<< "Error was in tolerance when it should not have been. Error was "
<< controller.GetPositionError();
controller.Calculate(0.0);
EXPECT_FALSE(controller.AtSetpoint())