Control Library by altb
Dependents: My_Libraries IndNav_QK3_T265
DT1_Cntrl.cpp
- Committer:
- pmic
- Date:
- 2020-01-16
- Revision:
- 15:c70cad2f4e64
- Parent:
- 9:074f4f94b584
File content as of revision 15:c70cad2f4e64:
/* DT1 Controller class everything is calculated in float z - 1 G(s) = Kd/tau ------- , p = 1 - Ts/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); set_limits(uMin, uMax); reset(0.0f); } DT1_Cntrl::~DT1_Cntrl() {} void DT1_Cntrl::reset(float initValue) { eold = initValue; 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->Ts = Ts; }