.
Fork of Cntrlol_Lib by
Diff: PID_Cntrl.cpp
- Revision:
- 1:bf62e74fbcf3
- Parent:
- 0:e2a7d7f91e49
- Child:
- 2:1f8ddc46c578
diff -r e2a7d7f91e49 -r bf62e74fbcf3 PID_Cntrl.cpp --- a/PID_Cntrl.cpp Fri Sep 28 08:34:20 2018 +0000 +++ b/PID_Cntrl.cpp Fri Sep 28 09:01:42 2018 +0000 @@ -3,59 +3,47 @@ see e.g.: https://www.dsprelated.com/freebooks/filters/Four_Direct_Forms.html everything is calculated in double - Tn*s + 1 - G(s) = Kp ------------- with s ~ (1 - z^-1)/Ts - Ts*s + Ts z - 1 + G(s) = P + I ------- + D ------- + z - 1 z - p */ -#include "PI_Cntrl.h" +#include "PID_Cntrl.h" using namespace std; -PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts) -{ - setCoefficients(Kp, Tn, Ts); - uMax = 10000000000.0; - uMin = -uMax; - reset(0.0f); -} - -PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax) +PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMax, float uMin) { - setCoefficients(Kp, Tn, Ts); - this->uMax = (double)uMax; - uMin = -(double)uMax; - reset(0.0f); -} - -PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax, float uMin) -{ - setCoefficients(Kp, Tn, Ts); + setCoefficients(P, I, D, tau_f, Ts); this->uMax = (double)uMax; this->uMin = (double)uMin; reset(0.0f); } -PI_Cntrl::~PI_Cntrl() {} +PID_Cntrl::~PID_Cntrl() {} -void PI_Cntrl::reset(float initValue) +void PID_Cntrl::reset(float initValue) { - s = (double)initValue; + Iold = (double)initValue; + eold = 0.0;yold = 0.0; + del = 0.0; } -void PI_Cntrl::setCoefficients(float Kp, float Tn, float Ts) +void PID_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float Ts) { - b0 = (double)Kp*(1.0 + (double)Ts/(double)Tn); - b1 = -(double)Kp; - b2 = (double)Ts/(double)Tn; + this->p = 1.0 - (double)Ts/(double)tau_f; + this->P = P; + this->I = I; + this->D = D; + } -float PI_Cntrl::doStep(double e) +float PID_Cntrl::doStep(double e) { - double u = b0*e + s; // unconstrained output + double u = P*e + D*(e-eold)+p*yold +Iold+I*Ts*(e-del); // unconstrained output double uc = u; // constrained output if(u > uMax) uc = uMax; else if(u < uMin) uc = uMin; - s = b1*e + u - b2*(u - uc); + del=(u-uc)/P; return (float)uc; } \ No newline at end of file