From f0e3bb51642acae7e7a002c0072538dd94743432 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 14 Jul 2015 17:01:42 -0700 Subject: [PATCH] artf4165: D term of PID controller now uses change in input instead of change in error This avoids large additions introduced by the D term when a step change occurs in the setpoint. Otherwise, the changes return the same values. Let error = setpoint - input and prevError = prevSetpoint - prevInput. If the D term is calculated via error - prevError, then: error - prevError = (setpoint - input) - (prevSetpoint - prevInput) If we ignore the setpoint changing, then we get: error - prevError = (setpoint - input) - (setpoint - prevInput) = prevInput - input Change-Id: Ifa4af9b265e3c4bd263e8541355f2b80269693e9 --- wpilibc/wpilibC++/include/PIDController.h | 2 +- wpilibc/wpilibC++Devices/src/PIDController.cpp | 6 +++--- wpilibc/wpilibC++Sim/src/PIDController.cpp | 8 ++++---- .../main/java/edu/wpi/first/wpilibj/PIDController.java | 8 ++++---- 4 files changed, 12 insertions(+), 12 deletions(-) diff --git a/wpilibc/wpilibC++/include/PIDController.h b/wpilibc/wpilibC++/include/PIDController.h index 7ff0b5c63f..e5ab1ffe49 100644 --- a/wpilibc/wpilibC++/include/PIDController.h +++ b/wpilibc/wpilibC++/include/PIDController.h @@ -79,7 +79,7 @@ class PIDController : public LiveWindowSendable, bool m_continuous = false; // do the endpoints wrap around? eg. Absolute encoder bool m_enabled = false; // is the pid controller enabled bool m_destruct; // should the calculate thread stop running - float m_prevError = 0; // the prior sensor input (used to compute velocity) + float m_prevInput = 0; // the prior sensor input (used to compute velocity) double m_totalError = 0; // the sum of the errors for use in the integral calc enum { kAbsoluteTolerance, diff --git a/wpilibc/wpilibC++Devices/src/PIDController.cpp b/wpilibc/wpilibC++Devices/src/PIDController.cpp index 434a8e22a6..e80eacd59e 100644 --- a/wpilibc/wpilibC++Devices/src/PIDController.cpp +++ b/wpilibc/wpilibC++Devices/src/PIDController.cpp @@ -140,8 +140,8 @@ void PIDController::Calculate() { } m_result = m_P * m_error + m_I * m_totalError + - m_D * (m_error - m_prevError) + m_setpoint * m_F; - m_prevError = m_error; + m_D * (m_prevInput - input) + m_setpoint * m_F; + m_prevInput = input; if (m_result > m_maximumOutput) m_result = m_maximumOutput; @@ -447,7 +447,7 @@ void PIDController::Reset() { Disable(); std::unique_lock sync(m_mutex); - m_prevError = 0; + m_prevInput = 0; m_totalError = 0; m_result = 0; } diff --git a/wpilibc/wpilibC++Sim/src/PIDController.cpp b/wpilibc/wpilibC++Sim/src/PIDController.cpp index 1a6f025f4c..a180a835e5 100644 --- a/wpilibc/wpilibC++Sim/src/PIDController.cpp +++ b/wpilibc/wpilibC++Sim/src/PIDController.cpp @@ -74,7 +74,7 @@ void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf, m_enabled = false; m_setpoint = 0; - m_prevError = 0; + m_prevInput = 0; m_totalError = 0; m_tolerance = .05; @@ -164,8 +164,8 @@ void PIDController::Calculate() } } - m_result = m_P * m_error + m_I * m_totalError + m_D * (m_error - m_prevError) + m_setpoint * m_F; - m_prevError = m_error; + m_result = m_P * m_error + m_I * m_totalError + m_D * (m_prevInput - input) + m_setpoint * m_F; + m_prevInput = input; if (m_result > m_maximumOutput) m_result = m_maximumOutput; else if (m_result < m_minimumOutput) m_result = m_minimumOutput; @@ -482,7 +482,7 @@ void PIDController::Reset() Disable(); std::unique_lock lock(m_mutex); - m_prevError = 0; + m_prevInput = 0; m_totalError = 0; m_result = 0; } diff --git a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDController.java b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDController.java index aa669fa4a4..139c4d26c0 100644 --- a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDController.java +++ b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDController.java @@ -33,7 +33,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll private boolean m_continuous = false; // do the endpoints wrap around? eg. // Absolute encoder private boolean m_enabled = false; // is the pid controller enabled - private double m_prevError = 0.0; // the prior sensor input (used to compute + private double m_prevInput = 0.0; // the prior sensor input (used to compute // velocity) private double m_totalError = 0.0; // the sum of the errors for use in the // integral calc @@ -270,8 +270,8 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll } m_result = - m_P * m_error + m_I * m_totalError + m_D * (m_error - m_prevError) + m_setpoint * m_F; - m_prevError = m_error; + m_P * m_error + m_I * m_totalError + m_D * (m_prevInput - input) + m_setpoint * m_F; + m_prevInput = input; if (m_result > m_maximumOutput) { m_result = m_maximumOutput; @@ -572,7 +572,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll */ public synchronized void reset() { disable(); - m_prevError = 0; + m_prevInput = 0; m_totalError = 0; m_result = 0; }