Diff: main.cpp

Revision:

26:191ec0e78774

Parent:

25:7155cb993870

Child:

27:b980fce784ea

--- a/main.cpp	Sun May 21 06:34:21 2017 +0000
+++ b/main.cpp	Sun May 21 08:10:29 2017 +0000
@@ -13,14 +13,11 @@
 const int STRAIGHT = 0, LEFT = 1, RIGHT = 2, UTURN = 3;
 const int START_POS = 0, END_POS = 0;
 const int CONTROL = 1;
+Timer flood_fill_timer;
+Timer state_timeout_timer;
 
 void check_battery () {
-    // pc.baud(9600);
-    // using the serial functions will have an impact on timing.
-    // serial.printf("voltage value is: %3.3f%%\r\n", battery.read()*100.0f);
-    // serial.printf("normalized: 0x%04X \r\n", battery.read_u16());
     if (battery.read() < 0.70f){
-        // flash led
         led_1 = 1;
         led_2 = 1;
         led_3 = 1;
@@ -40,25 +37,113 @@
     driver->resetEncoders();
     int state, count = 0;
     bool finished_traverse_one_cell = true;
+    Mouse *my_mouse = new Mouse();
+    flood_fill_timer.start();
+    while(flood_fill_timer.read() < 50) {
+       check_battery();
+       wait_ms(1);
+       if (finished_traverse_one_cell == true) {
+            if (state_timeout_timer.read() > 10) {
+                driver->stop();
+                wait(0.5);
+                driver->reverse();
+                wait(0.5);
+                driver->stop();
+                driver->resetEncoders();
+                state_timeout_timer.reset();
+                state_timeout_timer.start();
+            }
+            state = my_mouse->solve_maze();
+            if(state == RIGHT){
+                flash_led(1,1,0,0);
+                wait(0.25);
+                driver->resetEncoders();
+            }
+            else if(state == LEFT){
+                flash_led(1,0,1,0);
+                wait(0.25);
+                driver->resetEncoders();
+            }
+            else if(state == UTURN){
+                flash_led(1,1,1,0);
+                wait(0.25);
+                driver->resetEncoders();
+                count = 0;
+            }
+            finished_traverse_one_cell = false;
+        }
+        state_timeout_timer.start();
+        if (state == STRAIGHT) {
+            if (!driver->should_finish_drive_forward() && !driver-> has_front_wall()) {
+                driver->drive_forward();
+                flash_led(1, 0, 0, 0);
+            }
+            else {
+                driver->clear_pid();
+                driver->stop();
+                driver->resetEncoders();
+                flash_led (0, 0, 0, 0);
+                finished_traverse_one_cell = true;
+                continue;
+            }
+        }
+        if (state == RIGHT) {
+            if (!driver->should_finish_turn_right()) {
+                driver->turn_right();
+                flash_led (0, 1, 0, 0);
+            }
+            else {
+                driver->stop();
+                flash_led (0, 0, 0, 0);
+                //finished_traverse_one_cell = true;
+                state = STRAIGHT;
+                driver->resetEncoders();
+                driver->clear_pid();
+               // wait(0.25);
+                continue;
+            }
+        }
+        if (state == LEFT) {
+            if (!driver->should_finish_turn_left()) {
+                driver->turn_left();
+                flash_led (0, 0, 1, 0);
+            }
+            else {
+                driver->stop();
+                flash_led (0, 0, 0, 0);
+                state = STRAIGHT;
+                driver->resetEncoders();
+                driver->clear_pid();
+                continue;
+            }
+        }
+        
+        if (state == UTURN){
+            if (!driver->should_finish_turn_left()) {
+                driver->turn_left();
+                flash_led (0, 1, 1, 0);
+            }
+            else {
+                driver->stop();
+                driver->resetEncoders();
+                flash_led (0, 0, 0, 0);
+                count == 1 ? finished_traverse_one_cell = true: count++;
+                continue;
+            }
+        }
+    }
+    flood_fill_timer.stop();
+    state = count = 0;
+    finished_traverse_one_cell = true;
     
-    Mouse *my_mouse = new Mouse();
-    
-    //wait(1.5);
+    flash_led(1,1,1,1);
     while(CONTROL) {
-        check_battery();
-        wait_ms(1);
-
-        //wait(2);
-        //driver->print_serial_ports();
-        
-        
-    
-        
+       check_battery();
+       wait_ms(1);
        if (finished_traverse_one_cell == true) {
-            
-            /*if (!driver->has_right_wall() && !driver->has_left_wall()) {
+            if (!driver->has_right_wall() && !driver->has_left_wall()) {
                 wait(1);
-                if ((rand() % 4) == 0) {
+                if ((rand() % 3) == 0) {
                     state = LEFT;
                     driver->resetEncoders();
                 }
@@ -88,26 +173,6 @@
                 driver->resetEncoders();
                 count = 0;
             }
-            */
-            
-            state = my_mouse->solve_maze();
-            if(state == RIGHT){
-                flash_led(1,1,0,0);
-                wait(0.25);
-                driver->resetEncoders();
-            }
-            else if(state == LEFT){
-                flash_led(1,0,1,0);
-                wait(0.25);
-                driver->resetEncoders();
-            }
-            else if(state == UTURN){
-                flash_led(1,1,1,0);
-                wait(0.25);
-                driver->resetEncoders();
-                count = 0;
-            }
-            
             finished_traverse_one_cell = false;
         }
         
@@ -149,11 +214,9 @@
             else {
                 driver->stop();
                 flash_led (0, 0, 0, 0);
-                //finished_traverse_one_cell = true;
                 state = STRAIGHT;
                 driver->resetEncoders();
                 driver->clear_pid();
-                //wait(0.25);
                 continue;
             }
         }
@@ -166,39 +229,10 @@
             else {
                 driver->stop();
                 driver->resetEncoders();
-                //wait(0.25);
                 flash_led (0, 0, 0, 0);
                 count == 1 ? finished_traverse_one_cell = true: count++;
                 continue;
             }
         }
     }
-}
-
-
-/*
-
-    if (!driver->has_front_wall()) {
-            if (!driver->should_finish_drive_forward()) {
-                driver->drive_forward();
-                flash_led(0, 1, 1, 0);
-            }
-            else {
-                driver->stop();
-                wait(1);
-                flash_led (0,0,0,0);
-                driver->resetEncoders();
-                finished_traverse_one_cell = true;
-                continue;
-            }
-        }
-        else {
-            driver->stop();
-            driver->resetEncoders();
-            flash_led (1, 1, 1, 0);
-        }
-
-
-
-
-*/
\ No newline at end of file
+}
\ No newline at end of file