Diff: main.cpp

Revision:

22:1a02d66218e4

Parent:

20:b18eed69ee32

Child:

23:953651133b29

--- a/main.cpp	Sat May 20 23:31:30 2017 +0000
+++ b/main.cpp	Sun May 21 03:09:44 2017 +0000
@@ -46,16 +46,31 @@
 
         //wait(2);
         //driver->print_serial_ports();
+        
+        
     
         
-        if (finished_traverse_one_cell == true) {
-            if (!driver->has_right_wall()) {
-                wait(2);
+       if (finished_traverse_one_cell == true) {
+            
+            if (!driver->has_right_wall() && !driver->has_left_wall()) {
+                wait(1);
+                if ((rand() % 4) == 0) {
+                    state = LEFT;
+                    driver->resetEncoders();
+                }
+                else {
+                    state = RIGHT;
+                    driver->resetEncoders();
+                }
+                    
+            }
+            else if (!driver->has_right_wall()) {
+                wait(0.25);
                 state = RIGHT;  
                 driver->resetEncoders();
             }
             else if (!driver->has_left_wall()) {
-                wait(2);
+                wait(0.25);
                 state = LEFT;  
                 driver->resetEncoders();
             }
@@ -64,7 +79,7 @@
                 driver->resetEncoders();
             }
             else{
-                wait(2);
+                wait(0.25);
                 state = UTURN;
                 driver->resetEncoders();
                 count = 0;
@@ -99,7 +114,7 @@
                 state = STRAIGHT;
                 driver->resetEncoders();
                 driver->clear_pid();
-                wait(0.25);
+               // wait(0.25);
                 continue;
             }
         }
@@ -115,7 +130,7 @@
                 state = STRAIGHT;
                 driver->resetEncoders();
                 driver->clear_pid();
-                wait(0.25);
+                //wait(0.25);
                 continue;
             }
         }
@@ -128,7 +143,7 @@
             else {
                 driver->stop();
                 driver->resetEncoders();
-                wait(0.25);
+                //wait(0.25);
                 flash_led (0, 0, 0, 0);
                 count == 1 ? finished_traverse_one_cell = true: count++;
                 continue;