Ruprecht Altenburger
/
mirror_actuator_preston
corrections altb2
Lib_Cntrl/PID_Cntrl.cpp@3:8aa09988fa58, 2021-08-24 (annotated)
- Committer:
- altb2
- Date:
- Tue Aug 24 12:34:00 2021 +0000
- Revision:
- 3:8aa09988fa58
- Parent:
- 1:25a2b47ca291
corrected memory allocation in class
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb2 | 0:d2e117716219 | 1 | /* |
altb2 | 0:d2e117716219 | 2 | */ |
altb2 | 0:d2e117716219 | 3 | |
altb2 | 0:d2e117716219 | 4 | #include "PID_Cntrl.h" |
altb2 | 0:d2e117716219 | 5 | |
altb2 | 0:d2e117716219 | 6 | PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMin, float uMax) |
altb2 | 0:d2e117716219 | 7 | { |
ernstpre | 1:25a2b47ca291 | 8 | this->P = P; |
ernstpre | 1:25a2b47ca291 | 9 | this->I = I; |
ernstpre | 1:25a2b47ca291 | 10 | this->D = D; |
ernstpre | 1:25a2b47ca291 | 11 | this->tau_f = tau_f; |
ernstpre | 1:25a2b47ca291 | 12 | this->Ts = Ts; |
ernstpre | 1:25a2b47ca291 | 13 | this->uMin = uMin; |
ernstpre | 1:25a2b47ca291 | 14 | this->uMax = uMax; |
ernstpre | 1:25a2b47ca291 | 15 | reset(0); |
ernstpre | 1:25a2b47ca291 | 16 | |
ernstpre | 1:25a2b47ca291 | 17 | |
altb2 | 0:d2e117716219 | 18 | } |
altb2 | 0:d2e117716219 | 19 | |
altb2 | 0:d2e117716219 | 20 | PID_Cntrl::~PID_Cntrl() {} |
altb2 | 0:d2e117716219 | 21 | |
altb2 | 0:d2e117716219 | 22 | void PID_Cntrl::reset(float initValue) |
altb2 | 0:d2e117716219 | 23 | { |
ernstpre | 1:25a2b47ca291 | 24 | Ipart = initValue; |
ernstpre | 1:25a2b47ca291 | 25 | Dpart = 0.0f; |
ernstpre | 1:25a2b47ca291 | 26 | e_old = 0.0f; |
altb2 | 0:d2e117716219 | 27 | } |
altb2 | 0:d2e117716219 | 28 | |
altb2 | 0:d2e117716219 | 29 | |
altb2 | 0:d2e117716219 | 30 | float PID_Cntrl::update(float e) |
altb2 | 0:d2e117716219 | 31 | { |
altb2 | 0:d2e117716219 | 32 | // the main update |
ernstpre | 1:25a2b47ca291 | 33 | float Ppart = P * e; |
ernstpre | 1:25a2b47ca291 | 34 | Ipart += I * Ts*e; |
ernstpre | 1:25a2b47ca291 | 35 | Dpart = Dpart*tau_f/(tau_f + Ts) + D/(tau_f + Ts)*(e - e_old); |
ernstpre | 1:25a2b47ca291 | 36 | Ipart = saturate(Ipart); |
ernstpre | 1:25a2b47ca291 | 37 | return saturate(Ppart + Ipart + Dpart); |
ernstpre | 1:25a2b47ca291 | 38 | } |
ernstpre | 1:25a2b47ca291 | 39 | float PID_Cntrl::saturate(float x) |
ernstpre | 1:25a2b47ca291 | 40 | { |
ernstpre | 1:25a2b47ca291 | 41 | if(x > uMax) |
ernstpre | 1:25a2b47ca291 | 42 | return x = uMax; |
ernstpre | 1:25a2b47ca291 | 43 | else if (x < uMin) |
ernstpre | 1:25a2b47ca291 | 44 | return x = uMin; |
ernstpre | 1:25a2b47ca291 | 45 | return x; |
altb2 | 0:d2e117716219 | 46 | } |