PID_Cntrl.cpp

00001 /*  
00002     PI Controller class with anti windup reset in biquad transposed direct form 2
00003     see e.g.: https://www.dsprelated.com/freebooks/filters/Four_Direct_Forms.html
00004     everything is calculated in double
00005     
00006                      Ts         z - 1             
00007       G(s) = P + I ------- + D -------          D corresponds Kd/Tf in Matlab-formlism pid(...)
00008                     z - 1       z - p              
00009 */
00010 
00011 #include "PID_Cntrl.h"
00012 using namespace std;
00013 
00014 PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMin, float uMax)
00015 {
00016     setCoefficients(P, I, D, tau_f, Ts);
00017     this->uMin = (double)uMin;
00018     this->uMax = (double)uMax;
00019     reset(0.0f);
00020 }
00021 
00022 PID_Cntrl::~PID_Cntrl() {}
00023 
00024 void PID_Cntrl::reset(float initValue)
00025 {
00026     Iold = (double)initValue;
00027     eold = 0.0;yold = 0.0;
00028     del = 0.0;
00029 }
00030 
00031 void PID_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float Ts)
00032 {
00033     this->p = 1.0 - (double)Ts/(double)tau_f;
00034     this->P = P;
00035     this->I = I;
00036     this->D = D;
00037     this->Ts = Ts;
00038     if(P!=0)
00039         this->Ka=1/P;
00040     else
00041         this->Ka=1.0f;
00042     
00043 }
00044 
00045 float PID_Cntrl::doStep(double e)
00046 {
00047     double Ipart = Iold+I*Ts*(e-del);
00048     double Dpart = D*(e-eold)+p*yold;
00049     double u = P*e + Dpart  + Ipart;          // unconstrained output
00050     double uc = u;                // constrained output
00051     if(u > uMax) uc = uMax;
00052     else if(u < uMin) uc = uMin;
00053     del=(u-uc)*Ka;
00054     eold=e;
00055     Iold=Ipart;
00056     yold=Dpart;
00057     return (float)uc;
00058 }
00059 
00060 void PID_Cntrl::set_limits(double ll, double ul)
00061 {
00062     this->uMin = (double)ll;
00063     this->uMax = (double)ul;
00064 }
00065 
00066 float PID_Cntrl::get_ulimit(void)
00067 {
00068     return (float)this->uMax;
00069 }