Michael Ernst Peter / Cntrl_Libary

Libary for control.

Dependencies:   FastPWM

Dependents:   RT2_Cuboid

PID_Cntrl.cpp@10:eb29810d831b, 2022-05-05 (annotated)

Committer:
pmic
Date:
Thu May 05 09:18:40 2022 +0000
Revision:
10:eb29810d831b
Parent:
8:3a2131231969 
Adjusted AvgFilter (damn...)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 4:1cbd6af9836c 1 #include "PID_Cntrl.h"
pmic 4:1cbd6af9836c 2
pmic 4:1cbd6af9836c 3 /*
pmic 4:1cbd6af9836c 4 Ts
pmic 4:1cbd6af9836c 5 C(z) = (P + I ---------- + D * tustin(s / (tau_f * s + 1))) * tustin(1 / (tau_ro * s + 1))
pmic 4:1cbd6af9836c 6 1 - z^-1
pmic 4:1cbd6af9836c 7 - Corresponds to MATLAB command: C = pid(P, I, D, tau_f, Ts, 'IFormula', 'BackwardEuler', 'DFormula', 'Trapezoidal') * c2d(tf(1, [tau_ro 1]), Ts, 'tustin')
pmic 4:1cbd6af9836c 8 - Anti-Windup: Saturation of Ipart and u = P*e + IPart + DPart
pmic 4:1cbd6af9836c 9 */
pmic 4:1cbd6af9836c 10
pmic 4:1cbd6af9836c 11 PID_Cntrl::PID_Cntrl(float I, float Ts, float uMin, float uMax)
pmic 4:1cbd6af9836c 12 {
pmic 6:e93f67d98616 13 setup(I, Ts, uMin, uMax);
pmic 4:1cbd6af9836c 14 }
pmic 4:1cbd6af9836c 15
pmic 4:1cbd6af9836c 16 PID_Cntrl::PID_Cntrl(float P, float I, float Ts, float uMin, float uMax)
pmic 4:1cbd6af9836c 17 {
pmic 6:e93f67d98616 18 setup(P, I, Ts, uMin, uMax);
pmic 4:1cbd6af9836c 19 }
pmic 4:1cbd6af9836c 20
pmic 4:1cbd6af9836c 21 PID_Cntrl::PID_Cntrl(float P, float I, float D, float Ts, float uMin, float uMax)
pmic 4:1cbd6af9836c 22 {
pmic 6:e93f67d98616 23 setup(P, I, D, Ts, uMin, uMax);
pmic 4:1cbd6af9836c 24 }
pmic 4:1cbd6af9836c 25
pmic 4:1cbd6af9836c 26 PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMin, float uMax)
pmic 4:1cbd6af9836c 27 {
pmic 6:e93f67d98616 28 setup(P, I, D, tau_f, Ts, uMin, uMax);
pmic 4:1cbd6af9836c 29 }
pmic 4:1cbd6af9836c 30
pmic 4:1cbd6af9836c 31 PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float tau_ro, float Ts, float uMin, float uMax)
pmic 4:1cbd6af9836c 32 {
pmic 6:e93f67d98616 33 setup(P, I, D, tau_f, tau_ro, Ts, uMin, uMax);
pmic 4:1cbd6af9836c 34 }
pmic 4:1cbd6af9836c 35
pmic 4:1cbd6af9836c 36 PID_Cntrl::~PID_Cntrl() {}
pmic 4:1cbd6af9836c 37
pmic 6:e93f67d98616 38 void PID_Cntrl::reset()
pmic 6:e93f67d98616 39 {
pmic 6:e93f67d98616 40 reset(0.0f);
pmic 6:e93f67d98616 41 }
pmic 6:e93f67d98616 42
pmic 4:1cbd6af9836c 43 void PID_Cntrl::reset(float initValue)
pmic 4:1cbd6af9836c 44 {
pmic 4:1cbd6af9836c 45 IPart = initValue;
pmic 4:1cbd6af9836c 46 Dpart = 0.0f;
pmic 4:1cbd6af9836c 47 d_old = 0.0f;
pmic 4:1cbd6af9836c 48 u_old = initValue;
pmic 4:1cbd6af9836c 49 uf = initValue;
pmic 4:1cbd6af9836c 50 }
pmic 4:1cbd6af9836c 51
pmic 6:e93f67d98616 52 void PID_Cntrl::setup(float I, float Ts, float uMin, float uMax)
pmic 4:1cbd6af9836c 53 {
pmic 4:1cbd6af9836c 54 P = 0.0f;
pmic 4:1cbd6af9836c 55 D = 0.0f;
pmic 4:1cbd6af9836c 56 double Ts_d = static_cast<double>( Ts );
pmic 6:e93f67d98616 57 tau_f = static_cast<float>( Ts_d/(0.95*pi) ); // unused
pmic 4:1cbd6af9836c 58 tau_ro = 0.0f;
pmic 6:e93f67d98616 59 setup(P, I, D, tau_f, tau_ro, Ts, uMin, uMax);
pmic 4:1cbd6af9836c 60 }
pmic 4:1cbd6af9836c 61
pmic 6:e93f67d98616 62 void PID_Cntrl::setup(float P, float I, float Ts, float uMin, float uMax)
pmic 4:1cbd6af9836c 63 {
pmic 4:1cbd6af9836c 64 D = 0.0f;
pmic 4:1cbd6af9836c 65 double Ts_d = static_cast<double>( Ts );
pmic 6:e93f67d98616 66 tau_f = static_cast<float>( Ts_d/(0.95*pi) ); // unused
pmic 4:1cbd6af9836c 67 tau_ro = 0.0f;
pmic 6:e93f67d98616 68 setup(P, I, D, tau_f, tau_ro, Ts, uMin, uMax);
pmic 4:1cbd6af9836c 69 }
pmic 4:1cbd6af9836c 70
pmic 6:e93f67d98616 71 void PID_Cntrl::setup(float P, float I, float D, float Ts, float uMin, float uMax)
pmic 4:1cbd6af9836c 72 {
pmic 4:1cbd6af9836c 73 double Ts_d = static_cast<double>( Ts );
pmic 4:1cbd6af9836c 74 tau_f = static_cast<float>( Ts_d/(0.95*pi) ); // 1.0/(0.95*2.0*pi*fnyq)
pmic 4:1cbd6af9836c 75 tau_ro = 0.0f;
pmic 6:e93f67d98616 76 setup(P, I, D, tau_f, tau_ro, Ts, uMin, uMax);
pmic 4:1cbd6af9836c 77 }
pmic 4:1cbd6af9836c 78
pmic 6:e93f67d98616 79 void PID_Cntrl::setup(float P, float I, float D, float tau_f, float Ts, float uMin, float uMax)
pmic 4:1cbd6af9836c 80 {
pmic 4:1cbd6af9836c 81 tau_ro = 0.0f;
pmic 6:e93f67d98616 82 setup(P, I, D, tau_f, tau_ro, Ts, uMin, uMax);
pmic 4:1cbd6af9836c 83 }
pmic 4:1cbd6af9836c 84
pmic 6:e93f67d98616 85 void PID_Cntrl::setup(float P, float I, float D, float tau_f, float tau_ro, float Ts, float uMin, float uMax)
pmic 4:1cbd6af9836c 86 {
pmic 6:e93f67d98616 87 setCoefficients(P, I, D, tau_f, tau_ro, Ts);
pmic 4:1cbd6af9836c 88 this->uMin = uMin;
pmic 4:1cbd6af9836c 89 this->uMax = uMax;
pmic 6:e93f67d98616 90 reset();
pmic 4:1cbd6af9836c 91 }
pmic 4:1cbd6af9836c 92
pmic 4:1cbd6af9836c 93 void PID_Cntrl::setCoeff_P(float P)
pmic 4:1cbd6af9836c 94 {
pmic 4:1cbd6af9836c 95 this->P = P;
pmic 4:1cbd6af9836c 96 }
pmic 4:1cbd6af9836c 97
pmic 4:1cbd6af9836c 98 void PID_Cntrl::setCoeff_I(float I)
pmic 4:1cbd6af9836c 99 {
pmic 4:1cbd6af9836c 100 this->I = I;
pmic 4:1cbd6af9836c 101 updateCoeff_I(I, Ts);
pmic 4:1cbd6af9836c 102 }
pmic 4:1cbd6af9836c 103
pmic 4:1cbd6af9836c 104 void PID_Cntrl::setCoeff_D(float D)
pmic 4:1cbd6af9836c 105 {
pmic 4:1cbd6af9836c 106 this->D = D;
pmic 4:1cbd6af9836c 107 updateCoeff_D(D, Ts, tau_f);
pmic 4:1cbd6af9836c 108 }
pmic 4:1cbd6af9836c 109
pmic 4:1cbd6af9836c 110 void PID_Cntrl::scale_PIDT2_param(float scale)
pmic 4:1cbd6af9836c 111 {
pmic 4:1cbd6af9836c 112 P = P_init * scale;
pmic 4:1cbd6af9836c 113 I = I_init * scale;
pmic 6:e93f67d98616 114 D = D_init * scale;
pmic 4:1cbd6af9836c 115 updateCoeff_I(I, Ts);
pmic 4:1cbd6af9836c 116 updateCoeff_D(D, Ts, tau_f);
pmic 4:1cbd6af9836c 117 }
pmic 4:1cbd6af9836c 118
pmic 4:1cbd6af9836c 119 float PID_Cntrl::update(float e)
pmic 4:1cbd6af9836c 120 {
pmic 8:3a2131231969 121 if(bi !=0) {
pmic 8:3a2131231969 122 IPart = saturate(IPart + bi*e, uMin, uMax);
pmic 8:3a2131231969 123 } else {
pmic 8:3a2131231969 124 IPart = 0.0f;
pmic 8:3a2131231969 125 }
pmic 4:1cbd6af9836c 126 Dpart = bd*(e - d_old) - ad*Dpart;
pmic 4:1cbd6af9836c 127 d_old = e;
pmic 4:1cbd6af9836c 128 float u = P*e + IPart + Dpart;
pmic 4:1cbd6af9836c 129 uf = saturate(bf*(u + u_old) - af*uf, uMin, uMax);
pmic 4:1cbd6af9836c 130 u_old = u;
pmic 4:1cbd6af9836c 131 return uf;
pmic 4:1cbd6af9836c 132 }
pmic 4:1cbd6af9836c 133
pmic 4:1cbd6af9836c 134 float PID_Cntrl::update(float e, float y)
pmic 4:1cbd6af9836c 135 {
pmic 8:3a2131231969 136 if(bi !=0) {
pmic 8:3a2131231969 137 IPart = saturate(IPart + bi*e, uMin, uMax);
pmic 8:3a2131231969 138 } else {
pmic 8:3a2131231969 139 IPart = 0.0f;
pmic 8:3a2131231969 140 }
pmic 4:1cbd6af9836c 141 Dpart = bd*(y - d_old) - ad*Dpart;
pmic 4:1cbd6af9836c 142 d_old = y;
pmic 4:1cbd6af9836c 143 float u = P*e + IPart - Dpart;
pmic 4:1cbd6af9836c 144 uf = saturate(bf*(u + u_old) - af*uf, uMin, uMax);
pmic 4:1cbd6af9836c 145 u_old = u;
pmic 4:1cbd6af9836c 146 return uf;
pmic 4:1cbd6af9836c 147 }
pmic 4:1cbd6af9836c 148
pmic 4:1cbd6af9836c 149 bool PID_Cntrl::update_param(int cntrlr_param, float value)
pmic 4:1cbd6af9836c 150 {
pmic 4:1cbd6af9836c 151 switch (cntrlr_param) {
pmic 4:1cbd6af9836c 152 case 0:
pmic 4:1cbd6af9836c 153 setCoeff_P(value);
pmic 4:1cbd6af9836c 154 return true;
pmic 4:1cbd6af9836c 155 break;
pmic 4:1cbd6af9836c 156 case 1:
pmic 4:1cbd6af9836c 157 setCoeff_I(value);
pmic 4:1cbd6af9836c 158 return true;
pmic 4:1cbd6af9836c 159 break;
pmic 4:1cbd6af9836c 160 case 2:
pmic 4:1cbd6af9836c 161 setCoeff_D(value);
pmic 4:1cbd6af9836c 162 return true;
pmic 4:1cbd6af9836c 163 break;
pmic 4:1cbd6af9836c 164 case 3:
pmic 4:1cbd6af9836c 165 this->tau_f = value;
pmic 4:1cbd6af9836c 166 break;
pmic 4:1cbd6af9836c 167 case 4:
pmic 4:1cbd6af9836c 168 this->tau_ro = value;
pmic 4:1cbd6af9836c 169 break;
pmic 4:1cbd6af9836c 170 case 5:
pmic 4:1cbd6af9836c 171 return false;
pmic 4:1cbd6af9836c 172 break;
pmic 4:1cbd6af9836c 173 case 6:
pmic 4:1cbd6af9836c 174 this->uMin = value;
pmic 4:1cbd6af9836c 175 return true;
pmic 4:1cbd6af9836c 176 break;
pmic 4:1cbd6af9836c 177 case 7:
pmic 4:1cbd6af9836c 178 this->uMax = value;
pmic 4:1cbd6af9836c 179 return true;
pmic 4:1cbd6af9836c 180 break;
pmic 4:1cbd6af9836c 181 default:
pmic 4:1cbd6af9836c 182 return false;
pmic 4:1cbd6af9836c 183 break;
pmic 4:1cbd6af9836c 184 }
pmic 4:1cbd6af9836c 185 updateCoeff_I(I, Ts);
pmic 4:1cbd6af9836c 186 updateCoeff_D(D, Ts, tau_f);
pmic 4:1cbd6af9836c 187 updateCoeff_RO(Ts, tau_ro);
pmic 4:1cbd6af9836c 188 return true;
pmic 4:1cbd6af9836c 189 }
pmic 4:1cbd6af9836c 190
pmic 4:1cbd6af9836c 191 void PID_Cntrl::set_limits(float uMin, float uMax)
pmic 4:1cbd6af9836c 192 {
pmic 4:1cbd6af9836c 193 this->uMin = uMin;
pmic 4:1cbd6af9836c 194 this->uMax = uMax;
pmic 4:1cbd6af9836c 195 }
pmic 4:1cbd6af9836c 196
pmic 6:e93f67d98616 197 float PID_Cntrl::prewarp(float T, float Ts)
pmic 6:e93f67d98616 198 {
pmic 6:e93f67d98616 199 double T_d = static_cast<double>( T );
pmic 6:e93f67d98616 200 double Ts_d = static_cast<double>( Ts );
pmic 6:e93f67d98616 201 return static_cast<float>( Ts_d/(2.0*tan(Ts_d/(2.0*T_d))) );
pmic 6:e93f67d98616 202 }
pmic 6:e93f67d98616 203
pmic 4:1cbd6af9836c 204 float PID_Cntrl::get_ulimit()
pmic 4:1cbd6af9836c 205 {
pmic 4:1cbd6af9836c 206 return uMax;
pmic 4:1cbd6af9836c 207 }
pmic 4:1cbd6af9836c 208
pmic 4:1cbd6af9836c 209 float PID_Cntrl::get_P_gain()
pmic 4:1cbd6af9836c 210 {
pmic 4:1cbd6af9836c 211 return P;
pmic 4:1cbd6af9836c 212 }
pmic 4:1cbd6af9836c 213
pmic 4:1cbd6af9836c 214 float PID_Cntrl::get_bd()
pmic 4:1cbd6af9836c 215 {
pmic 4:1cbd6af9836c 216 return bd;
pmic 4:1cbd6af9836c 217 }
pmic 4:1cbd6af9836c 218
pmic 4:1cbd6af9836c 219 float PID_Cntrl::get_ad()
pmic 4:1cbd6af9836c 220 {
pmic 4:1cbd6af9836c 221 return ad;
pmic 4:1cbd6af9836c 222 }
pmic 4:1cbd6af9836c 223
pmic 6:e93f67d98616 224 void PID_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float tau_ro, float Ts)
pmic 4:1cbd6af9836c 225 {
pmic 4:1cbd6af9836c 226 /* store parameters */
pmic 4:1cbd6af9836c 227 this->P = P;
pmic 4:1cbd6af9836c 228 this->I = I;
pmic 4:1cbd6af9836c 229 this->D = D;
pmic 4:1cbd6af9836c 230 this->tau_f = tau_f;
pmic 4:1cbd6af9836c 231 this->tau_ro = tau_ro;
pmic 4:1cbd6af9836c 232 this->Ts = Ts;
pmic 4:1cbd6af9836c 233 updateCoeff_I(I, Ts);
pmic 4:1cbd6af9836c 234 updateCoeff_D(D, Ts, tau_f);
pmic 4:1cbd6af9836c 235 updateCoeff_RO(Ts, tau_ro);
pmic 4:1cbd6af9836c 236
pmic 4:1cbd6af9836c 237 /* store initial parameters */
pmic 4:1cbd6af9836c 238 this->P_init = P;
pmic 4:1cbd6af9836c 239 this->I_init = I;
pmic 4:1cbd6af9836c 240 this->D_init = D;
pmic 4:1cbd6af9836c 241 }
pmic 4:1cbd6af9836c 242
pmic 4:1cbd6af9836c 243 float PID_Cntrl::saturate(float u, float uMin, float uMax)
pmic 4:1cbd6af9836c 244 {
pmic 4:1cbd6af9836c 245 if(u > uMax) {
pmic 4:1cbd6af9836c 246 u = uMax;
pmic 4:1cbd6af9836c 247 } else if(u < uMin) {
pmic 4:1cbd6af9836c 248 u = uMin;
pmic 4:1cbd6af9836c 249 }
pmic 4:1cbd6af9836c 250 return u;
pmic 4:1cbd6af9836c 251 }
pmic 4:1cbd6af9836c 252
pmic 4:1cbd6af9836c 253 void PID_Cntrl::updateCoeff_I(float I, float Ts)
pmic 4:1cbd6af9836c 254 {
pmic 4:1cbd6af9836c 255 double I_d = static_cast<double>( I );
pmic 4:1cbd6af9836c 256 double Ts_d = static_cast<double>( Ts );
pmic 4:1cbd6af9836c 257 bi = static_cast<float>( I_d*Ts_d );
pmic 4:1cbd6af9836c 258 }
pmic 4:1cbd6af9836c 259
pmic 4:1cbd6af9836c 260 void PID_Cntrl::updateCoeff_D(float D, float Ts, float tau_f)
pmic 4:1cbd6af9836c 261 {
pmic 4:1cbd6af9836c 262 double D_d = static_cast<double>( D );
pmic 4:1cbd6af9836c 263 double Ts_d = static_cast<double>( Ts );
pmic 4:1cbd6af9836c 264 double tau_f_d = static_cast<double>( tau_f );
pmic 4:1cbd6af9836c 265 bd = static_cast<float>( 2.0*D_d/(Ts_d + 2.0*tau_f_d) );
pmic 4:1cbd6af9836c 266 ad = static_cast<float>( (Ts_d - 2.0*tau_f_d)/(Ts_d + 2.0*tau_f_d) );
pmic 4:1cbd6af9836c 267 }
pmic 4:1cbd6af9836c 268
pmic 4:1cbd6af9836c 269 void PID_Cntrl::updateCoeff_RO(float Ts, float tau_ro)
pmic 4:1cbd6af9836c 270 {
pmic 4:1cbd6af9836c 271 double Ts_d = static_cast<double>( Ts );
pmic 4:1cbd6af9836c 272 double tau_ro_d = static_cast<double>( tau_ro );
pmic 4:1cbd6af9836c 273 bf = static_cast<float>( Ts_d/(Ts_d + 2.0*tau_ro_d) );
pmic 4:1cbd6af9836c 274 af = static_cast<float>( (Ts_d - 2.0*tau_ro_d)/(Ts_d + 2.0*tau_ro_d) );
pmic 4:1cbd6af9836c 275 }