PID_Cntrl.cpp

00001 /*
00002     PID-T1 Controller class
00003 
00004                       1           s
00005       G(s) = Kp + Ki --- + Kd ---------
00006                       s       T_f*s + p
00007 
00008     Eigther reseting the Nucleo via the black button or save a new software on 
00009     the Nucleo sets the analog output to zero. Zero is equal to -4 Ampere!!!
00010     Therefor: NEVER !!! reset or save a new software while the VC is powered on
00011     (the green button on the VC is glowing green)                      
00012 
00013 */
00014 
00015 #include "PID_Cntrl.h"
00016 using namespace std;
00017 
00018 PID_Cntrl::PID_Cntrl(float Kp, float Ki, float Kd, float Tf, float Ts, float uMin, float uMax)
00019 {
00020     // link member variables
00021     this->Kp = Kp;
00022     this->Ki = Ki;
00023     this->Kd = Kd;
00024     
00025     this->Tf = Tf;
00026     this->Ts = Ts;
00027     
00028     this->uMin = uMin;
00029     this->uMax = uMax;
00030     
00031     reset(0.0f);
00032 }
00033 
00034 PID_Cntrl::~PID_Cntrl() {}
00035 
00036 void PID_Cntrl::reset(float initValue)
00037 {
00038 
00039     // implement controller reset
00040     e_old = 0.0f;
00041     uI_old = initValue;
00042     uD_old = 0.0f;
00043 
00044 }
00045 
00046 float PID_Cntrl::update(double e)
00047 {
00048 
00049     // controller update function 
00050     
00051     // calculate uI
00052     float uI = Ki * Ts * e + uI_old;
00053     
00054     // saturate uI, uMin <= uI <= uMax (anti-windup for the integrator part)
00055     if (uI > uMax)
00056         uI = uMax;
00057     else if (uI < uMin)
00058         uI = uMin;
00059 
00060     // calculate uD
00061     float uD = (Kd * (e - e_old) + Tf * uD_old) / (Tf + Ts);
00062     
00063     // calculate u
00064     float u = (Kp * e) + uI + uD;
00065     
00066     // saturate u, uMin <= u <= uMax
00067     if (u > uMax)
00068         u = uMax;
00069     else if (u < uMin)
00070         u = uMin;
00071     
00072     // update signal storage
00073     e_old = e;
00074     uI_old = uI;
00075     uD_old = uD;
00076 
00077     return u;
00078 }
00079