Mike Panetta / Mbed 2 deprecated BeaconAvoid

BeaconAvoid code for AHRC competition.

Dependencies:   MODSERIAL PiSlingers m3pi mbed

Diff: PID.cpp

Revision:
3:aa1a77b080eb
Parent:
0:9ac4a91b71fa
--- a/PID.cpp	Fri Nov 02 03:00:54 2012 +0000
+++ b/PID.cpp	Sat Nov 03 01:03:33 2012 +0000
@@ -67,13 +67,19 @@
     error = in;
     
     v_p  = error;
-    v_i += v_p;
+    
+    if (k_i != 0)
+        v_i += v_p;
+    else
+        v_i = 0;
+        
     v_d  = error - prev_error;    
     
     if (v_i > clip)
         v_i = clip;
-    if (v_i < clip)
+    if (v_i < -clip)
         v_i = -clip;
+        
     if (v_i != v_i) // NAN check...
         v_i = 0;