altb_pmic / Lib_Cntrl

Control Library by altb

Dependents:   My_Libraries IndNav_QK3_T265

PID_Cntrl.cpp@15:c70cad2f4e64, 2020-01-16 (annotated)

Committer:
pmic
Date:
Thu Jan 16 09:12:50 2020 +0000
Revision:
15:c70cad2f4e64
Parent:
12:81787539c2cb 
Revisit IIR_filter.h and IIR_filter.cpp. Change internal double to float arithmetic.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb 0:d49418189c5c 1 /*
altb 0:d49418189c5c 2 PI Controller class with anti windup reset in biquad transposed direct form 2
altb 0:d49418189c5c 3 see e.g.: https://www.dsprelated.com/freebooks/filters/Four_Direct_Forms.html
altb2 2:e7c9e50a2e46 4 everything is calculated in float
altb 0:d49418189c5c 5
altb2 2:e7c9e50a2e46 6 Ts z - 1
altb2 2:e7c9e50a2e46 7 G(s) = P + I ------- + D/tau ------- D corresponds Kd in Matlab-formlism pid(...)
altb2 2:e7c9e50a2e46 8 z - 1 z - p
altb 0:d49418189c5c 9 */
altb 0:d49418189c5c 10
altb 0:d49418189c5c 11 #include "PID_Cntrl.h"
altb 0:d49418189c5c 12 using namespace std;
altb 0:d49418189c5c 13
altb 0:d49418189c5c 14 PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMin, float uMax)
altb 0:d49418189c5c 15 {
altb 0:d49418189c5c 16 setCoefficients(P, I, D, tau_f, Ts);
altb2 2:e7c9e50a2e46 17 this->uMin = uMin;
altb2 2:e7c9e50a2e46 18 this->uMax = uMax;
altb 0:d49418189c5c 19 reset(0.0f);
altb 0:d49418189c5c 20 }
altb 0:d49418189c5c 21
altb 0:d49418189c5c 22 PID_Cntrl::~PID_Cntrl() {}
altb 0:d49418189c5c 23
altb 0:d49418189c5c 24 void PID_Cntrl::reset(float initValue)
altb 0:d49418189c5c 25 {
altb2 2:e7c9e50a2e46 26 Iold = initValue;
altb2 10:447f6a864598 27 eold = 0.0;yold = 0.0;Dpart_old = 0.0;
altb 0:d49418189c5c 28 del = 0.0;
altb 0:d49418189c5c 29 }
altb 0:d49418189c5c 30
altb2 11:7ea3efaf0469 31 void PID_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float Ts,float uMin, float uMax)
altb2 11:7ea3efaf0469 32 {
altb2 12:81787539c2cb 33 setCoefficients(P,I,D,tau_f,Ts);
altb2 11:7ea3efaf0469 34 this->uMin = uMin;
altb2 11:7ea3efaf0469 35 this->uMax = uMax;
altb2 11:7ea3efaf0469 36
altb2 11:7ea3efaf0469 37 }
altb 0:d49418189c5c 38 void PID_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float Ts)
altb 0:d49418189c5c 39 {
altb2 10:447f6a864598 40 this->p = 1.0f - Ts/tau_f;
altb 0:d49418189c5c 41 this->P = P;
altb2 5:d8c53cece01b 42 this->P_init = P;
altb 0:d49418189c5c 43 this->I = I;
altb2 5:d8c53cece01b 44 this->I_init = I;
altb2 3:27595dd9e433 45 this->tau_f = tau_f;
altb2 2:e7c9e50a2e46 46 this->D_ = D/tau_f; // modified D, now D is consistent with Matlab PID
altb2 5:d8c53cece01b 47 this->D__init = D/tau_f; // modified D, now D is consistent with Matlab PID
altb 0:d49418189c5c 48 this->Ts = Ts;
altb 0:d49418189c5c 49 if(P!=0)
altb 0:d49418189c5c 50 this->Ka=1/P;
altb 0:d49418189c5c 51 else
altb 0:d49418189c5c 52 this->Ka=1.0f;
altb 0:d49418189c5c 53
altb 0:d49418189c5c 54 }
altb2 3:27595dd9e433 55 void PID_Cntrl::setCoeff_P(float P)
altb2 3:27595dd9e433 56 {
altb2 3:27595dd9e433 57 this->P = P;
altb2 3:27595dd9e433 58 }
altb2 3:27595dd9e433 59 void PID_Cntrl::setCoeff_I(float I)
altb2 3:27595dd9e433 60 {
altb2 3:27595dd9e433 61 this->I = I;
altb2 3:27595dd9e433 62 }
altb2 3:27595dd9e433 63 void PID_Cntrl::setCoeff_D(float D)
altb2 3:27595dd9e433 64 {
altb2 3:27595dd9e433 65 this->D_ = D/this->tau_f;
altb2 3:27595dd9e433 66 }
altb2 5:d8c53cece01b 67 void PID_Cntrl::scale_PID_param(float sc)
altb2 5:d8c53cece01b 68 {
altb2 5:d8c53cece01b 69 this->P = this->P_init * sc;
altb2 5:d8c53cece01b 70 this->I = this->I_init * sc;
altb2 5:d8c53cece01b 71 this->D_ = this->D__init * sc;
altb2 5:d8c53cece01b 72 }
altb 0:d49418189c5c 73
altb2 2:e7c9e50a2e46 74 float PID_Cntrl::doStep(float e)
altb 0:d49418189c5c 75 {
altb2 2:e7c9e50a2e46 76 float Ipart = Iold+I*Ts*(e-del);
altb2 10:447f6a864598 77 float Dpart = D_*(e-eold)+p*Dpart_old;
altb2 2:e7c9e50a2e46 78 float u = P*e + Dpart + Ipart; // unconstrained output
altb2 2:e7c9e50a2e46 79 float uc = u; // constrained output
altb 0:d49418189c5c 80 if(u > uMax) uc = uMax;
altb 0:d49418189c5c 81 else if(u < uMin) uc = uMin;
altb 0:d49418189c5c 82 del=(u-uc)*Ka;
altb 0:d49418189c5c 83 eold=e;
altb 0:d49418189c5c 84 Iold=Ipart;
altb2 10:447f6a864598 85 Dpart_old=Dpart;
altb2 10:447f6a864598 86 return uc;
altb2 10:447f6a864598 87 }
altb2 10:447f6a864598 88 float PID_Cntrl::doStep(float e,float y)
altb2 10:447f6a864598 89 {
altb2 10:447f6a864598 90 float Ipart = Iold+I*Ts*(e-del);
altb2 10:447f6a864598 91 float Dpart = D_ * (y-yold) + p * Dpart_old;
altb2 10:447f6a864598 92 float u = P*e - Dpart + Ipart; // unconstrained output
altb2 10:447f6a864598 93 float uc = u; // constrained output
altb2 10:447f6a864598 94 if(u > uMax) uc = uMax;
altb2 10:447f6a864598 95 else if(u < uMin) uc = uMin;
altb2 10:447f6a864598 96 del=(u-uc)*Ka;
altb2 10:447f6a864598 97 eold=e;
altb2 10:447f6a864598 98 Iold=Ipart;
altb2 10:447f6a864598 99 Dpart_old = Dpart;
altb2 10:447f6a864598 100 yold = y;
altb2 2:e7c9e50a2e46 101 return uc;
altb 0:d49418189c5c 102 }
altb 0:d49418189c5c 103
altb2 2:e7c9e50a2e46 104 void PID_Cntrl::set_limits(float ll, float ul)
altb 0:d49418189c5c 105 {
altb2 2:e7c9e50a2e46 106 this->uMin = ll;
altb2 2:e7c9e50a2e46 107 this->uMax = ul;
altb 0:d49418189c5c 108 }
altb 0:d49418189c5c 109
altb 0:d49418189c5c 110 float PID_Cntrl::get_ulimit(void)
altb 0:d49418189c5c 111 {
altb2 2:e7c9e50a2e46 112 return this->uMax;
altb2 5:d8c53cece01b 113 }
altb2 5:d8c53cece01b 114
altb2 5:d8c53cece01b 115 float PID_Cntrl::get_P_gain(void)
altb2 5:d8c53cece01b 116 {
altb2 5:d8c53cece01b 117 return this->P;
altb 0:d49418189c5c 118 }