From dc6f641fd23a05e0d6527e97f57150c8853015ad Mon Sep 17 00:00:00 2001 From: Tommy Beadle Date: Fri, 6 May 2022 11:44:08 -0400 Subject: [PATCH] [wpimath] PIDController: Reset position and velocity error when reset() is called. (#4064) In addition to m_prevError and m_totalError, m_positionError and m_velocityError need to be reset to 0 when reset() is called. Otherwise, the next time calculate() is called, the old values will be used as the previous error, but this is inaccurate since the caller wanted to reset the state of the PID controller. --- .../main/java/edu/wpi/first/math/controller/PIDController.java | 2 ++ wpimath/src/main/native/cpp/controller/PIDController.cpp | 2 ++ 2 files changed, 4 insertions(+) diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java index f0420b7426..49c077dad0 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java @@ -338,8 +338,10 @@ public class PIDController implements Sendable, AutoCloseable { /** Resets the previous error and the integral term. */ public void reset() { + m_positionError = 0; m_prevError = 0; m_totalError = 0; + m_velocityError = 0; } @Override diff --git a/wpimath/src/main/native/cpp/controller/PIDController.cpp b/wpimath/src/main/native/cpp/controller/PIDController.cpp index f5d4801fe6..b18395520e 100644 --- a/wpimath/src/main/native/cpp/controller/PIDController.cpp +++ b/wpimath/src/main/native/cpp/controller/PIDController.cpp @@ -155,8 +155,10 @@ double PIDController::Calculate(double measurement, double setpoint) { } void PIDController::Reset() { + m_positionError = 0; m_prevError = 0; m_totalError = 0; + m_velocityError = 0; } void PIDController::InitSendable(wpi::SendableBuilder& builder) {