succes
PID.cpp@0:c15430f1895f, 2017-02-11 (annotated)
- Committer:
- mk1
- Date:
- Sat Feb 11 15:55:34 2017 +0000
- Revision:
- 0:c15430f1895f
succes
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mk1 | 0:c15430f1895f | 1 | // Nicked this code from https://nicisdigital.wordpress.com/2011/06/27/proportional-integral-derivative-pid-controller/ |
mk1 | 0:c15430f1895f | 2 | |
mk1 | 0:c15430f1895f | 3 | #include "PID.h" |
mk1 | 0:c15430f1895f | 4 | |
mk1 | 0:c15430f1895f | 5 | void pid_zeroize(PID* pid) { |
mk1 | 0:c15430f1895f | 6 | // set prev and integrated error to zero |
mk1 | 0:c15430f1895f | 7 | pid->prev_error = 0; |
mk1 | 0:c15430f1895f | 8 | pid->int_error = 0; |
mk1 | 0:c15430f1895f | 9 | } |
mk1 | 0:c15430f1895f | 10 | |
mk1 | 0:c15430f1895f | 11 | void pid_update(PID* pid, double curr_error, double dt) |
mk1 | 0:c15430f1895f | 12 | { |
mk1 | 0:c15430f1895f | 13 | double diff; |
mk1 | 0:c15430f1895f | 14 | double p_term; |
mk1 | 0:c15430f1895f | 15 | double i_term; |
mk1 | 0:c15430f1895f | 16 | double d_term; |
mk1 | 0:c15430f1895f | 17 | |
mk1 | 0:c15430f1895f | 18 | // integration with windup guarding |
mk1 | 0:c15430f1895f | 19 | pid->int_error += (curr_error * dt); |
mk1 | 0:c15430f1895f | 20 | if (pid->int_error < -(pid->windup_guard)) |
mk1 | 0:c15430f1895f | 21 | pid->int_error = -(pid->windup_guard); |
mk1 | 0:c15430f1895f | 22 | else if (pid->int_error > pid->windup_guard) |
mk1 | 0:c15430f1895f | 23 | pid->int_error = pid->windup_guard; |
mk1 | 0:c15430f1895f | 24 | |
mk1 | 0:c15430f1895f | 25 | // differentiation |
mk1 | 0:c15430f1895f | 26 | diff = ((curr_error - pid->prev_error) / dt); |
mk1 | 0:c15430f1895f | 27 | |
mk1 | 0:c15430f1895f | 28 | // scaling |
mk1 | 0:c15430f1895f | 29 | p_term = (pid->proportional_gain * curr_error); |
mk1 | 0:c15430f1895f | 30 | i_term = (pid->integral_gain * pid->int_error); |
mk1 | 0:c15430f1895f | 31 | d_term = (pid->derivative_gain * diff); |
mk1 | 0:c15430f1895f | 32 | |
mk1 | 0:c15430f1895f | 33 | // summation of terms |
mk1 | 0:c15430f1895f | 34 | pid->control = p_term + i_term + d_term; |
mk1 | 0:c15430f1895f | 35 | |
mk1 | 0:c15430f1895f | 36 | // save current error as previous error for next iteration |
mk1 | 0:c15430f1895f | 37 | pid->prev_error = curr_error; |
mk1 | 0:c15430f1895f | 38 | } |