diff --git a/libraries/PID/PID.cpp b/libraries/PID/PID.cpp index 4499d26900..56dcf60488 100644 --- a/libraries/PID/PID.cpp +++ b/libraries/PID/PID.cpp @@ -88,6 +88,12 @@ float PID::get_pid(float error, float scaler) return output; } +int16_t PID::get_pid_4500(float error, float scaler) +{ + float v = get_pid(error, scaler); + return constrain(v, -4500, 4500); +} + void PID::reset_I() { diff --git a/libraries/PID/PID.h b/libraries/PID/PID.h index aced3e566a..b1890e0cb8 100644 --- a/libraries/PID/PID.h +++ b/libraries/PID/PID.h @@ -42,6 +42,9 @@ public: /// float get_pid(float error, float scaler = 1.0); + // get_pid() constrained to +/- 4500 + int16_t get_pid_4500(float error, float scaler = 1.0); + /// Reset the PID integrator /// void reset_I();