Control Library by altb
Dependents: My_Libraries IndNav_QK3_T265
PID_Cntrl.cpp
- Committer:
- altb2
- Date:
- 2019-07-05
- Revision:
- 3:27595dd9e433
- Parent:
- 2:e7c9e50a2e46
- Child:
- 5:d8c53cece01b
File content as of revision 3:27595dd9e433:
/* 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 float Ts z - 1 G(s) = P + I ------- + D/tau ------- D corresponds Kd 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 = uMin; this->uMax = uMax; reset(0.0f); } PID_Cntrl::~PID_Cntrl() {} void PID_Cntrl::reset(float initValue) { Iold = 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 - Ts/tau_f; this->P = P; this->I = I; this->tau_f = tau_f; this->D_ = D/tau_f; // modified D, now D is consistent with Matlab PID this->Ts = Ts; if(P!=0) this->Ka=1/P; else this->Ka=1.0f; } void PID_Cntrl::setCoeff_P(float P) { this->P = P; } void PID_Cntrl::setCoeff_I(float I) { this->I = I; } void PID_Cntrl::setCoeff_D(float D) { this->D_ = D/this->tau_f; } float PID_Cntrl::doStep(float e) { float Ipart = Iold+I*Ts*(e-del); float Dpart = D_*(e-eold)+p*yold; float u = P*e + Dpart + Ipart; // unconstrained output float 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 uc; } void PID_Cntrl::set_limits(float ll, float ul) { this->uMin = ll; this->uMax = ul; } float PID_Cntrl::get_ulimit(void) { return this->uMax; }