Ruprecht Altenburger / Mbed OS mirror_actuator_stud

template for students for mirror actuator

Dependencies:   FastPWM

Lib_Cntrl/PID_Cntrl.cpp@1:a7fc1afe0575, 2021-05-17 (annotated)

Committer:
altb2
Date:
Mon May 17 12:03:40 2021 +0000
Revision:
1:a7fc1afe0575
Parent:
0:d2e117716219
Child:
2:c4c4cc1bff45
Implemented PI-speed controller (1st Lab finished)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 0:d2e117716219 1 /*
altb2 0:d2e117716219 2 */
altb2 0:d2e117716219 3
altb2 0:d2e117716219 4 #include "PID_Cntrl.h"
altb2 0:d2e117716219 5
altb2 1:a7fc1afe0575 6 // Matlab
altb2 1:a7fc1afe0575 7 // Tn = .005;
altb2 1:a7fc1afe0575 8 // Gpi= tf([Tn 1],[Tn 0]);
altb2 1:a7fc1afe0575 9 // Kp = 0.0158;
altb2 1:a7fc1afe0575 10 // pid(Kp*Gpi);
altb2 1:a7fc1afe0575 11
altb2 0:d2e117716219 12 PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMin, float uMax)
altb2 0:d2e117716219 13 {
altb2 0:d2e117716219 14 // ------------------
altb2 1:a7fc1afe0575 15 this->P = P;
altb2 1:a7fc1afe0575 16 this->I = I;
altb2 1:a7fc1afe0575 17 this->Ts = Ts;
altb2 1:a7fc1afe0575 18 this->uMin = uMin;
altb2 1:a7fc1afe0575 19 this->uMax = uMax;
altb2 1:a7fc1afe0575 20 reset(0);
altb2 0:d2e117716219 21 }
altb2 0:d2e117716219 22
altb2 0:d2e117716219 23 PID_Cntrl::~PID_Cntrl() {}
altb2 0:d2e117716219 24
altb2 0:d2e117716219 25 void PID_Cntrl::reset(float initValue)
altb2 0:d2e117716219 26 {
altb2 0:d2e117716219 27 // -----------------------
altb2 1:a7fc1afe0575 28 Ipart = initValue;
altb2 0:d2e117716219 29 }
altb2 0:d2e117716219 30
altb2 0:d2e117716219 31
altb2 0:d2e117716219 32 float PID_Cntrl::update(float e)
altb2 0:d2e117716219 33 {
altb2 0:d2e117716219 34 // the main update
altb2 1:a7fc1afe0575 35
altb2 1:a7fc1afe0575 36 Ipart += I * Ts * e;
altb2 1:a7fc1afe0575 37 Ipart = saturate(Ipart);
altb2 1:a7fc1afe0575 38 return saturate(P * e + Ipart);
altb2 1:a7fc1afe0575 39 }
altb2 1:a7fc1afe0575 40
altb2 1:a7fc1afe0575 41 float PID_Cntrl::saturate(float x)
altb2 1:a7fc1afe0575 42 {
altb2 1:a7fc1afe0575 43 if(x > uMax)
altb2 1:a7fc1afe0575 44 return uMax;
altb2 1:a7fc1afe0575 45 else if(x < uMin)
altb2 1:a7fc1afe0575 46 return uMin;
altb2 1:a7fc1afe0575 47 return x;
altb2 0:d2e117716219 48 }