GRT_FS2019_VoiceCoil / Mbed 2 deprecated Cube_Mini_Solution

Cube_Mini_Solution

Dependencies:   mbed QEI MPU6050 BLE_API nRF51822 MCP4725 eMPL_MPU6050

PID_Cntrl.cpp@18:3b1aa67e11ca, 2020-05-29 (annotated)

Committer:
BoulusAJ
Date:
Fri May 29 15:40:58 2020 +0000
Revision:
18:3b1aa67e11ca
Parent:
16:ff375f62a95f 
Version May 29, 2020

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BoulusAJ 0:8e87cdf07037 1 /*
BoulusAJ 0:8e87cdf07037 2 PI Controller class with anti windup reset in biquad transposed direct form 2
BoulusAJ 0:8e87cdf07037 3 see e.g.: https://www.dsprelated.com/freebooks/filters/Four_Direct_Forms.html
BoulusAJ 0:8e87cdf07037 4 everything is calculated in double
BoulusAJ 0:8e87cdf07037 5
BoulusAJ 0:8e87cdf07037 6 Tn*s + 1
BoulusAJ 0:8e87cdf07037 7 G(s) = Kp ------------- with s ~ (1 - z^-1)/Ts
BoulusAJ 0:8e87cdf07037 8 Ts*s
BoulusAJ 0:8e87cdf07037 9
BoulusAJ 0:8e87cdf07037 10 #include "PI_Cntrl.h"
BoulusAJ 0:8e87cdf07037 11 using namespace std;
BoulusAJ 0:8e87cdf07037 12
BoulusAJ 0:8e87cdf07037 13 */
BoulusAJ 0:8e87cdf07037 14
BoulusAJ 0:8e87cdf07037 15
BoulusAJ 0:8e87cdf07037 16
BoulusAJ 0:8e87cdf07037 17 /*
BoulusAJ 0:8e87cdf07037 18 PID-T1 Controller class
BoulusAJ 0:8e87cdf07037 19
BoulusAJ 0:8e87cdf07037 20 1 s
BoulusAJ 0:8e87cdf07037 21 G(s) = Kp + Ki --- + Kd ---------
BoulusAJ 0:8e87cdf07037 22 s T_f*s + p
BoulusAJ 0:8e87cdf07037 23
BoulusAJ 0:8e87cdf07037 24 Eigther reseting the Nucleo via the black button or save a new software on
BoulusAJ 0:8e87cdf07037 25 the Nucleo sets the analog output to zero. Zero is equal to -4 Ampere!!!
BoulusAJ 0:8e87cdf07037 26 Therefor: NEVER !!! reset or save a new software while the VC is powered on
BoulusAJ 0:8e87cdf07037 27 (the green button on the VC is glowing green)
BoulusAJ 0:8e87cdf07037 28
BoulusAJ 0:8e87cdf07037 29 */
BoulusAJ 0:8e87cdf07037 30
BoulusAJ 0:8e87cdf07037 31 #include "PID_Cntrl.h"
BoulusAJ 0:8e87cdf07037 32 using namespace std;
BoulusAJ 0:8e87cdf07037 33
BoulusAJ 0:8e87cdf07037 34 PID_Cntrl::PID_Cntrl(float Kp, float Ki, float Kd, float Tf, float Ts, float uMin, float uMax)
BoulusAJ 0:8e87cdf07037 35 {
BoulusAJ 0:8e87cdf07037 36 // link member variables
BoulusAJ 0:8e87cdf07037 37 Kp_ = Kp;
BoulusAJ 0:8e87cdf07037 38 Ki_ = Ki;
BoulusAJ 0:8e87cdf07037 39 Kd_ = Kd;
BoulusAJ 0:8e87cdf07037 40 Tf_ = Tf;
BoulusAJ 0:8e87cdf07037 41 Ts_ = Ts;
BoulusAJ 0:8e87cdf07037 42 uMin_ = uMin;
BoulusAJ 0:8e87cdf07037 43 uMax_ = uMax;
BoulusAJ 0:8e87cdf07037 44
BoulusAJ 0:8e87cdf07037 45
BoulusAJ 0:8e87cdf07037 46 reset(0.0f);
BoulusAJ 0:8e87cdf07037 47 }
BoulusAJ 0:8e87cdf07037 48
BoulusAJ 0:8e87cdf07037 49 PID_Cntrl::~PID_Cntrl() {}
BoulusAJ 0:8e87cdf07037 50
BoulusAJ 0:8e87cdf07037 51 void PID_Cntrl::reset(float initValue)
BoulusAJ 0:8e87cdf07037 52 {
BoulusAJ 0:8e87cdf07037 53
BoulusAJ 0:8e87cdf07037 54 // implement controller reset
BoulusAJ 0:8e87cdf07037 55 P_new = 0;
BoulusAJ 0:8e87cdf07037 56 I_new = 0;
BoulusAJ 0:8e87cdf07037 57 D_new = 0;
BoulusAJ 0:8e87cdf07037 58 PID_output = 0;
BoulusAJ 0:8e87cdf07037 59 P_old = 0;
BoulusAJ 0:8e87cdf07037 60 I_old = 0;
BoulusAJ 0:8e87cdf07037 61 D_old = 0;
BoulusAJ 0:8e87cdf07037 62 delta_error = 0;
BoulusAJ 0:8e87cdf07037 63 e_old = 0;
BoulusAJ 0:8e87cdf07037 64
BoulusAJ 0:8e87cdf07037 65 }
BoulusAJ 0:8e87cdf07037 66
BoulusAJ 0:8e87cdf07037 67 float PID_Cntrl::update(double e)
BoulusAJ 0:8e87cdf07037 68 {
BoulusAJ 0:8e87cdf07037 69
BoulusAJ 0:8e87cdf07037 70 // Controller Input Value --> e
BoulusAJ 0:8e87cdf07037 71
BoulusAJ 0:8e87cdf07037 72 // controller update function
BoulusAJ 0:8e87cdf07037 73
BoulusAJ 0:8e87cdf07037 74 // Delta Error (for D-Part)
BoulusAJ 0:8e87cdf07037 75 delta_error = e - e_old;
BoulusAJ 0:8e87cdf07037 76
BoulusAJ 0:8e87cdf07037 77 // calculate I-Part Output
BoulusAJ 16:ff375f62a95f 78 float I_new = I_old + Ts_ * Ki_ * e_old;
BoulusAJ 0:8e87cdf07037 79
BoulusAJ 0:8e87cdf07037 80 // saturate uI, uMin <= uI <= uMax (anti-windup for the integrator part)
BoulusAJ 0:8e87cdf07037 81 if(I_new > uMax_) {
BoulusAJ 0:8e87cdf07037 82 I_new = uMax_;
BoulusAJ 0:8e87cdf07037 83 }
BoulusAJ 0:8e87cdf07037 84 if(I_new < uMin_) {
BoulusAJ 0:8e87cdf07037 85 I_new = uMin_;
BoulusAJ 0:8e87cdf07037 86 }
BoulusAJ 0:8e87cdf07037 87
BoulusAJ 0:8e87cdf07037 88 // calculate uD
BoulusAJ 0:8e87cdf07037 89 //D_new = (Kd_*delta_error + Tf_*D_old - Ts_*D_old)/(Tf_);
BoulusAJ 0:8e87cdf07037 90 D_new = 0;
BoulusAJ 0:8e87cdf07037 91
BoulusAJ 0:8e87cdf07037 92 // calculate u
BoulusAJ 0:8e87cdf07037 93 P_new = Kp_ * e;
BoulusAJ 0:8e87cdf07037 94
BoulusAJ 0:8e87cdf07037 95
BoulusAJ 0:8e87cdf07037 96 // update signal storage
BoulusAJ 0:8e87cdf07037 97 P_old = P_new;
BoulusAJ 0:8e87cdf07037 98 I_old = I_new;
BoulusAJ 0:8e87cdf07037 99 D_old = D_new;
BoulusAJ 0:8e87cdf07037 100 e_old = e;
BoulusAJ 0:8e87cdf07037 101
BoulusAJ 0:8e87cdf07037 102 // PID Output
BoulusAJ 0:8e87cdf07037 103 PID_output = P_new + I_new + D_new;
BoulusAJ 0:8e87cdf07037 104
BoulusAJ 0:8e87cdf07037 105 // saturate u, uMin <= u <= uMax
BoulusAJ 0:8e87cdf07037 106 if(PID_output > uMax_) {
BoulusAJ 0:8e87cdf07037 107 PID_output = uMax_;
BoulusAJ 0:8e87cdf07037 108 }
BoulusAJ 0:8e87cdf07037 109 if(PID_output < uMin_) {
BoulusAJ 0:8e87cdf07037 110 PID_output = uMin_;
BoulusAJ 0:8e87cdf07037 111 }
BoulusAJ 0:8e87cdf07037 112
BoulusAJ 0:8e87cdf07037 113 return PID_output;
BoulusAJ 0:8e87cdf07037 114 }
BoulusAJ 0:8e87cdf07037 115