succes
PID.cpp
- Committer:
- mk1
- Date:
- 2017-02-11
- Revision:
- 0:c15430f1895f
File content as of revision 0:c15430f1895f:
// 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; }