Library for control puposes
Fork of Cntrl_Lib by
PID_Cntrl.cpp
- Committer:
- altb
- Date:
- 2018-10-18
- Revision:
- 3:27b75e7dd54a
- Parent:
- 2:1f8ddc46c578
- Child:
- 4:9cf60fa0a579
File content as of revision 3:27b75e7dd54a:
/* PI Controller class with anti windup reset in biquad transposed direct form 2 see e.g.: https://www.dsprelated.com/freebooks/filters/Four_Direct_Forms.html everything is calculated in double Ts z - 1 G(s) = P + I ------- + D ------- D corresponds Kd/Tf in Matlab-formlism pid(...) z - 1 z - p */ #include "PID_Cntrl.h" using namespace std; PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMin, float uMax) { setCoefficients(P, I, D, tau_f, Ts); this->uMin = (double)uMin; this->uMax = (double)uMax; reset(0.0f); } PID_Cntrl::~PID_Cntrl() {} void PID_Cntrl::reset(float initValue) { Iold = (double)initValue; eold = 0.0;yold = 0.0; del = 0.0; } void PID_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float Ts) { this->p = 1.0 - (double)Ts/(double)tau_f; this->P = P; this->I = I; this->D = D; this->Ts = Ts; if(P!=0) this->Ka=1/P; else this->Ka=1.0f; } float PID_Cntrl::doStep(double e) { double Ipart = Iold+I*Ts*(e-del); double Dpart = D*(e-eold)+p*yold; double u = P*e + Dpart + Ipart; // unconstrained output double uc = u; // constrained output if(u > uMax) uc = uMax; else if(u < uMin) uc = uMin; del=(u-uc)*Ka; eold=e; Iold=Ipart; yold=Dpart; return (float)uc; } void PID_Cntrl::set_limits(double ll, double ul) { this->uMin = (double)ll; this->uMax = (double)ul; } float PID_Cntrl::get_ulimit(void) { return (float)this->uMax; }