Diff: PI_Cntrl.cpp

Revision:

0:15be70d21d7c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PI_Cntrl.cpp	Wed Jan 10 16:08:07 2018 +0000
@@ -0,0 +1,47 @@
+#include "PI_Cntrl.h"
+
+using namespace std;
+
+PI_Cntrl::PI_Cntrl(float Kp_, float Tn_){
+    Kp = Kp_;
+    Tn = Tn_;
+    i_part_old = 0;
+    del = 0;
+    out_max =1e10;
+    out_min=-1e10;
+    }
+
+PI_Cntrl::PI_Cntrl(float Kp_, float Tn_,float ma){
+    Kp = Kp_;
+    Tn = Tn_;
+    i_part_old = 0;
+    del = 0;
+    out_max = ma;
+    out_min = -ma;
+    }
+
+PI_Cntrl::~PI_Cntrl() {} 
+    
+void PI_Cntrl::reset(float initValue) {
+    i_part_old = initValue;
+    del = 0;
+}
+
+float PI_Cntrl::doStep(float error){
+    float kpe = Kp * error;
+    float i_part = 1.0f/Tn * Ts * (kpe-del) + i_part_old;  // I with windup subtraction
+    float y_out_temp = (kpe + i_part_old );               // temporary variable
+    float y_out;
+    // -------- limit output --------
+    if(y_out_temp > out_max)
+        y_out = out_max;
+    else if(y_out_temp < out_min)
+        y_out = out_min;
+    else
+        y_out = y_out_temp;
+    // -------- Anti-Windup -------- 
+    del = (y_out_temp - y_out);
+    // -------- Timeshift --------
+    i_part_old = i_part;
+    return y_out;
+    }
\ No newline at end of file