succes
Diff: PID.cpp
- Revision:
- 0:c15430f1895f
diff -r 000000000000 -r c15430f1895f PID.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.cpp Sat Feb 11 15:55:34 2017 +0000 @@ -0,0 +1,38 @@ +// Nicked this code from https://nicisdigital.wordpress.com/2011/06/27/proportional-integral-derivative-pid-controller/ + +#include "PID.h" + +void pid_zeroize(PID* pid) { + // set prev and integrated error to zero + pid->prev_error = 0; + pid->int_error = 0; +} + +void pid_update(PID* pid, double curr_error, double dt) +{ + double diff; + double p_term; + double i_term; + double d_term; + + // integration with windup guarding + pid->int_error += (curr_error * dt); + if (pid->int_error < -(pid->windup_guard)) + pid->int_error = -(pid->windup_guard); + else if (pid->int_error > pid->windup_guard) + pid->int_error = pid->windup_guard; + + // differentiation + diff = ((curr_error - pid->prev_error) / dt); + + // scaling + p_term = (pid->proportional_gain * curr_error); + i_term = (pid->integral_gain * pid->int_error); + d_term = (pid->derivative_gain * diff); + + // summation of terms + pid->control = p_term + i_term + d_term; + + // save current error as previous error for next iteration + pid->prev_error = curr_error; +} \ No newline at end of file