Ruprecht Altenburger / Mbed OS mirror_actuator_V1

Mirror actuator for RT2 lab

Dependencies:   FastPWM

Library_Cntrl/PID_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 Ts z - 1
altb2 15:9f32f64eee5b 7 G(s) = P + I ------- + D ------- D corresponds Kd/Tf in Matlab-formlism pid(...)
altb2 15:9f32f64eee5b 8 z - 1 z - p
altb2 15:9f32f64eee5b 9 */
altb2 15:9f32f64eee5b 10
altb2 15:9f32f64eee5b 11 #include "PID_Cntrl.h"
altb2 15:9f32f64eee5b 12 using namespace std;
altb2 15:9f32f64eee5b 13
altb2 15:9f32f64eee5b 14 PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMin, float uMax)
altb2 15:9f32f64eee5b 15 {
altb2 15:9f32f64eee5b 16 setCoefficients(P, I, D, tau_f, Ts);
altb2 15:9f32f64eee5b 17 this->uMin = (double)uMin;
altb2 15:9f32f64eee5b 18 this->uMax = (double)uMax;
altb2 15:9f32f64eee5b 19 reset(0.0f);
altb2 15:9f32f64eee5b 20 }
altb2 15:9f32f64eee5b 21
altb2 15:9f32f64eee5b 22 PID_Cntrl::~PID_Cntrl() {}
altb2 15:9f32f64eee5b 23
altb2 15:9f32f64eee5b 24 void PID_Cntrl::reset(float initValue)
altb2 15:9f32f64eee5b 25 {
altb2 15:9f32f64eee5b 26 Iold = (double)initValue;
altb2 15:9f32f64eee5b 27 eold = 0.0;yold = 0.0;
altb2 15:9f32f64eee5b 28 del = 0.0;
altb2 15:9f32f64eee5b 29 }
altb2 15:9f32f64eee5b 30
altb2 15:9f32f64eee5b 31 void PID_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float Ts)
altb2 15:9f32f64eee5b 32 {
altb2 15:9f32f64eee5b 33 this->p = 1.0 - (double)Ts/(double)tau_f;
altb2 15:9f32f64eee5b 34 this->P = P;
altb2 15:9f32f64eee5b 35 this->I = I;
altb2 15:9f32f64eee5b 36 this->D = D;
altb2 15:9f32f64eee5b 37 this->Ts = Ts;
altb2 15:9f32f64eee5b 38 if(P!=0)
altb2 15:9f32f64eee5b 39 this->Ka=1/P;
altb2 15:9f32f64eee5b 40 else
altb2 15:9f32f64eee5b 41 this->Ka=1.0f;
altb2 15:9f32f64eee5b 42
altb2 15:9f32f64eee5b 43 }
altb2 15:9f32f64eee5b 44
altb2 15:9f32f64eee5b 45 float PID_Cntrl::doStep(double e)
altb2 15:9f32f64eee5b 46 {
altb2 15:9f32f64eee5b 47 double Ipart = Iold+I*Ts*(e-del);
altb2 15:9f32f64eee5b 48 double Dpart = D*(e-eold)+p*yold;
altb2 15:9f32f64eee5b 49 double u = P*e + Dpart + Ipart; // unconstrained output
altb2 15:9f32f64eee5b 50 double uc = u; // constrained output
altb2 15:9f32f64eee5b 51 if(u > uMax) uc = uMax;
altb2 15:9f32f64eee5b 52 else if(u < uMin) uc = uMin;
altb2 15:9f32f64eee5b 53 del=(u-uc)*Ka;
altb2 15:9f32f64eee5b 54 eold=e;
altb2 15:9f32f64eee5b 55 Iold=Ipart;
altb2 15:9f32f64eee5b 56 yold=Dpart;
altb2 15:9f32f64eee5b 57 return (float)uc;
altb2 15:9f32f64eee5b 58 }
altb2 15:9f32f64eee5b 59
altb2 15:9f32f64eee5b 60 void PID_Cntrl::set_limits(double ll, double ul)
altb2 15:9f32f64eee5b 61 {
altb2 15:9f32f64eee5b 62 this->uMin = (double)ll;
altb2 15:9f32f64eee5b 63 this->uMax = (double)ul;
altb2 15:9f32f64eee5b 64 }
altb2 15:9f32f64eee5b 65
altb2 15:9f32f64eee5b 66 float PID_Cntrl::get_ulimit(void)
altb2 15:9f32f64eee5b 67 {
altb2 15:9f32f64eee5b 68 return (float)this->uMax;
altb2 15:9f32f64eee5b 69 }