Diff: Main.cpp

Revision:

5:9e504a5a1f48

Parent:

4:ecfe2115e9a4

Child:

6:6e96e93689df

--- a/Main.cpp	Thu Apr 03 17:34:00 2014 +0000
+++ b/Main.cpp	Thu Apr 03 18:47:30 2014 +0000
@@ -66,6 +66,7 @@
     
     */
 
+    initMapping();
     
     for(int i = 0; i < 100; i++)
     {
@@ -102,13 +103,13 @@
     
     
     //while (true)
-    while(SenseF.read() < 0.99)
-    //while(frontExpAvg.average() < .90 ) //&& wallRight() && wallLeft()
+    //while(SenseF.read() < 0.99)
+    while(frontExpAvg.average() < .8 && wallRight() && wallLeft()) //
     { 
         frontExpAvg.add(SenseF.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())));
+        float PIDv = PID(linearize(SenseR.read()), linearize(SenseL.read()));
         //WIRELESS.printf("ERR: %f  Front: %f\n\r", PIDv, SenseF.read());
         
         if(PIDv < -0.1)
@@ -137,7 +138,9 @@
     
     //stop_mov();
     wait_ms(20);
-    handbrake();   
+    handbrake();  
+    
+    printMap(WIRELESS); 
     return 0;
 }
 void moveNORTH()