Ruprecht Altenburger / Mbed OS mirror_actuator_V1

Mirror actuator for RT2 lab

Dependencies:   FastPWM

Library_Cntrl/PI_Cntrl.cpp@15:9f32f64eee5b, 2021-05-02 (annotated)

Committer:
altb2
Date:
Sun May 02 08:17:06 2021 +0000
Revision:
15:9f32f64eee5b
commit for students, 2.5.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 15:9f32f64eee5b 1 /*
altb2 15:9f32f64eee5b 2 PI Controller class with anti windup reset in biquad transposed direct form 2
altb2 15:9f32f64eee5b 3 see e.g.: https://www.dsprelated.com/freebooks/filters/Four_Direct_Forms.html
altb2 15:9f32f64eee5b 4 everything is calculated in double
altb2 15:9f32f64eee5b 5
altb2 15:9f32f64eee5b 6 Tn*s + 1
altb2 15:9f32f64eee5b 7 G(s) = Kp ------------- with s ~ (1 - z^-1)/Ts
altb2 15:9f32f64eee5b 8 Ts*s
altb2 15:9f32f64eee5b 9 */
altb2 15:9f32f64eee5b 10
altb2 15:9f32f64eee5b 11 #include "PI_Cntrl.h"
altb2 15:9f32f64eee5b 12
altb2 15:9f32f64eee5b 13 using namespace std;
altb2 15:9f32f64eee5b 14
altb2 15:9f32f64eee5b 15 PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts)
altb2 15:9f32f64eee5b 16 {
altb2 15:9f32f64eee5b 17 setCoefficients(Kp, Tn, Ts);
altb2 15:9f32f64eee5b 18 uMax = 10000000000.0;
altb2 15:9f32f64eee5b 19 uMin = -uMax;
altb2 15:9f32f64eee5b 20 reset(0.0f);
altb2 15:9f32f64eee5b 21 }
altb2 15:9f32f64eee5b 22
altb2 15:9f32f64eee5b 23 PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax)
altb2 15:9f32f64eee5b 24 {
altb2 15:9f32f64eee5b 25 setCoefficients(Kp, Tn, Ts);
altb2 15:9f32f64eee5b 26 this->uMax = (double)uMax;
altb2 15:9f32f64eee5b 27 uMin = -(double)uMax;
altb2 15:9f32f64eee5b 28 reset(0.0f);
altb2 15:9f32f64eee5b 29 }
altb2 15:9f32f64eee5b 30
altb2 15:9f32f64eee5b 31 PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax, float uMin)
altb2 15:9f32f64eee5b 32 {
altb2 15:9f32f64eee5b 33 setCoefficients(Kp, Tn, Ts);
altb2 15:9f32f64eee5b 34 this->uMax = (double)uMax;
altb2 15:9f32f64eee5b 35 this->uMin = (double)uMin;
altb2 15:9f32f64eee5b 36 reset(0.0f);
altb2 15:9f32f64eee5b 37 }
altb2 15:9f32f64eee5b 38
altb2 15:9f32f64eee5b 39 PI_Cntrl::~PI_Cntrl() {}
altb2 15:9f32f64eee5b 40
altb2 15:9f32f64eee5b 41 void PI_Cntrl::reset(float initValue)
altb2 15:9f32f64eee5b 42 {
altb2 15:9f32f64eee5b 43 s = (double)initValue;
altb2 15:9f32f64eee5b 44 }
altb2 15:9f32f64eee5b 45
altb2 15:9f32f64eee5b 46 void PI_Cntrl::setCoefficients(float Kp, float Tn, float Ts)
altb2 15:9f32f64eee5b 47 {
altb2 15:9f32f64eee5b 48 b0 = (double)Kp*(1.0 + (double)Ts/(double)Tn);
altb2 15:9f32f64eee5b 49 b1 = -(double)Kp;
altb2 15:9f32f64eee5b 50 b2 = (double)Ts/(double)Tn;
altb2 15:9f32f64eee5b 51 }
altb2 15:9f32f64eee5b 52
altb2 15:9f32f64eee5b 53 float PI_Cntrl::doStep(double e)
altb2 15:9f32f64eee5b 54 {
altb2 15:9f32f64eee5b 55 double u = b0*e + s; // unconstrained output
altb2 15:9f32f64eee5b 56 double uc = u; // constrained output
altb2 15:9f32f64eee5b 57 if(u > uMax) uc = uMax;
altb2 15:9f32f64eee5b 58 else if(u < uMin) uc = uMin;
altb2 15:9f32f64eee5b 59 s = b1*e + u - b2*(u - uc);
altb2 15:9f32f64eee5b 60 return (float)uc;
altb2 15:9f32f64eee5b 61 }