Diff: Main.cpp

Revision:

4:ecfe2115e9a4

Parent:

3:1a8a7cc709cc

Child:

5:9e504a5a1f48

--- a/Main.cpp	Thu Apr 03 15:52:54 2014 +0000
+++ b/Main.cpp	Thu Apr 03 17:34:00 2014 +0000
@@ -82,6 +82,7 @@
         leftExpAvg.add(l);
         frontExpAvg.add(f);
         */
+        leftExpAvg.add(l);
         rightExpAvg.add(r);
         frontExpAvg.add(f);
         //leftBufferAvg.add(l);
@@ -90,8 +91,10 @@
         
         WIRELESS.printf("%i || F: %f \n\r", i, frontExpAvg.average());
     }
-    
-    float avgRight = rightExpAvg.average();
+        cal_R = abs(log(1-rightExpAvg.average()));
+        cal_L = abs(log(1-leftExpAvg.average()));
+        //cal_F = abs(log(1-frontExpAvg.average()));
+        //float avgRight = rightExpAvg.average();
     
     
     setRightSpeed(3);
@@ -100,25 +103,25 @@
     
     //while (true)
     while(SenseF.read() < 0.99)
+    //while(frontExpAvg.average() < .90 ) //&& wallRight() && wallLeft()
     { 
-//        frontExpAvg.add(SenseF.read());
+        frontExpAvg.add(SenseF.read());
     
-        //WIRELESS.printf("R: %f  L: %f  F: %f\n\r", SenseR.read(), SenseL.read(), SenseF.read());
-        
-        float PIDv = PID(SenseR.read(), SenseL.read());
+        //WIRELESS.printf("R: %f  L: %f\n\r", cal_R, cal_L);
+        float PIDv = PID(abs(log(1-SenseR.read())), abs(log(1-SenseL.read())));
         //WIRELESS.printf("ERR: %f  Front: %f\n\r", PIDv, SenseF.read());
         
         if(PIDv < -0.1)
         {
-            setRightSpeed(4);
-            setLeftSpeed(3);
+            setRightSpeed(3);
+            setLeftSpeed(4);
             wait_ms(20); 
               
         }
         else if(PIDv > .1)
         {
-            setRightSpeed(3);
-            setLeftSpeed(4);
+            setRightSpeed(4); //** Just flipped these values
+            setLeftSpeed(3);
             wait_ms(20);        
         }
         else