2nd Library
Dependents: cuboid_balance_ros cuboid_balance mirror_actuator_V1
PID_Cntrl.cpp@3:e3b84ea63c0f, 2021-04-01 (annotated)
- Committer:
- altb2
- Date:
- Thu Apr 01 13:38:43 2021 +0000
- Revision:
- 3:e3b84ea63c0f
- Parent:
- 0:a201a6cd4c0c
PIDT2 added
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb2 | 0:a201a6cd4c0c | 1 | /* |
altb2 | 0:a201a6cd4c0c | 2 | PI Controller class with anti windup reset in biquad transposed direct form 2 |
altb2 | 0:a201a6cd4c0c | 3 | see e.g.: https://www.dsprelated.com/freebooks/filters/Four_Direct_Forms.html |
altb2 | 0:a201a6cd4c0c | 4 | everything is calculated in double |
altb2 | 0:a201a6cd4c0c | 5 | |
altb2 | 0:a201a6cd4c0c | 6 | Ts z - 1 |
altb2 | 0:a201a6cd4c0c | 7 | G(s) = P + I ------- + D ------- D corresponds Kd/Tf in Matlab-formlism pid(...) |
altb2 | 0:a201a6cd4c0c | 8 | z - 1 z - p |
altb2 | 0:a201a6cd4c0c | 9 | */ |
altb2 | 0:a201a6cd4c0c | 10 | |
altb2 | 0:a201a6cd4c0c | 11 | #include "PID_Cntrl.h" |
altb2 | 0:a201a6cd4c0c | 12 | using namespace std; |
altb2 | 0:a201a6cd4c0c | 13 | |
altb2 | 0:a201a6cd4c0c | 14 | PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMin, float uMax) |
altb2 | 0:a201a6cd4c0c | 15 | { |
altb2 | 0:a201a6cd4c0c | 16 | setCoefficients(P, I, D, tau_f, Ts); |
altb2 | 0:a201a6cd4c0c | 17 | this->uMin = (double)uMin; |
altb2 | 0:a201a6cd4c0c | 18 | this->uMax = (double)uMax; |
altb2 | 0:a201a6cd4c0c | 19 | reset(0.0f); |
altb2 | 0:a201a6cd4c0c | 20 | } |
altb2 | 0:a201a6cd4c0c | 21 | |
altb2 | 0:a201a6cd4c0c | 22 | PID_Cntrl::~PID_Cntrl() {} |
altb2 | 0:a201a6cd4c0c | 23 | |
altb2 | 0:a201a6cd4c0c | 24 | void PID_Cntrl::reset(float initValue) |
altb2 | 0:a201a6cd4c0c | 25 | { |
altb2 | 0:a201a6cd4c0c | 26 | Iold = (double)initValue; |
altb2 | 0:a201a6cd4c0c | 27 | eold = 0.0;yold = 0.0; |
altb2 | 0:a201a6cd4c0c | 28 | del = 0.0; |
altb2 | 0:a201a6cd4c0c | 29 | } |
altb2 | 0:a201a6cd4c0c | 30 | |
altb2 | 0:a201a6cd4c0c | 31 | void PID_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float Ts) |
altb2 | 0:a201a6cd4c0c | 32 | { |
altb2 | 0:a201a6cd4c0c | 33 | this->p = 1.0 - (double)Ts/(double)tau_f; |
altb2 | 0:a201a6cd4c0c | 34 | this->P = P; |
altb2 | 0:a201a6cd4c0c | 35 | this->I = I; |
altb2 | 0:a201a6cd4c0c | 36 | this->D = D; |
altb2 | 0:a201a6cd4c0c | 37 | this->Ts = Ts; |
altb2 | 0:a201a6cd4c0c | 38 | if(P!=0) |
altb2 | 0:a201a6cd4c0c | 39 | this->Ka=1/P; |
altb2 | 0:a201a6cd4c0c | 40 | else |
altb2 | 0:a201a6cd4c0c | 41 | this->Ka=1.0f; |
altb2 | 0:a201a6cd4c0c | 42 | |
altb2 | 0:a201a6cd4c0c | 43 | } |
altb2 | 0:a201a6cd4c0c | 44 | |
altb2 | 0:a201a6cd4c0c | 45 | float PID_Cntrl::doStep(double e) |
altb2 | 0:a201a6cd4c0c | 46 | { |
altb2 | 0:a201a6cd4c0c | 47 | double Ipart = Iold+I*Ts*(e-del); |
altb2 | 0:a201a6cd4c0c | 48 | double Dpart = D*(e-eold)+p*yold; |
altb2 | 0:a201a6cd4c0c | 49 | double u = P*e + Dpart + Ipart; // unconstrained output |
altb2 | 0:a201a6cd4c0c | 50 | double uc = u; // constrained output |
altb2 | 0:a201a6cd4c0c | 51 | if(u > uMax) uc = uMax; |
altb2 | 0:a201a6cd4c0c | 52 | else if(u < uMin) uc = uMin; |
altb2 | 0:a201a6cd4c0c | 53 | del=(u-uc)*Ka; |
altb2 | 0:a201a6cd4c0c | 54 | eold=e; |
altb2 | 0:a201a6cd4c0c | 55 | Iold=Ipart; |
altb2 | 0:a201a6cd4c0c | 56 | yold=Dpart; |
altb2 | 0:a201a6cd4c0c | 57 | return (float)uc; |
altb2 | 0:a201a6cd4c0c | 58 | } |
altb2 | 0:a201a6cd4c0c | 59 | |
altb2 | 0:a201a6cd4c0c | 60 | void PID_Cntrl::set_limits(double ll, double ul) |
altb2 | 0:a201a6cd4c0c | 61 | { |
altb2 | 0:a201a6cd4c0c | 62 | this->uMin = (double)ll; |
altb2 | 0:a201a6cd4c0c | 63 | this->uMax = (double)ul; |
altb2 | 0:a201a6cd4c0c | 64 | } |
altb2 | 0:a201a6cd4c0c | 65 | |
altb2 | 0:a201a6cd4c0c | 66 | float PID_Cntrl::get_ulimit(void) |
altb2 | 0:a201a6cd4c0c | 67 | { |
altb2 | 0:a201a6cd4c0c | 68 | return (float)this->uMax; |
altb2 | 0:a201a6cd4c0c | 69 | } |