Diff: main.cpp

Revision:

30:daf286ac049f

Parent:

29:2224bc8bb49d

Child:

31:f7a8a9b82bc1

--- a/main.cpp	Sat May 27 08:07:13 2017 +0000
+++ b/main.cpp	Sun May 28 06:49:41 2017 +0000
@@ -17,7 +17,6 @@
 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;
 
@@ -42,29 +41,51 @@
     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++;
-}
+//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++;
+//}
 
 void run_flood_fill_algorithm() {
-DriveControl * driver = new DriveControl (START_POS, END_POS);
-    my_mouse = new Mouse(driver);
+    DriveControl * driver = new DriveControl (START_POS, END_POS);
+    Mouse * my_mouse = new Mouse(driver);
     driver->resetEncoders();
     int count = 0;
     bool finished_traverse_one_cell = true;
-    wait(2);
+    wait(1);
+    int count_move = 0;
     
     while(CONTROL) {
         check_battery();
         wait_ms(1);
-//        if (driver->right_wall_pid_debug()) {
-//            flash_led(1,0,1,1);
+//        if(driver->right_wall_pid_debug()) {
+//            flash_led(0,1,1,0);
 //        }
+//       if (my_mouse -> center_found()){
+//         flash_led(1,1,1,1);
+//         //wait(2);
+//         //my_mouse = new Mouse(driver);
+//       }
        if (finished_traverse_one_cell == true) {
+            count_move++;
+//            if (count_move > 18 && my_mouse->can_reset_mouse) {
+//                my_mouse->clear_stack();
+//                delete my_mouse;
+//                count_move = 0;
+//                flash_led(0,1,1,1);
+//                wait(2);
+//                run_flood_fill_algorithm();
+//            }
+            if (count_move > 40) {
+                my_mouse->clear_stack();
+                delete my_mouse;
+                count_move = 0;
+                flash_led(0,1,1,1);
+                wait(2);
+            }
             state = my_mouse->solve_maze();
             wait(0.5);
             if(state == RIGHT){
@@ -96,7 +117,6 @@
                
             }
             else {
-                driver->clear_pid();
                 driver->stop();
                 wait(0.25);
                 driver->resetEncoders();
@@ -151,16 +171,25 @@
 
 void run_heuristic_wall_follower_algorithm() {
     DriveControl * driver = new DriveControl (START_POS, END_POS);
+    driver->set_wall_follower_speed();
     driver->resetEncoders();
     int state, count = 0;
+    int heuristic = 0;
     bool finished_traverse_one_cell = true;
-    wait(2);
+    wait(1);
     while(CONTROL) {
        check_battery();
        wait_ms(1);
        if (finished_traverse_one_cell == true) {
+            heuristic ++;
+            if (heuristic % 10 == 0) {
+                if (!driver->has_front_wall()){
+                    wait(0.25);
+                    state = STRAIGHT;
+                    driver->resetEncoders();
+                }
+            }
             if (!driver->has_right_wall() && !driver->has_left_wall()) {
-               wait(1);
                 if ((rand() % 3) == 1) {
                     state = LEFT;
                     driver->resetEncoders();
@@ -168,7 +197,8 @@
                 else {
                     state = RIGHT;
                     driver->resetEncoders();
-                }   
+                }
+                wait(0.25);   
             }
             else if (!driver->has_right_wall()) {
                 wait(0.25);
@@ -199,11 +229,9 @@
                 flash_led(1, 0, 0, 0);
             }
             else {
-                driver->clear_pid();
                 driver->stop();
                 driver->resetEncoders();
                 flash_led (0, 0, 0, 0);
-                wait(0.5);
                 finished_traverse_one_cell = true;
                 continue;
             }