Control Library by altb
Dependents: My_Libraries IndNav_QK3_T265
DT1_Cntrl.cpp
- Committer:
- altb2
- Date:
- 2019-09-19
- Revision:
- 8:3a8d1218022c
- Parent:
- 7:cb1492f4f2c6
- Child:
- 9:074f4f94b584
File content as of revision 8:3a8d1218022c:
/* 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.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->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; }