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