Tobias Vögeli / Mbed 2 deprecated GRT_VC_PIDT1

pr7

Dependencies:   mbed

PID_Cntrl.cpp@2:1ded9d10f322, 2019-05-14 (annotated)

Committer:
voegetob
Date:
Tue May 14 15:29:24 2019 +0000
Revision:
2:1ded9d10f322
Parent:
1:92f175969d90 
pr7

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 1:92f175969d90 1 /*
pmic 1:92f175969d90 2 PID-T1 Controller class
pmic 1:92f175969d90 3
pmic 1:92f175969d90 4 1 s
pmic 1:92f175969d90 5 G(s) = Kp + Ki --- + Kd ---------
pmic 1:92f175969d90 6 s T_f*s + p
pmic 1:92f175969d90 7
pmic 1:92f175969d90 8 Eigther reseting the Nucleo via the black button or save a new software on
pmic 1:92f175969d90 9 the Nucleo sets the analog output to zero. Zero is equal to -4 Ampere!!!
pmic 1:92f175969d90 10 Therefor: NEVER !!! reset or save a new software while the VC is powered on
pmic 1:92f175969d90 11 (the green button on the VC is glowing green)
pmic 1:92f175969d90 12
altb2 0:05dd1de8cc3f 13 */
altb2 0:05dd1de8cc3f 14
altb2 0:05dd1de8cc3f 15 #include "PID_Cntrl.h"
altb2 0:05dd1de8cc3f 16 using namespace std;
altb2 0:05dd1de8cc3f 17
pmic 1:92f175969d90 18 PID_Cntrl::PID_Cntrl(float Kp, float Ki, float Kd, float Tf, float Ts, float uMin, float uMax)
altb2 0:05dd1de8cc3f 19 {
pmic 1:92f175969d90 20 // link member variables
voegetob 2:1ded9d10f322 21 this->Kp = Kp;
voegetob 2:1ded9d10f322 22 this->Ki = Ki;
voegetob 2:1ded9d10f322 23 this->Kd = Kd;
voegetob 2:1ded9d10f322 24
voegetob 2:1ded9d10f322 25 this->Tf = Tf;
voegetob 2:1ded9d10f322 26 this->Ts = Ts;
voegetob 2:1ded9d10f322 27
voegetob 2:1ded9d10f322 28 this->uMin = uMin;
voegetob 2:1ded9d10f322 29 this->uMax = uMax;
pmic 1:92f175969d90 30
altb2 0:05dd1de8cc3f 31 reset(0.0f);
altb2 0:05dd1de8cc3f 32 }
altb2 0:05dd1de8cc3f 33
altb2 0:05dd1de8cc3f 34 PID_Cntrl::~PID_Cntrl() {}
altb2 0:05dd1de8cc3f 35
altb2 0:05dd1de8cc3f 36 void PID_Cntrl::reset(float initValue)
altb2 0:05dd1de8cc3f 37 {
pmic 1:92f175969d90 38
pmic 1:92f175969d90 39 // implement controller reset
voegetob 2:1ded9d10f322 40 e_old = 0.0f;
voegetob 2:1ded9d10f322 41 uI_old = initValue;
voegetob 2:1ded9d10f322 42 uD_old = 0.0f;
altb2 0:05dd1de8cc3f 43
altb2 0:05dd1de8cc3f 44 }
altb2 0:05dd1de8cc3f 45
altb2 0:05dd1de8cc3f 46 float PID_Cntrl::update(double e)
altb2 0:05dd1de8cc3f 47 {
pmic 1:92f175969d90 48
pmic 1:92f175969d90 49 // controller update function
pmic 1:92f175969d90 50
pmic 1:92f175969d90 51 // calculate uI
voegetob 2:1ded9d10f322 52 float uI = Ki * Ts * e + uI_old;
altb2 0:05dd1de8cc3f 53
pmic 1:92f175969d90 54 // saturate uI, uMin <= uI <= uMax (anti-windup for the integrator part)
voegetob 2:1ded9d10f322 55 if (uI > uMax)
voegetob 2:1ded9d10f322 56 uI = uMax;
voegetob 2:1ded9d10f322 57 else if (uI < uMin)
voegetob 2:1ded9d10f322 58 uI = uMin;
pmic 1:92f175969d90 59
pmic 1:92f175969d90 60 // calculate uD
voegetob 2:1ded9d10f322 61 float uD = (Kd * (e - e_old) + Tf * uD_old) / (Tf + Ts);
pmic 1:92f175969d90 62
pmic 1:92f175969d90 63 // calculate u
voegetob 2:1ded9d10f322 64 float u = (Kp * e) + uI + uD;
pmic 1:92f175969d90 65
pmic 1:92f175969d90 66 // saturate u, uMin <= u <= uMax
voegetob 2:1ded9d10f322 67 if (u > uMax)
voegetob 2:1ded9d10f322 68 u = uMax;
voegetob 2:1ded9d10f322 69 else if (u < uMin)
voegetob 2:1ded9d10f322 70 u = uMin;
pmic 1:92f175969d90 71
pmic 1:92f175969d90 72 // update signal storage
voegetob 2:1ded9d10f322 73 e_old = e;
voegetob 2:1ded9d10f322 74 uI_old = uI;
voegetob 2:1ded9d10f322 75 uD_old = uD;
pmic 1:92f175969d90 76
voegetob 2:1ded9d10f322 77 return u;
altb2 0:05dd1de8cc3f 78 }
altb2 0:05dd1de8cc3f 79