altb_pmic
Control Library by altb
Dependents: My_Libraries IndNav_QK3_T265
DT1_Cntrl.cpp@8:3a8d1218022c, 2019-09-19 (annotated)
- Committer:
- altb2
- Date:
- Thu Sep 19 08:51:03 2019 +0000
- Revision:
- 8:3a8d1218022c
- Parent:
- 7:cb1492f4f2c6
- Child:
- 9:074f4f94b584
corrected dostep in DT1 cntrl
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| altb2 | 7:cb1492f4f2c6 | 1 | /* |
| altb2 | 7:cb1492f4f2c6 | 2 | DT1 Controller class |
| altb2 | 7:cb1492f4f2c6 | 3 | everything is calculated in float |
| altb2 | 7:cb1492f4f2c6 | 4 | |
| altb2 | 7:cb1492f4f2c6 | 5 | z - 1 |
| altb2 | 7:cb1492f4f2c6 | 6 | G(s) = Kd/tau ------- D corresponds Kd in Matlab-formlism pid(...) |
| altb2 | 7:cb1492f4f2c6 | 7 | z - p |
| altb2 | 7:cb1492f4f2c6 | 8 | */ |
| altb2 | 7:cb1492f4f2c6 | 9 | |
| altb2 | 7:cb1492f4f2c6 | 10 | #include "DT1_Cntrl.h" |
| altb2 | 7:cb1492f4f2c6 | 11 | using namespace std; |
| altb2 | 7:cb1492f4f2c6 | 12 | |
| altb2 | 7:cb1492f4f2c6 | 13 | DT1_Cntrl::DT1_Cntrl(float D, float tau_f, float Ts, float uMin, float uMax) |
| altb2 | 7:cb1492f4f2c6 | 14 | { |
| altb2 | 7:cb1492f4f2c6 | 15 | setCoefficients(D, tau_f, Ts); |
| altb2 | 7:cb1492f4f2c6 | 16 | this->uMin = uMin; |
| altb2 | 7:cb1492f4f2c6 | 17 | this->uMax = uMax; |
| altb2 | 7:cb1492f4f2c6 | 18 | reset(0.0f); |
| altb2 | 7:cb1492f4f2c6 | 19 | } |
| altb2 | 7:cb1492f4f2c6 | 20 | |
| altb2 | 7:cb1492f4f2c6 | 21 | DT1_Cntrl::~DT1_Cntrl() {} |
| altb2 | 7:cb1492f4f2c6 | 22 | |
| altb2 | 7:cb1492f4f2c6 | 23 | void DT1_Cntrl::reset(float initValue) |
| altb2 | 7:cb1492f4f2c6 | 24 | { |
| altb2 | 7:cb1492f4f2c6 | 25 | eold = initValue; |
| altb2 | 7:cb1492f4f2c6 | 26 | yold = 0.0; |
| altb2 | 7:cb1492f4f2c6 | 27 | } |
| altb2 | 7:cb1492f4f2c6 | 28 | |
| altb2 | 7:cb1492f4f2c6 | 29 | void DT1_Cntrl::setCoefficients(float D, float tau_f, float Ts) |
| altb2 | 7:cb1492f4f2c6 | 30 | { |
| altb2 | 8:3a8d1218022c | 31 | this->p = 1.0f - Ts/tau_f; |
| altb2 | 7:cb1492f4f2c6 | 32 | this->tau_f = tau_f; |
| altb2 | 7:cb1492f4f2c6 | 33 | this->D_ = D/tau_f; // modified D, now D is consistent with Matlab PID |
| altb2 | 7:cb1492f4f2c6 | 34 | this->D__init = D/tau_f; // modified D, now D is consistent with Matlab PID |
| altb2 | 7:cb1492f4f2c6 | 35 | this->Ts = Ts; |
| altb2 | 7:cb1492f4f2c6 | 36 | } |
| altb2 | 7:cb1492f4f2c6 | 37 | void DT1_Cntrl::setCoeff_D(float D) |
| altb2 | 7:cb1492f4f2c6 | 38 | { |
| altb2 | 7:cb1492f4f2c6 | 39 | this->D_ = D/this->tau_f; |
| altb2 | 7:cb1492f4f2c6 | 40 | } |
| altb2 | 7:cb1492f4f2c6 | 41 | |
| altb2 | 7:cb1492f4f2c6 | 42 | float DT1_Cntrl::doStep(float e) |
| altb2 | 7:cb1492f4f2c6 | 43 | { |
| altb2 | 7:cb1492f4f2c6 | 44 | float Dpart = D_*(e-eold)+p*yold; |
| altb2 | 7:cb1492f4f2c6 | 45 | if(Dpart > uMax) Dpart = uMax; |
| altb2 | 7:cb1492f4f2c6 | 46 | else if(Dpart < uMin) Dpart = uMin; |
| altb2 | 8:3a8d1218022c | 47 | eold=e; |
| altb2 | 8:3a8d1218022c | 48 | yold=Dpart; |
| altb2 | 7:cb1492f4f2c6 | 49 | return Dpart; |
| altb2 | 7:cb1492f4f2c6 | 50 | } |
| altb2 | 7:cb1492f4f2c6 | 51 | |
| altb2 | 7:cb1492f4f2c6 | 52 | void DT1_Cntrl::set_limits(float ll, float ul) |
| altb2 | 7:cb1492f4f2c6 | 53 | { |
| altb2 | 7:cb1492f4f2c6 | 54 | this->uMin = ll; |
| altb2 | 7:cb1492f4f2c6 | 55 | this->uMax = ul; |
| altb2 | 7:cb1492f4f2c6 | 56 | } |
| altb2 | 7:cb1492f4f2c6 | 57 |