Control Library by altb
Dependents: My_Libraries IndNav_QK3_T265
Diff: DT1_Cntrl.cpp
- Revision:
- 7:cb1492f4f2c6
- Child:
- 8:3a8d1218022c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/DT1_Cntrl.cpp Wed Sep 18 09:56:35 2019 +0000 @@ -0,0 +1,55 @@ +/* + DT1 Controller class + everything is calculated in float + + z - 1 + G(s) = Kd/tau ------- D corresponds Kd in Matlab-formlism pid(...) + z - p +*/ + +#include "DT1_Cntrl.h" +using namespace std; + +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; + reset(0.0f); +} + +DT1_Cntrl::~DT1_Cntrl() {} + +void DT1_Cntrl::reset(float initValue) +{ + eold = initValue; + yold = 0.0; +} + +void DT1_Cntrl::setCoefficients(float D, float tau_f, float Ts) +{ + this->p = 1.0 - 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->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; + return Dpart; +} + +void DT1_Cntrl::set_limits(float ll, float ul) +{ + this->uMin = ll; + this->uMax = ul; +} +