Control Library by altb
Dependents: My_Libraries IndNav_QK3_T265
Diff: DT1_Cntrl.cpp
- Revision:
- 9:074f4f94b584
- Parent:
- 8:3a8d1218022c
diff -r 3a8d1218022c -r 074f4f94b584 DT1_Cntrl.cpp --- a/DT1_Cntrl.cpp Thu Sep 19 08:51:03 2019 +0000 +++ b/DT1_Cntrl.cpp Fri Sep 20 09:26:23 2019 +0000 @@ -1,10 +1,10 @@ -/* - DT1 Controller class +/* + DT1 Controller class everything is calculated in float - - z - 1 - G(s) = Kd/tau ------- D corresponds Kd in Matlab-formlism pid(...) - z - p + + z - 1 + G(s) = Kd/tau ------- , p = 1 - Ts/tau D corresponds Kd in Matlab-formlism pid(...) + z - p */ #include "DT1_Cntrl.h" @@ -13,45 +13,45 @@ DT1_Cntrl::DT1_Cntrl(float D, float tau_f, float Ts, float uMin, float uMax) { setCoefficients(D, tau_f, Ts); - this->uMin = uMin; - this->uMax = uMax; + set_limits(uMin, uMax); reset(0.0f); } DT1_Cntrl::~DT1_Cntrl() {} + void DT1_Cntrl::reset(float initValue) { eold = initValue; - yold = 0.0; + yold = 0.0f; +} + +float DT1_Cntrl::doStep(float e) +{ + float Dpart = D * ( e - eold ) + p * yold; + if(Dpart > uMax) Dpart = uMax; + else if(Dpart < uMin) Dpart = uMin; + eold = e; + yold = Dpart; + return Dpart; +} + +void DT1_Cntrl::set_limits(float uMin, float uMax) +{ + this->uMin = uMin; + this->uMax = uMax; +} + +void DT1_Cntrl::setCoeff_D(float D) +{ + this->D = D/tau_f; } void DT1_Cntrl::setCoefficients(float D, float tau_f, float Ts) { this->p = 1.0f - Ts/tau_f; this->tau_f = tau_f; - this->D_ = D/tau_f; // modified D, now D is consistent with Matlab PID - this->D__init = D/tau_f; // modified D, now D is consistent with Matlab PID + this->D = D/tau_f; // modified D, now D is consistent with Matlab PID + this->D_init = D/tau_f; // modified D, now D is consistent with Matlab PID this->Ts = Ts; -} -void DT1_Cntrl::setCoeff_D(float D) -{ - this->D_ = D/this->tau_f; -} - -float DT1_Cntrl::doStep(float e) -{ - float Dpart = D_*(e-eold)+p*yold; - if(Dpart > uMax) Dpart = uMax; - else if(Dpart < uMin) Dpart = uMin; - eold=e; - yold=Dpart; - return Dpart; -} - -void DT1_Cntrl::set_limits(float ll, float ul) -{ - this->uMin = ll; - this->uMax = ul; -} - +} \ No newline at end of file