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 float
00005     
00006                      Ts             z - 1             
00007       G(s) = P + I ------- + D/tau -------          D corresponds Kd 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 = uMin;
00018     this->uMax = uMax;
00019     reset(0.0f);
00020 }
00021 
00022 PID_Cntrl::~PID_Cntrl() {}
00023 
00024 void PID_Cntrl::reset(float initValue)
00025 {
00026     Iold = initValue;
00027     eold = 0.0;yold = 0.0;Dpart_old = 0.0;
00028     del = 0.0;
00029 }
00030 
00031 void PID_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float Ts,float uMin, float uMax)
00032 {
00033     setCoefficients(P,I,D,tau_f,Ts);
00034     this->uMin = uMin;
00035     this->uMax = uMax;
00036 
00037 }
00038 void PID_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float Ts)
00039 {
00040     this->p = 1.0f - Ts/tau_f;
00041     this->P = P;
00042     this->P_init = P;
00043     this->I = I;
00044     this->I_init = I;
00045     this->tau_f = tau_f;
00046     this->D_ = D/tau_f;     // modified D, now D is consistent with Matlab PID
00047     this->D__init = D/tau_f;     // modified D, now D is consistent with Matlab PID
00048     this->Ts = Ts;
00049     if(P!=0)
00050         this->Ka=1/P;
00051     else
00052         this->Ka=1.0f;
00053     
00054 }
00055 void PID_Cntrl::setCoeff_P(float P)
00056 {
00057     this->P = P;
00058 }
00059 void PID_Cntrl::setCoeff_I(float I)
00060 {
00061     this->I = I;
00062 }
00063 void PID_Cntrl::setCoeff_D(float D)
00064 {
00065     this->D_ = D/this->tau_f;
00066 }
00067 void PID_Cntrl::scale_PID_param(float sc)
00068 {
00069     this->P = this->P_init * sc;
00070     this->I = this->I_init * sc;
00071     this->D_ = this->D__init * sc;
00072 }
00073 
00074 float PID_Cntrl::doStep(float e)
00075 {
00076     float Ipart = Iold+I*Ts*(e-del);
00077     float Dpart = D_*(e-eold)+p*Dpart_old;
00078     float u = P*e + Dpart  + Ipart;          // unconstrained output
00079     float uc = u;                // constrained output
00080     if(u > uMax) uc = uMax;
00081     else if(u < uMin) uc = uMin;
00082     del=(u-uc)*Ka;
00083     eold=e;
00084     Iold=Ipart;
00085     Dpart_old=Dpart;
00086     return uc;
00087 }
00088 float PID_Cntrl::doStep(float e,float y)
00089 {
00090     float Ipart = Iold+I*Ts*(e-del);
00091     float Dpart = D_ * (y-yold) + p * Dpart_old;
00092     float u = P*e - Dpart  + Ipart;          // unconstrained output
00093     float uc = u;                // constrained output
00094     if(u > uMax) uc = uMax;
00095     else if(u < uMin) uc = uMin;
00096     del=(u-uc)*Ka;
00097     eold=e;
00098     Iold=Ipart;
00099     Dpart_old = Dpart;
00100     yold = y;
00101     return uc;
00102 }
00103 
00104 void PID_Cntrl::set_limits(float ll, float ul)
00105 {
00106     this->uMin = ll;
00107     this->uMax = ul;
00108 }
00109 
00110 float PID_Cntrl::get_ulimit(void)
00111 {
00112     return this->uMax;
00113 }
00114 
00115 float PID_Cntrl::get_P_gain(void)
00116 {
00117     return this->P;
00118 }