Control Library by altb
Dependents: My_Libraries IndNav_QK3_T265
DT1_Cntrl.cpp@15:c70cad2f4e64, 2020-01-16 (annotated)
- Committer:
- pmic
- Date:
- Thu Jan 16 09:12:50 2020 +0000
- Revision:
- 15:c70cad2f4e64
- Parent:
- 9:074f4f94b584
Revisit IIR_filter.h and IIR_filter.cpp. Change internal double to float arithmetic.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pmic | 9:074f4f94b584 | 1 | /* |
pmic | 9:074f4f94b584 | 2 | DT1 Controller class |
altb2 | 7:cb1492f4f2c6 | 3 | everything is calculated in float |
pmic | 9:074f4f94b584 | 4 | |
pmic | 9:074f4f94b584 | 5 | z - 1 |
pmic | 9:074f4f94b584 | 6 | G(s) = Kd/tau ------- , p = 1 - Ts/tau D corresponds Kd in Matlab-formlism pid(...) |
pmic | 9:074f4f94b584 | 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); |
pmic | 9:074f4f94b584 | 16 | set_limits(uMin, uMax); |
altb2 | 7:cb1492f4f2c6 | 17 | reset(0.0f); |
altb2 | 7:cb1492f4f2c6 | 18 | } |
altb2 | 7:cb1492f4f2c6 | 19 | |
altb2 | 7:cb1492f4f2c6 | 20 | DT1_Cntrl::~DT1_Cntrl() {} |
altb2 | 7:cb1492f4f2c6 | 21 | |
pmic | 9:074f4f94b584 | 22 | |
altb2 | 7:cb1492f4f2c6 | 23 | void DT1_Cntrl::reset(float initValue) |
altb2 | 7:cb1492f4f2c6 | 24 | { |
altb2 | 7:cb1492f4f2c6 | 25 | eold = initValue; |
pmic | 9:074f4f94b584 | 26 | yold = 0.0f; |
pmic | 9:074f4f94b584 | 27 | } |
pmic | 9:074f4f94b584 | 28 | |
pmic | 9:074f4f94b584 | 29 | float DT1_Cntrl::doStep(float e) |
pmic | 9:074f4f94b584 | 30 | { |
pmic | 9:074f4f94b584 | 31 | float Dpart = D * ( e - eold ) + p * yold; |
pmic | 9:074f4f94b584 | 32 | if(Dpart > uMax) Dpart = uMax; |
pmic | 9:074f4f94b584 | 33 | else if(Dpart < uMin) Dpart = uMin; |
pmic | 9:074f4f94b584 | 34 | eold = e; |
pmic | 9:074f4f94b584 | 35 | yold = Dpart; |
pmic | 9:074f4f94b584 | 36 | return Dpart; |
pmic | 9:074f4f94b584 | 37 | } |
pmic | 9:074f4f94b584 | 38 | |
pmic | 9:074f4f94b584 | 39 | void DT1_Cntrl::set_limits(float uMin, float uMax) |
pmic | 9:074f4f94b584 | 40 | { |
pmic | 9:074f4f94b584 | 41 | this->uMin = uMin; |
pmic | 9:074f4f94b584 | 42 | this->uMax = uMax; |
pmic | 9:074f4f94b584 | 43 | } |
pmic | 9:074f4f94b584 | 44 | |
pmic | 9:074f4f94b584 | 45 | void DT1_Cntrl::setCoeff_D(float D) |
pmic | 9:074f4f94b584 | 46 | { |
pmic | 9:074f4f94b584 | 47 | this->D = D/tau_f; |
altb2 | 7:cb1492f4f2c6 | 48 | } |
altb2 | 7:cb1492f4f2c6 | 49 | |
altb2 | 7:cb1492f4f2c6 | 50 | void DT1_Cntrl::setCoefficients(float D, float tau_f, float Ts) |
altb2 | 7:cb1492f4f2c6 | 51 | { |
altb2 | 8:3a8d1218022c | 52 | this->p = 1.0f - Ts/tau_f; |
altb2 | 7:cb1492f4f2c6 | 53 | this->tau_f = tau_f; |
pmic | 9:074f4f94b584 | 54 | this->D = D/tau_f; // modified D, now D is consistent with Matlab PID |
pmic | 9:074f4f94b584 | 55 | this->D_init = D/tau_f; // modified D, now D is consistent with Matlab PID |
altb2 | 7:cb1492f4f2c6 | 56 | this->Ts = Ts; |
pmic | 9:074f4f94b584 | 57 | } |