Ruprecht Altenburger / Cntrl_Lib

.

Fork of Cntrlol_Lib by Ruprecht Altenburger

PID_Cntrl.cpp@8:32445aab4589, 2018-10-26 (annotated)

Committer:
altb
Date:
Fri Oct 26 12:40:51 2018 +0000
Revision:
8:32445aab4589
Parent:
4:9cf60fa0a579 
corrected bugs

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb 0:e2a7d7f91e49 1 /*
altb 0:e2a7d7f91e49 2 PI Controller class with anti windup reset in biquad transposed direct form 2
altb 0:e2a7d7f91e49 3 see e.g.: https://www.dsprelated.com/freebooks/filters/Four_Direct_Forms.html
altb 0:e2a7d7f91e49 4 everything is calculated in double
altb 0:e2a7d7f91e49 5
altb 1:bf62e74fbcf3 6 Ts z - 1
altb 3:27b75e7dd54a 7 G(s) = P + I ------- + D ------- D corresponds Kd/Tf in Matlab-formlism pid(...)
altb 1:bf62e74fbcf3 8 z - 1 z - p
altb 0:e2a7d7f91e49 9 */
altb 0:e2a7d7f91e49 10
altb 1:bf62e74fbcf3 11 #include "PID_Cntrl.h"
altb 0:e2a7d7f91e49 12 using namespace std;
altb 0:e2a7d7f91e49 13
altb 2:1f8ddc46c578 14 PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMin, float uMax)
altb 0:e2a7d7f91e49 15 {
altb 1:bf62e74fbcf3 16 setCoefficients(P, I, D, tau_f, Ts);
altb 3:27b75e7dd54a 17 this->uMin = (double)uMin;
altb 0:e2a7d7f91e49 18 this->uMax = (double)uMax;
altb 0:e2a7d7f91e49 19 reset(0.0f);
altb 0:e2a7d7f91e49 20 }
altb 0:e2a7d7f91e49 21
altb 1:bf62e74fbcf3 22 PID_Cntrl::~PID_Cntrl() {}
altb 0:e2a7d7f91e49 23
altb 1:bf62e74fbcf3 24 void PID_Cntrl::reset(float initValue)
altb 0:e2a7d7f91e49 25 {
altb 1:bf62e74fbcf3 26 Iold = (double)initValue;
altb 1:bf62e74fbcf3 27 eold = 0.0;yold = 0.0;
altb 1:bf62e74fbcf3 28 del = 0.0;
altb 0:e2a7d7f91e49 29 }
altb 0:e2a7d7f91e49 30
altb 1:bf62e74fbcf3 31 void PID_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float Ts)
altb 0:e2a7d7f91e49 32 {
altb 1:bf62e74fbcf3 33 this->p = 1.0 - (double)Ts/(double)tau_f;
altb 1:bf62e74fbcf3 34 this->P = P;
altb 1:bf62e74fbcf3 35 this->I = I;
altb 1:bf62e74fbcf3 36 this->D = D;
altb 2:1f8ddc46c578 37 this->Ts = Ts;
altb 2:1f8ddc46c578 38 if(P!=0)
altb 2:1f8ddc46c578 39 this->Ka=1/P;
altb 2:1f8ddc46c578 40 else
altb 2:1f8ddc46c578 41 this->Ka=1.0f;
altb 1:bf62e74fbcf3 42
altb 0:e2a7d7f91e49 43 }
altb 0:e2a7d7f91e49 44
altb 1:bf62e74fbcf3 45 float PID_Cntrl::doStep(double e)
altb 0:e2a7d7f91e49 46 {
altb 2:1f8ddc46c578 47 double Ipart = Iold+I*Ts*(e-del);
altb 2:1f8ddc46c578 48 double Dpart = D*(e-eold)+p*yold;
altb 2:1f8ddc46c578 49 double u = P*e + Dpart + Ipart; // unconstrained output
altb 0:e2a7d7f91e49 50 double uc = u; // constrained output
altb 0:e2a7d7f91e49 51 if(u > uMax) uc = uMax;
altb 0:e2a7d7f91e49 52 else if(u < uMin) uc = uMin;
altb 2:1f8ddc46c578 53 del=(u-uc)*Ka;
altb 2:1f8ddc46c578 54 eold=e;
altb 2:1f8ddc46c578 55 Iold=Ipart;
altb 2:1f8ddc46c578 56 yold=Dpart;
altb 0:e2a7d7f91e49 57 return (float)uc;
altb 3:27b75e7dd54a 58 }
altb 3:27b75e7dd54a 59
altb 3:27b75e7dd54a 60 void PID_Cntrl::set_limits(double ll, double ul)
altb 3:27b75e7dd54a 61 {
altb 3:27b75e7dd54a 62 this->uMin = (double)ll;
altb 3:27b75e7dd54a 63 this->uMax = (double)ul;
altb 3:27b75e7dd54a 64 }
altb 4:9cf60fa0a579 65
altb 3:27b75e7dd54a 66 float PID_Cntrl::get_ulimit(void)
altb 3:27b75e7dd54a 67 {
altb 3:27b75e7dd54a 68 return (float)this->uMax;
altb 0:e2a7d7f91e49 69 }