Diff: PID.cpp

Revision:

0:1f4d5c5491b8

Child:

1:d3e12393b71d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.cpp	Mon Oct 24 16:10:48 2016 +0000
@@ -0,0 +1,36 @@
+#include "PID.h"
+
+PIDClass::PIDClass(int _Kr,int _Ki, int _Kd,int _SetPoint):Kr(_Kr),Ki(_Ki),Kd(_Kd),SetPoint(_SetPoint)
+{
+    /*Kr = _Kr;
+    Ki = _Ki;
+    Kd = _Kd;*/
+}
+
+
+float PIDClass::ComputeCommand(float inputADC)
+{
+    static PID_static param;
+    static float inter = Ki*Kd;
+    static float Kc = sqrt(inter);
+    float outputPWM = 0;
+    float error = 0;
+    
+    error = SetPoint - inputADC;
+    param.integral = param.integral + Ki*Te*error;
+    outputPWM = Kr*error + param.integral + Kd*(error - param.last_error)/Te;
+    param.last_error = error;
+    
+    if(outputPWM > SUPERIOR_MARGIN)
+    {
+        param.integral = param.integral + Kc*(SUPERIOR_MARGIN - outputPWM);
+        outputPWM = SUPERIOR_MARGIN;
+    }else if(outputPWM < INFERIOR_MARGIN)
+    {
+        param.integral = param.integral + Kc*(INFERIOR_MARGIN - outputPWM);
+        outputPWM = 0;   
+    }
+    
+    return outputPWM;
+    
+}