altb_pmic / Lib_Cntrl

Control Library by altb

Dependents:   My_Libraries IndNav_QK3_T265

DT1_Cntrl.cpp@9:074f4f94b584, 2019-09-20 (annotated)

Committer:
pmic
Date:
Fri Sep 20 09:26:23 2019 +0000
Revision:
9:074f4f94b584
Parent:
8:3a8d1218022c 
Clean up DT1_Cntrl.

Who changed what in which revision?

UserRevisionLine numberNew 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 }