Diff: Headers/PID.h

Revision:

7:95ebadc83fc7

Parent:

6:6e96e93689df

--- a/Headers/PID.h	Thu Apr 03 19:21:58 2014 +0000
+++ b/Headers/PID.h	Thu Apr 03 23:57:31 2014 +0000
@@ -7,7 +7,7 @@
 
 
 #include "Sensors.h"
-
+#include "Motors.h"
 
 
 float proportional = 0;
@@ -32,5 +32,33 @@
     return (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM)); 
 }
 
-
+void pid()
+{
+ 
+        if ( wallRight() && wallLeft())
+        PIDv = PID(linearize(SenseR.read()), linearize(SenseL.read()));
+        if ( wallRight() && !wallLeft())
+        PIDv = PID(linearize(SenseR.read()),cal_R);
+        if ( !wallRight() && wallLeft())
+        PIDv = PID(cal_L, linearize(SenseL.read()));
+        
+        if(PIDv < -0.1)
+        {
+            setRightSpeed(3);
+            setLeftSpeed(4);
+            wait_ms(20); 
+        }
+        else if(PIDv > .1)
+        {
+            setRightSpeed(4); //** Just flipped these values
+            setLeftSpeed(3);
+            wait_ms(20);        
+        }
+        else
+        {
+            setRightSpeed(3);
+            setLeftSpeed(3);
+        } 
+    
+}
 #endif
\ No newline at end of file