Control Library by altb
Dependents: My_Libraries IndNav_QK3_T265
DT1_Cntrl.cpp@7:cb1492f4f2c6, 2019-09-18 (annotated)
- Committer:
- altb2
- Date:
- Wed Sep 18 09:56:35 2019 +0000
- Revision:
- 7:cb1492f4f2c6
- Child:
- 8:3a8d1218022c
Addet DT1 Controler
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 | 7:cb1492f4f2c6 | 31 | this->p = 1.0 - 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 | 7:cb1492f4f2c6 | 47 | return Dpart; |
altb2 | 7:cb1492f4f2c6 | 48 | } |
altb2 | 7:cb1492f4f2c6 | 49 | |
altb2 | 7:cb1492f4f2c6 | 50 | void DT1_Cntrl::set_limits(float ll, float ul) |
altb2 | 7:cb1492f4f2c6 | 51 | { |
altb2 | 7:cb1492f4f2c6 | 52 | this->uMin = ll; |
altb2 | 7:cb1492f4f2c6 | 53 | this->uMax = ul; |
altb2 | 7:cb1492f4f2c6 | 54 | } |
altb2 | 7:cb1492f4f2c6 | 55 |