From 5e9957b5c9c8d7992b20c202cc4c8b437a42a6e9 Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Sun, 4 Feb 2024 16:30:53 +0800
Subject: [PATCH] [wombat/utils] fix pid handling of negative numbers (#122)

* merged with silly stuff

* [wombat/utils] fix pid handling of negative numbers

* Revert "merged with silly stuff"

This reverts commit d0cc0b03d45fe01173880656c33d1ae1ada74611.
---
 wombat/src/main/include/utils/PID.h | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/wombat/src/main/include/utils/PID.h b/wombat/src/main/include/utils/PID.h
index a0f2eaf9..7d9b9516 100644
--- a/wombat/src/main/include/utils/PID.h
+++ b/wombat/src/main/include/utils/PID.h
@@ -103,6 +103,11 @@ class PIDController {
   void Reset() { _integralSum = sum_t{0}; }
 
   out_t Calculate(in_t pv, units::second_t dt, out_t feedforward = out_t{0}) {
+    bool is_negative;
+    if (pv.value() < 0) {
+      is_negative = true;
+      pv = units::math::fabs(pv);
+    }
     auto error = do_wrap(_setpoint - pv);
     _integralSum += error * dt;
     if (config.izone.value() > 0 && (error > config.izone || error < -config.izone))
@@ -130,6 +135,9 @@ class PIDController {
     _last_pv = pv;
     _last_error = error;
     _iterations++;
+    if (is_negative) {
+      return out * -1;
+    }
     return out;
   }