From 1a07aa2edf9ddec9d79a0b7dac78462fcfdb2167 Mon Sep 17 00:00:00 2001
From: Jess Zarchi <jackfz01@gmail.com>
Date: Sat, 1 Jun 2024 18:03:41 -0700
Subject: [PATCH] Fixed derivative kick #99

---
 include/EZ-Template/PID.hpp | 33 +++++++++++++++++++++++----------
 src/EZ-Template/PID.cpp     | 22 +++++++++++++++++++---
 2 files changed, 42 insertions(+), 13 deletions(-)

diff --git a/include/EZ-Template/PID.hpp b/include/EZ-Template/PID.hpp
index b0e6e266..06f485d8 100644
--- a/include/EZ-Template/PID.hpp
+++ b/include/EZ-Template/PID.hpp
@@ -97,10 +97,21 @@ class PID {
    * Computes PID.
    *
    * \param current
-   *        Current sensor library.
+   *        Current sensor value.
    */
   double compute(double current);
 
+  /**
+   * Computes PID, but you set the error yourself.  This function ignores target.
+   * Current is only used here for calculative derivative.
+   *
+   * \param err
+   *        Error in PID, you need to calculate this yourself.
+   * \param current
+   *        Current sensor value.
+   */
+  double compute_error(double err, double current);
+
   /**
    * Returns target value.
    */
@@ -183,15 +194,16 @@ class PID {
   /**
    * PID variables.
    */
-  double output;
-  double cur;
-  double error;
-  double target;
-  double prev_error;
-  double integral;
-  double derivative;
-  long time;
-  long prev_time;
+  double output = 0.0;
+  double cur = 0.0;
+  double error = 0.0;
+  double target = 0.0;
+  double prev_error = 0.0;
+  double prev_current = 0.0;
+  double integral = 0.0;
+  double derivative = 0.0;
+  long time = 0;
+  long prev_time = 0;
 
  private:
   int i = 0, j = 0, k = 0, l = 0;
@@ -201,5 +213,6 @@ class PID {
   bool name_active = false;
   void exit_condition_print(ez::exit_output exit_type);
   bool reset_i_sgn = true;
+  double raw_compute();
 };
 };  // namespace ez
\ No newline at end of file
diff --git a/src/EZ-Template/PID.cpp b/src/EZ-Template/PID.cpp
index 709b73e4..290cc923 100644
--- a/src/EZ-Template/PID.cpp
+++ b/src/EZ-Template/PID.cpp
@@ -59,18 +59,34 @@ bool PID::i_reset_get() { return reset_i_sgn; };
 
 double PID::compute(double current) {
   error = target - current;
-  derivative = error - prev_error;
+  return compute_error(error, current);
+}
+
+double PID::compute_error(double err, double current) {
+  error = err;
+  cur = current;
+
+  return raw_compute();
+}
+
+double PID::raw_compute() {
+  // calculate derivative on measurement instead of error to avoid "derivative kick"
+  // https://www.isa.org/intech-home/2023/june-2023/features/fundamentals-pid-control
+  derivative = cur - prev_current;
 
   if (constants.ki != 0) {
+    // Only compute i when within a threshold of target
     if (fabs(error) < constants.start_i)
       integral += error;
 
-    if (util::sgn(error) != util::sgn(prev_error) && reset_i_sgn)
+    // Reset i when the sign of error flips
+    if (util::sgn(error) != util::sgn(prev_current) && reset_i_sgn)
       integral = 0;
   }
 
-  output = (error * constants.kp) + (integral * constants.ki) + (derivative * constants.kd);
+  output = (error * constants.kp) + (integral * constants.ki) - (derivative * constants.kd);
 
+  prev_current = cur;
   prev_error = error;
 
   return output;