Diff: Main.cpp

Revision:

6:6e96e93689df

Parent:

5:9e504a5a1f48

Child:

7:95ebadc83fc7

--- a/Main.cpp	Thu Apr 03 18:47:30 2014 +0000
+++ b/Main.cpp	Thu Apr 03 19:21:58 2014 +0000
@@ -67,57 +67,31 @@
     */
 
     initMapping();
-    
-    for(int i = 0; i < 100; i++)
-    {
-        float l = SenseL.read();
-        float r = SenseR.read();
-        float f = SenseF.read();
-        
-        /*
-        leftWeightedAvg.add(l);
-        rightWeightedAvg.add(r);
-        frontWeightedAvg.add(f);
-        
+    initSensors();
 
-        leftExpAvg.add(l);
-        frontExpAvg.add(f);
-        */
-        leftExpAvg.add(l);
-        rightExpAvg.add(r);
-        frontExpAvg.add(f);
-        //leftBufferAvg.add(l);
-        //rightBufferAvg.add(r);
-        //frontBufferAvg.add(f);
-        
-        WIRELESS.printf("%i || F: %f \n\r", i, frontExpAvg.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);
     setLeftSpeed(3);
-    
+    t.start();
     
     //while (true)
     //while(SenseF.read() < 0.99)
-    while(frontExpAvg.average() < .8 && wallRight() && wallLeft()) //
+    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(linearize(SenseR.read()), linearize(SenseL.read()));
-        //WIRELESS.printf("ERR: %f  Front: %f\n\r", PIDv, SenseF.read());
+        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)
         {
@@ -130,7 +104,9 @@
             setRightSpeed(3);
             setLeftSpeed(3);
         } 
-        //check_walls();    
+        //check_walls(); 
+        //WIRELESS.printf("ERR: %f  Front: %f\n\r", PIDv, SenseF.read());
+        //WIRELESS.printf("R: %f  L: %f\n\r", cal_R, cal_L);   
     }
     
     //setRightSpeed(-2);
@@ -138,8 +114,9 @@
     
     //stop_mov();
     wait_ms(20);
-    handbrake();  
-    
+    WIRELESS.printf("Time: %f\n\r", t.read());
+    handbrake();
+    wait_ms(20);  
     printMap(WIRELESS); 
     return 0;
 }