cuboid strong

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers PI_Cntrl.cpp Source File

PI_Cntrl.cpp

00001 #include "PI_Cntrl.h"
00002 
00003 using namespace std;
00004 
00005 PI_Cntrl::PI_Cntrl(float Kp_, float Tn_){
00006     Kp = Kp_;
00007     Tn = Tn_;
00008     i_part_old = 0;
00009     del = 0;
00010     out_max =1e10;
00011     out_min=-1e10;
00012     }
00013 
00014 PI_Cntrl::PI_Cntrl(float Kp_, float Tn_,float ma){
00015     Kp = Kp_;
00016     Tn = Tn_;
00017     i_part_old = 0;
00018     del = 0;
00019     out_max = ma;
00020     out_min = -ma;
00021     }
00022 
00023 PI_Cntrl::~PI_Cntrl() {} 
00024     
00025 void PI_Cntrl::reset(float initValue) {
00026     i_part_old = initValue;
00027     del = 0;
00028 }
00029 
00030 float PI_Cntrl::doStep(float error){
00031     float kpe = Kp * error;
00032     float i_part = 1.0f/Tn * Ts * (kpe-del) + i_part_old;  // I with windup subtraction
00033     float y_out_temp = (kpe + i_part_old );               // temporary variable
00034     float y_out;
00035     // -------- limit output --------
00036     if(y_out_temp > out_max)
00037         y_out = out_max;
00038     else if(y_out_temp < out_min)
00039         y_out = out_min;
00040     else
00041         y_out = y_out_temp;
00042     // -------- Anti-Windup -------- 
00043     del = (y_out_temp - y_out);
00044     // -------- Timeshift --------
00045     i_part_old = i_part;
00046     return y_out;
00047     }