Diff: main.cpp

Revision:

19:931f8257fb74

Parent:

17:043ed1d0196f

Child:

20:b18eed69ee32

--- a/main.cpp	Sat May 20 21:34:06 2017 +0000
+++ b/main.cpp	Sat May 20 22:57:39 2017 +0000
@@ -50,18 +50,21 @@
         
         if (finished_traverse_one_cell == true) {
             if (!driver->has_right_wall()) {
+                wait(2);
                 state = RIGHT;  
-                driver->resetEncoders();  
+                driver->resetEncoders();
             }
             else if (!driver->has_left_wall()) {
+                wait(2);
                 state = LEFT;  
-                driver->resetEncoders();  
+                driver->resetEncoders();
             }
             else if(!driver->has_front_wall()){
                 state = STRAIGHT;
                 driver->resetEncoders();
             }
             else{
+                wait(2);
                 state = UTURN;
                 driver->resetEncoders();
                 count = 0;
@@ -71,18 +74,6 @@
         
         
         if (state == STRAIGHT) {
-            /*
-            if (!driver->has_front_wall() && !driver->has_front_wall()) {
-                driver->drive_forward();
-            }
-            else {
-                driver->stop();
-                wait(0.25);
-                flash_led (1, 1, 0, 0);
-                finished_traverse_one_cell = true;
-                continue;
-            } 
-            */
             if (!driver->should_finish_drive_forward() && !driver-> has_front_wall()) {
                 driver->drive_forward();
                 flash_led(1, 0, 0, 0);
@@ -91,7 +82,6 @@
                 driver->clear_pid();
                 driver->stop();
                 driver->resetEncoders();
-                wait(0.2);
                 flash_led (0, 0, 0, 0);
                 finished_traverse_one_cell = true;
                 continue;
@@ -104,12 +94,12 @@
             }
             else {
                 driver->stop();
-                wait(0.25);
                 flash_led (0, 0, 0, 0);
                 //finished_traverse_one_cell = true;
                 state = STRAIGHT;
                 driver->resetEncoders();
                 driver->clear_pid();
+                wait(0.25);
                 continue;
             }
         }
@@ -120,12 +110,12 @@
             }
             else {
                 driver->stop();
-                wait(0.25);
                 flash_led (0, 0, 0, 0);
                 //finished_traverse_one_cell = true;
                 state = STRAIGHT;
                 driver->resetEncoders();
                 driver->clear_pid();
+                wait(0.25);
                 continue;
             }
         }