Boulus Abu Joudom / Mbed 2 deprecated Cube_Mini_Solution

Cube Mini Solution

Dependencies:   mbed QEI MPU6050 BLE_API nRF51822 MCP4725 eMPL_MPU6050

PID_Cntrl.cpp@24:c953b74ed88b, 2020-08-28 (annotated)

Committer:
BoulusAJ
Date:
Fri Aug 28 09:23:40 2020 +0000
Revision:
24:c953b74ed88b
Parent:
20:b142ae11a12a 
Prototype 2 - August 28, 2020.; Controller working well with updated parameters; State machine still needs updates

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BoulusAJ 20:b142ae11a12a 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 20:b142ae11a12a 5
BoulusAJ 20:b142ae11a12a 6 Tn*s + 1
BoulusAJ 0:8e87cdf07037 7 G(s) = Kp ------------- with s ~ (1 - z^-1)/Ts
BoulusAJ 20:b142ae11a12a 8 Ts*s
BoulusAJ 20:b142ae11a12a 9
BoulusAJ 0:8e87cdf07037 10 #include "PI_Cntrl.h"
BoulusAJ 20:b142ae11a12a 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 20:b142ae11a12a 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 20:b142ae11a12a 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 20:b142ae11a12a 44
BoulusAJ 20:b142ae11a12a 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 20:b142ae11a12a 51 float 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 20:b142ae11a12a 64 return I_old+I_new;
BoulusAJ 20:b142ae11a12a 65
BoulusAJ 0:8e87cdf07037 66 }
BoulusAJ 0:8e87cdf07037 67
BoulusAJ 20:b142ae11a12a 68 float PID_Cntrl::update(double e) // WITHOUT D_PART
BoulusAJ 0:8e87cdf07037 69 {
BoulusAJ 20:b142ae11a12a 70
BoulusAJ 0:8e87cdf07037 71 // Controller Input Value --> e
BoulusAJ 20:b142ae11a12a 72
BoulusAJ 20:b142ae11a12a 73 // controller update function
BoulusAJ 20:b142ae11a12a 74
BoulusAJ 20:b142ae11a12a 75 // Delta Error (for D-Part)
BoulusAJ 20:b142ae11a12a 76 //delta_error = e - e_old;
BoulusAJ 20:b142ae11a12a 77
BoulusAJ 20:b142ae11a12a 78 // calculate u
BoulusAJ 20:b142ae11a12a 79 P_new = Kp_ * e;
BoulusAJ 0:8e87cdf07037 80
BoulusAJ 20:b142ae11a12a 81 // calculate I-Part Output
BoulusAJ 20:b142ae11a12a 82 I_new = I_old + Ts_ * Ki_ * e_old;
BoulusAJ 0:8e87cdf07037 83
BoulusAJ 20:b142ae11a12a 84 //
BoulusAJ 20:b142ae11a12a 85 float PID_output_temp = P_new + I_new;
BoulusAJ 20:b142ae11a12a 86
BoulusAJ 20:b142ae11a12a 87
BoulusAJ 0:8e87cdf07037 88 // saturate uI, uMin <= uI <= uMax (anti-windup for the integrator part)
BoulusAJ 20:b142ae11a12a 89 if(PID_output_temp > uMax_) {
BoulusAJ 20:b142ae11a12a 90 PID_output = uMax_;
BoulusAJ 20:b142ae11a12a 91 }
BoulusAJ 20:b142ae11a12a 92 else if(PID_output_temp < uMin_) {
BoulusAJ 20:b142ae11a12a 93 PID_output = uMin_;
BoulusAJ 0:8e87cdf07037 94 }
BoulusAJ 20:b142ae11a12a 95 else{
BoulusAJ 20:b142ae11a12a 96 I_old = I_new;
BoulusAJ 20:b142ae11a12a 97 PID_output = PID_output_temp;
BoulusAJ 0:8e87cdf07037 98 }
BoulusAJ 20:b142ae11a12a 99
BoulusAJ 0:8e87cdf07037 100 // calculate uD
BoulusAJ 0:8e87cdf07037 101 //D_new = (Kd_*delta_error + Tf_*D_old - Ts_*D_old)/(Tf_);
BoulusAJ 20:b142ae11a12a 102 //D_new = 0;
BoulusAJ 20:b142ae11a12a 103
BoulusAJ 20:b142ae11a12a 104
BoulusAJ 20:b142ae11a12a 105
BoulusAJ 20:b142ae11a12a 106
BoulusAJ 0:8e87cdf07037 107 // update signal storage
BoulusAJ 20:b142ae11a12a 108 //P_old = P_new;
BoulusAJ 20:b142ae11a12a 109 //I_old = I_new;
BoulusAJ 20:b142ae11a12a 110 //D_old = D_new;
BoulusAJ 20:b142ae11a12a 111 //e_old = e;
BoulusAJ 20:b142ae11a12a 112
BoulusAJ 0:8e87cdf07037 113 // PID Output
BoulusAJ 20:b142ae11a12a 114 //PID_output = P_new + I_new + D_new;
BoulusAJ 20:b142ae11a12a 115
BoulusAJ 0:8e87cdf07037 116 // saturate u, uMin <= u <= uMax
BoulusAJ 20:b142ae11a12a 117 //if(PID_output > uMax_) {
BoulusAJ 20:b142ae11a12a 118 // PID_output = uMax_;
BoulusAJ 20:b142ae11a12a 119 //}
BoulusAJ 20:b142ae11a12a 120 //if(PID_output < uMin_) {
BoulusAJ 20:b142ae11a12a 121 // PID_output = uMin_;
BoulusAJ 20:b142ae11a12a 122 //}
BoulusAJ 20:b142ae11a12a 123
BoulusAJ 0:8e87cdf07037 124 return PID_output;
BoulusAJ 0:8e87cdf07037 125 }
BoulusAJ 0:8e87cdf07037 126