Diff: main.cpp

Revision:

28:600932142201

Parent:

27:b980fce784ea

Child:

29:2224bc8bb49d

--- a/main.cpp	Fri May 26 05:45:02 2017 +0000
+++ b/main.cpp	Sat May 27 07:21:02 2017 +0000
@@ -13,6 +13,9 @@
 const unsigned char  STRAIGHT = 0, LEFT = 1, RIGHT = 2, UTURN = 3;
 const int START_POS = 0, END_POS = 0;
 const int CONTROL = 1;
+Mouse * my_mouse;
+unsigned char state; 
+int move = 0;
 
 void check_battery () {
     // pc.baud(9600);
@@ -35,26 +38,42 @@
     led_4 = _led_4;
 }
 
+void print_debug_message(){
+    serial.printf("Next mouse position <%u,%u> \r\n", my_mouse->mouse_x, my_mouse->mouse_y);
+    serial.printf("next state: %u \r\n", state);
+    serial.printf("previous mouse direction <%d> \r\n", my_mouse->get_prev_direction());
+    serial.printf("next cell direction <%d> \r\n", my_mouse->get_next_cell_direction());
+  
+    move++;
+}
 int main() {
     DriveControl * driver = new DriveControl (START_POS, END_POS);
-    Mouse * my_mouse = new Mouse(driver);
+    my_mouse = new Mouse(driver);
     driver->resetEncoders();
-    unsigned char state; 
+ 
     int count = 0;
     bool finished_traverse_one_cell = true;
     
     
-    //wait(1.5);
+    wait(2);
     while(CONTROL) {
         check_battery();
         wait_ms(1);
+        if (driver->right_wall_pid_debug()) {
+            flash_led(1,0,1,1);
+        }
 
         //wait(2);
         //driver->print_serial_ports();
         
        if (finished_traverse_one_cell == true) {
+            
             state = my_mouse->solve_maze();
-           
+            wait(1);
+//            print_debug_message();
+//            serial.printf("left wall: %d, right wall:%d, front wall: %d \r\n", driver->has_left_wall(), driver->has_right_wall(), driver->has_front_wall());
+//            serial.printf("--- \r\n");
+//            wait(2);
             if(state == RIGHT){
                 flash_led(0,0,1,1);
                 wait(0.25);
@@ -74,6 +93,7 @@
                 //TODO
                 //Add error handling
                 state = STRAIGHT;
+                
                 flash_led(1, 0, 0, 0);
             }
             
@@ -88,6 +108,7 @@
             else {
                 driver->clear_pid();
                 driver->stop();
+                wait(0.25);
                 driver->resetEncoders();
                 finished_traverse_one_cell = true;
                 continue;
@@ -99,8 +120,8 @@
             }
             else {
                 driver->stop();
-                //finished_traverse_one_cell = true;
                 state = STRAIGHT;
+                wait(0.25);
                 driver->resetEncoders();
                 driver->clear_pid();
                // wait(0.25);
@@ -113,8 +134,8 @@
             }
             else {
                 driver->stop();
-                //finished_traverse_one_cell = true;
                 state = STRAIGHT;
+                wait(0.25);
                 driver->resetEncoders();
                 driver->clear_pid();
                 //wait(0.25);