Diff: main.cpp

Revision:

25:7155cb993870

Parent:

24:249505c7c080

Child:

26:191ec0e78774

--- a/main.cpp	Sun May 21 05:57:24 2017 +0000
+++ b/main.cpp	Sun May 21 06:34:21 2017 +0000
@@ -1,6 +1,7 @@
 #include "drivecontrol.h"
 #include "io_modules.h"
 #include "mbed.h"
+#include "maze_solver.h"
 //// PIN ASSIGNMENTS
 AnalogIn battery(PA_3);
 DigitalOut led_1(PB_12);
@@ -39,6 +40,9 @@
     driver->resetEncoders();
     int state, count = 0;
     bool finished_traverse_one_cell = true;
+    
+    Mouse *my_mouse = new Mouse();
+    
     //wait(1.5);
     while(CONTROL) {
         check_battery();
@@ -64,7 +68,7 @@
                 }
                     
             }
-            else */if (!driver->has_right_wall()) {
+            else if (!driver->has_right_wall()) {
                 wait(0.25);
                 state = RIGHT;  
                 driver->resetEncoders();
@@ -84,6 +88,26 @@
                 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;
         }