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     this->Tf = Tf;
00025     this->Ts = Ts;
00026     this->uMin = uMin;
00027     this->uMax = uMax;
00028     reset(0.0f);
00029 }
00030 
00031 PID_Cntrl::~PID_Cntrl() {}
00032 
00033 void PID_Cntrl::reset(float initValue)
00034 {
00035     // implement controller reset
00036     e_old = 0.0f;
00037     uI_old = initValue;
00038     uD_old = 0.0f;
00039 
00040 }
00041 
00042 float PID_Cntrl::update(double e)
00043 {
00044     // controller update function 
00045     
00046     // calculate uI
00047     float uI = Ki*Ts*e + uI_old;
00048     
00049     // saturate uI, uMin <= uI <= uMax (anti-windup for the integrator part)
00050     if( uI > uMax ) {
00051         uI = uMax;
00052     } else if( uI < uMin ) {
00053         uI = uMin;
00054     }
00055 
00056     // calculate uD
00057     // float uD = (Kd*(e - e_old) + Tf*uD_old)/(Tf+Ts); // sol (euler vorward)
00058     float uD = uD_old*(1.0f - Ts/Tf) + Kd/Tf*(e - e_old); // sol (euler backward)
00059     
00060     // calculate u
00061     float u = Kp*e + uI + uD;
00062     
00063     // saturate u, uMin <= u <= uMax
00064     if( u > uMax ) {
00065         u = uMax;
00066     } else if( u < uMin ) {
00067         u = uMin;
00068     }
00069     
00070     // update signal storage
00071     e_old = e;
00072     uI_old = uI;
00073     uD_old = uD;
00074 
00075     return u;
00076 }
00077