cuboid strong
Dependencies: mbed
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 }
Generated on Fri Jul 15 2022 09:11:08 by
1.7.2