Ruprecht Altenburger / Cntrlol_Lib

My Controller Library

PI_Cntrl.cpp@1:bf62e74fbcf3, 2018-09-28 (annotated)

Committer:
altb
Date:
Fri Sep 28 09:01:42 2018 +0000
Revision:
1:bf62e74fbcf3
Parent:
0:e2a7d7f91e49 
.

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 0:e2a7d7f91e49 6 Tn*s + 1
altb 0:e2a7d7f91e49 7 G(s) = Kp ------------- with s ~ (1 - z^-1)/Ts
altb 0:e2a7d7f91e49 8 Ts*s
altb 0:e2a7d7f91e49 9 */
altb 0:e2a7d7f91e49 10
altb 0:e2a7d7f91e49 11 #include "PI_Cntrl.h"
altb 0:e2a7d7f91e49 12
altb 0:e2a7d7f91e49 13 using namespace std;
altb 0:e2a7d7f91e49 14
altb 0:e2a7d7f91e49 15 PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts)
altb 0:e2a7d7f91e49 16 {
altb 0:e2a7d7f91e49 17 setCoefficients(Kp, Tn, Ts);
altb 0:e2a7d7f91e49 18 uMax = 10000000000.0;
altb 0:e2a7d7f91e49 19 uMin = -uMax;
altb 0:e2a7d7f91e49 20 reset(0.0f);
altb 0:e2a7d7f91e49 21 }
altb 0:e2a7d7f91e49 22
altb 0:e2a7d7f91e49 23 PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax)
altb 0:e2a7d7f91e49 24 {
altb 0:e2a7d7f91e49 25 setCoefficients(Kp, Tn, Ts);
altb 0:e2a7d7f91e49 26 this->uMax = (double)uMax;
altb 0:e2a7d7f91e49 27 uMin = -(double)uMax;
altb 0:e2a7d7f91e49 28 reset(0.0f);
altb 0:e2a7d7f91e49 29 }
altb 0:e2a7d7f91e49 30
altb 0:e2a7d7f91e49 31 PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax, float uMin)
altb 0:e2a7d7f91e49 32 {
altb 0:e2a7d7f91e49 33 setCoefficients(Kp, Tn, Ts);
altb 0:e2a7d7f91e49 34 this->uMax = (double)uMax;
altb 0:e2a7d7f91e49 35 this->uMin = (double)uMin;
altb 0:e2a7d7f91e49 36 reset(0.0f);
altb 0:e2a7d7f91e49 37 }
altb 0:e2a7d7f91e49 38
altb 0:e2a7d7f91e49 39 PI_Cntrl::~PI_Cntrl() {}
altb 0:e2a7d7f91e49 40
altb 0:e2a7d7f91e49 41 void PI_Cntrl::reset(float initValue)
altb 0:e2a7d7f91e49 42 {
altb 0:e2a7d7f91e49 43 s = (double)initValue;
altb 0:e2a7d7f91e49 44 }
altb 0:e2a7d7f91e49 45
altb 0:e2a7d7f91e49 46 void PI_Cntrl::setCoefficients(float Kp, float Tn, float Ts)
altb 0:e2a7d7f91e49 47 {
altb 0:e2a7d7f91e49 48 b0 = (double)Kp*(1.0 + (double)Ts/(double)Tn);
altb 0:e2a7d7f91e49 49 b1 = -(double)Kp;
altb 0:e2a7d7f91e49 50 b2 = (double)Ts/(double)Tn;
altb 0:e2a7d7f91e49 51 }
altb 0:e2a7d7f91e49 52
altb 0:e2a7d7f91e49 53 float PI_Cntrl::doStep(double e)
altb 0:e2a7d7f91e49 54 {
altb 0:e2a7d7f91e49 55 double u = b0*e + s; // unconstrained output
altb 0:e2a7d7f91e49 56 double uc = u; // constrained output
altb 0:e2a7d7f91e49 57 if(u > uMax) uc = uMax;
altb 0:e2a7d7f91e49 58 else if(u < uMin) uc = uMin;
altb 0:e2a7d7f91e49 59 s = b1*e + u - b2*(u - uc);
altb 0:e2a7d7f91e49 60 return (float)uc;
altb 0:e2a7d7f91e49 61 }