PID.cpp

00001 // Nicked this code from https://nicisdigital.wordpress.com/2011/06/27/proportional-integral-derivative-pid-controller/
00002 
00003 #include "PID.h"
00004 
00005 void pid_zeroize(PID* pid) {
00006     // set prev and integrated error to zero
00007     pid->prev_error = 0;
00008     pid->int_error = 0;
00009 }
00010  
00011 void pid_update(PID* pid, double curr_error, double dt) 
00012 {
00013     double diff;
00014     double p_term;
00015     double i_term;
00016     double d_term;
00017  
00018     // integration with windup guarding
00019     pid->int_error += (curr_error * dt);
00020     if (pid->int_error < -(pid->windup_guard))
00021         pid->int_error = -(pid->windup_guard);
00022     else if (pid->int_error > pid->windup_guard)
00023         pid->int_error = pid->windup_guard;
00024  
00025     // differentiation
00026     diff = ((curr_error - pid->prev_error) / dt);
00027  
00028     // scaling
00029     p_term = (pid->proportional_gain * curr_error);
00030     i_term = (pid->integral_gain     * pid->int_error);
00031     d_term = (pid->derivative_gain   * diff);
00032  
00033     // summation of terms
00034     pid->control = p_term + i_term + d_term;
00035  
00036     // save current error as previous error for next iteration
00037     pid->prev_error = curr_error;
00038 }