Marek Korczak / nrf51_microbit

succes

Dependencies:   microbit

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?

UserRevisionLine numberNew 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 }