Control Library by altb
Dependents: My_Libraries IndNav_QK3_T265
PID_Cntrl.cpp@11:7ea3efaf0469, 2019-10-02 (annotated)
- Committer:
- altb2
- Date:
- Wed Oct 02 15:30:32 2019 +0000
- Revision:
- 11:7ea3efaf0469
- Parent:
- 10:447f6a864598
- Child:
- 12:81787539c2cb
added setup parameters
Who changed what in which revision?
User | Revision | Line number | New 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 | 11:7ea3efaf0469 | 33 | set_Coefficients(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 | } |