.
Fork of Cntrlol_Lib by
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Wed Jul 20 2022 13:09:12 by 1.7.2