[wombat/utils] fix pid handling of negative numbers (CurtinFRC#122)

* merged with silly stuff

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

* Revert "merged with silly stuff"

This reverts commit d0cc0b0.

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;
   }