Diff: pidcontrol.cpp

Revision:

1:45f1573d65a1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pidcontrol.cpp	Mon Mar 21 20:21:12 2016 +0000
@@ -0,0 +1,168 @@
+#include "pidcontrol.h"
+
+PID::PID()
+{
+    Kp=1.0f;
+    Ki=0.0f;
+    Kd=0.0f;
+    il=65535.0;
+    margin = 0.0f;
+
+}
+
+PID::PID(float p,float i,float d)
+{
+    Kp=p;
+    Ki=i;
+    Kd=d;
+    il=65535.0;
+    margin =0.0f;
+}
+
+void PID::setGoal(float ref)
+{
+    setpoint = ref;
+}
+
+void PID::setCurrent(float sensor)
+{
+    input = sensor;
+}
+
+float PID::compute()
+{
+
+    e_n = setpoint - input;
+
+    if((e_i < il) && (e_i > -il))
+    {
+        e_i += e_n;
+    }
+    else
+    {
+#ifdef PID_DEBUG
+        printf("il overflow\n\r");
+#endif
+        e_i =il;
+    }
+
+
+    output = (Kp*e_n)+(Ki*e_i)+(Kd*(e_n-e_n_1));
+
+    if(output > 0)
+    {
+        if(output < margin)
+        {
+            output = 0.0;
+        }
+    }
+    else
+    {
+        if(output > -margin)
+        {
+            output = 0.0;
+        }
+    }
+
+    return output;
+}
+
+void PID::setMargin(float gap)
+{
+    margin =gap;
+}
+
+float PID::getMargin()
+{
+    return margin;
+}
+
+
+void PID::setIntegalLimit(float limit)
+{
+    il = limit;
+}
+float PID::getIntegalLimit()
+{
+    return il;
+}
+
+
+float PID::getErrorNow()
+{
+    return e_n;
+}
+
+float PID::getErrorLast()
+{
+    return e_n_1;
+}
+
+float PID::getErrorDiff()
+{
+    return e_n - e_n_1;
+}
+
+float PID::getErrorIntegal()
+{
+    return e_i;
+}
+
+void PID::setKp(float p)
+{
+    if(p < 0.0f)
+    {
+#ifdef PID_DEBUG
+        printf("Kp = 0.0\n\r");
+#endif
+        Kp=0.0;
+    }
+    else
+    {
+        Kp=p;
+    }
+}
+
+void PID::setKi(float i)
+{
+    if(i < 0.0f)
+    {
+#ifdef PID_DEBUG
+        printf("Ki = 0.0\n\r");
+#endif
+        Ki=0.0;
+    }
+    else
+    {
+        Ki=i;
+    }
+}
+void PID::setKd(float d)
+{
+    if(d < 0.0f)
+    {
+#ifdef PID_DEBUG
+        printf("Kd = 0.0\n\r");
+#endif
+        Kd=0.0;
+    }
+    else
+    {
+        Kd=d;
+    }
+}
+
+float PID::getKp()
+{
+    return Kp;    
+}
+
+float PID::getKi()
+{   
+    return Ki;    
+}
+
+float PID::getKd()
+{   
+    return Kd;    
+}