Library for control puposes
Fork of Cntrl_Lib by
Diff: PID_Cntrl.cpp
- Revision:
- 2:1f8ddc46c578
- Parent:
- 1:bf62e74fbcf3
- Child:
- 3:27b75e7dd54a
--- a/PID_Cntrl.cpp Fri Sep 28 09:01:42 2018 +0000 +++ b/PID_Cntrl.cpp Fri Sep 28 12:37:19 2018 +0000 @@ -9,10 +9,9 @@ */ #include "PID_Cntrl.h" - using namespace std; -PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMax, float uMin) +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->uMax = (double)uMax; @@ -35,15 +34,25 @@ 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 u = P*e + D*(e-eold)+p*yold +Iold+I*Ts*(e-del); // unconstrained output + 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)/P; + del=(u-uc)*Ka; + eold=e; + Iold=Ipart; + yold=Dpart; return (float)uc; } \ No newline at end of file