PT1_Controller.cpp

00001 #include "PT1_Controller.h"
00002 
00003 PT1_Controller::PT1_Controller(double Kp, double Ts, double Tf)
00004 {
00005     copyControllerPara(Kp, Ts, Tf);
00006     calcFilterCoeff();
00007     u_max = 1000000.0; // big number
00008     u_min = -u_max;
00009 }
00010 
00011 PT1_Controller::PT1_Controller(double Kp, double Ts, double Tf, double u_max)
00012 {
00013     copyControllerPara(Kp, Ts, Tf);
00014     calcFilterCoeff();
00015     this->u_max = u_max;
00016     u_min = -u_max;
00017 }
00018 
00019 PT1_Controller::PT1_Controller(double Kp, double Ts, double Tf, double u_max, double u_min)
00020 {
00021     copyControllerPara(Kp, Ts, Tf);
00022     calcFilterCoeff();
00023     this->u_max = u_max;
00024     this->u_min = u_min;
00025 }
00026 
00027 PT1_Controller::~PT1_Controller() {}
00028 
00029 void PT1_Controller::reset()
00030 {
00031     u_kmin1 = 0.0;
00032     uf_k = 0.0;
00033 }
00034 
00035 void PT1_Controller::reset(double setVal)
00036 {
00037     u_kmin1 = setVal;
00038     uf_k = setVal;
00039 }
00040 
00041 double PT1_Controller::update(double e)
00042 {
00043     // proportional controller
00044     double u = Kp * e;
00045 
00046     // constrained low-pass filter
00047     uf_k = u*bf + u_kmin1*bf - af*uf_k;
00048     u_kmin1 = u;
00049     return saturate(uf_k);
00050 }
00051 
00052 void PT1_Controller::copyControllerPara(double Kp, double Ts, double Tf)
00053 {
00054     this->Kp = Kp;
00055     this->Ts = Ts;
00056     this->Tf = Tf;
00057 }
00058 
00059 void PT1_Controller::calcFilterCoeff()
00060 {
00061     // tustin transformation
00062     bf = Ts / (2.0*Tf + Ts);
00063     af = (Ts - 2.0*Tf) / (2.0*Tf + Ts);
00064 }
00065 
00066 double PT1_Controller::saturate(double u)
00067 {
00068     if(u > u_max) {
00069         u = u_max;
00070     } else if(u < u_min) {
00071         u = u_min;
00072     }
00073     return u;
00074 }
00075