.
Fork of Cntrlol_Lib by
PID_Cntrl.cpp@1:bf62e74fbcf3, 2018-09-28 (annotated)
- Committer:
- altb
- Date:
- Fri Sep 28 09:01:42 2018 +0000
- Revision:
- 1:bf62e74fbcf3
- Parent:
- 0:e2a7d7f91e49
- Child:
- 2:1f8ddc46c578
.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb | 0:e2a7d7f91e49 | 1 | /* |
altb | 0:e2a7d7f91e49 | 2 | PI Controller class with anti windup reset in biquad transposed direct form 2 |
altb | 0:e2a7d7f91e49 | 3 | see e.g.: https://www.dsprelated.com/freebooks/filters/Four_Direct_Forms.html |
altb | 0:e2a7d7f91e49 | 4 | everything is calculated in double |
altb | 0:e2a7d7f91e49 | 5 | |
altb | 1:bf62e74fbcf3 | 6 | Ts z - 1 |
altb | 1:bf62e74fbcf3 | 7 | G(s) = P + I ------- + D ------- |
altb | 1:bf62e74fbcf3 | 8 | z - 1 z - p |
altb | 0:e2a7d7f91e49 | 9 | */ |
altb | 0:e2a7d7f91e49 | 10 | |
altb | 1:bf62e74fbcf3 | 11 | #include "PID_Cntrl.h" |
altb | 0:e2a7d7f91e49 | 12 | |
altb | 0:e2a7d7f91e49 | 13 | using namespace std; |
altb | 0:e2a7d7f91e49 | 14 | |
altb | 1:bf62e74fbcf3 | 15 | PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMax, float uMin) |
altb | 0:e2a7d7f91e49 | 16 | { |
altb | 1:bf62e74fbcf3 | 17 | setCoefficients(P, I, D, tau_f, Ts); |
altb | 0:e2a7d7f91e49 | 18 | this->uMax = (double)uMax; |
altb | 0:e2a7d7f91e49 | 19 | this->uMin = (double)uMin; |
altb | 0:e2a7d7f91e49 | 20 | reset(0.0f); |
altb | 0:e2a7d7f91e49 | 21 | } |
altb | 0:e2a7d7f91e49 | 22 | |
altb | 1:bf62e74fbcf3 | 23 | PID_Cntrl::~PID_Cntrl() {} |
altb | 0:e2a7d7f91e49 | 24 | |
altb | 1:bf62e74fbcf3 | 25 | void PID_Cntrl::reset(float initValue) |
altb | 0:e2a7d7f91e49 | 26 | { |
altb | 1:bf62e74fbcf3 | 27 | Iold = (double)initValue; |
altb | 1:bf62e74fbcf3 | 28 | eold = 0.0;yold = 0.0; |
altb | 1:bf62e74fbcf3 | 29 | del = 0.0; |
altb | 0:e2a7d7f91e49 | 30 | } |
altb | 0:e2a7d7f91e49 | 31 | |
altb | 1:bf62e74fbcf3 | 32 | void PID_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float Ts) |
altb | 0:e2a7d7f91e49 | 33 | { |
altb | 1:bf62e74fbcf3 | 34 | this->p = 1.0 - (double)Ts/(double)tau_f; |
altb | 1:bf62e74fbcf3 | 35 | this->P = P; |
altb | 1:bf62e74fbcf3 | 36 | this->I = I; |
altb | 1:bf62e74fbcf3 | 37 | this->D = D; |
altb | 1:bf62e74fbcf3 | 38 | |
altb | 0:e2a7d7f91e49 | 39 | } |
altb | 0:e2a7d7f91e49 | 40 | |
altb | 1:bf62e74fbcf3 | 41 | float PID_Cntrl::doStep(double e) |
altb | 0:e2a7d7f91e49 | 42 | { |
altb | 1:bf62e74fbcf3 | 43 | double u = P*e + D*(e-eold)+p*yold +Iold+I*Ts*(e-del); // unconstrained output |
altb | 0:e2a7d7f91e49 | 44 | double uc = u; // constrained output |
altb | 0:e2a7d7f91e49 | 45 | if(u > uMax) uc = uMax; |
altb | 0:e2a7d7f91e49 | 46 | else if(u < uMin) uc = uMin; |
altb | 1:bf62e74fbcf3 | 47 | del=(u-uc)/P; |
altb | 0:e2a7d7f91e49 | 48 | return (float)uc; |
altb | 0:e2a7d7f91e49 | 49 | } |