Diff: PID/PID.cpp

Revision:

3:27407c4984cf

Parent:

2:59ac9df97701

--- a/PID/PID.cpp	Fri Nov 15 20:53:36 2013 +0000
+++ b/PID/PID.cpp	Thu Feb 13 16:07:07 2014 +0000
@@ -5,7 +5,7 @@
 PID::PID()
 {   
     interval = 0.05;
-    integral_limit = 50;
+    differential_limit = 200;
     pid_limit = 300;
     kp = 0.5;
     ki = 0.5;
@@ -14,11 +14,11 @@
     old = 0;
 }
 
-void PID::init(float KP,float KI,float KD,float PID_LIM,float INTEGRAL_LIM)
+void PID::init(float KP,float KI,float KD,float PID_LIM,float DIFFERENTIAL_LIM)
 {   
 //    interval = INTERVAL;
     pid_limit = PID_LIM;
-    integral_limit = INTEGRAL_LIM;
+    differential_limit = DIFFERENTIAL_LIM;
     kp = KP;
     ki = KI;
     kd = KD;
@@ -28,13 +28,17 @@
 
 float PID::calc(float value,float target,float interval) 
 {
+    float differential;
     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 ( integral < -differential_limit ) integral = -differential_limit;
+//    if ( integral > integral_limit ) integral = integral_limit;
+    differential = ( kd * ( cur - old ) / interval );
+    if ( differential < -differential_limit ) differential = -differential_limit;
+    if ( differential > differential_limit ) differential = differential_limit;
+    float pid = ( kp * cur ) + ( ki * integral ) + differential;
     if ( pid < -pid_limit ) pid = -pid_limit;
     if ( pid > pid_limit ) pid = pid_limit;
     old = cur;