Diff: PID/PID.cpp

Revision:

2:59ac9df97701

Child:

3:27407c4984cf

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID/PID.cpp	Fri Nov 15 20:53:36 2013 +0000
@@ -0,0 +1,52 @@
+#include "mbed.h"
+#include "SerialLcd.h"
+#include "PID.h"
+
+PID::PID()
+{   
+    interval = 0.05;
+    integral_limit = 50;
+    pid_limit = 300;
+    kp = 0.5;
+    ki = 0.5;
+    kd = 0.2;
+    integral = 0;
+    old = 0;
+}
+
+void PID::init(float KP,float KI,float KD,float PID_LIM,float INTEGRAL_LIM)
+{   
+//    interval = INTERVAL;
+    pid_limit = PID_LIM;
+    integral_limit = INTEGRAL_LIM;
+    kp = KP;
+    ki = KI;
+    kd = KD;
+    integral = 0;
+    old = 0;    
+}
+
+float PID::calc(float value,float target,float interval) 
+{
+    cur = value - target;
+//    if ( (old >= 0 && cur <= 0) || (old <= 0 && cur >= 0) || fabsf(cur) < 1 )
+//        integral = 0;
+    integral += ( cur + old ) * interval * 0.5f;
+    if ( integral < -integral_limit ) integral = -integral_limit;
+    if ( integral > integral_limit ) integral = integral_limit;
+    float pid = kp * cur + ki * integral + kd * ( cur - old ) / interval;
+    if ( pid < -pid_limit ) pid = -pid_limit;
+    if ( pid > pid_limit ) pid = pid_limit;
+    old = cur;
+    return pid;
+}
+
+void PID::reset()
+{
+    integral = 0;
+    old = 0;
+}
+;
+
+
+