Diff: main.cpp

Revision:

29:2224bc8bb49d

Parent:

28:600932142201

Child:

30:daf286ac049f

--- a/main.cpp	Sat May 27 07:21:02 2017 +0000
+++ b/main.cpp	Sat May 27 08:07:13 2017 +0000
@@ -9,6 +9,10 @@
 DigitalOut led_3(PB_14);
 DigitalOut led_4(PB_15);
 Serial serial (PA_9, PA_10);
+
+// Algorithm Switch
+DigitalIn alg_pin (PC_10);
+
 // CONTROL CONSTANTS
 const unsigned char  STRAIGHT = 0, LEFT = 1, RIGHT = 2, UTURN = 3;
 const int START_POS = 0, END_POS = 0;
@@ -43,37 +47,26 @@
     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);
+
+void run_flood_fill_algorithm() {
+DriveControl * driver = new DriveControl (START_POS, END_POS);
     my_mouse = new Mouse(driver);
     driver->resetEncoders();
- 
     int count = 0;
     bool finished_traverse_one_cell = true;
-    
+    wait(2);
     
-    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 (driver->right_wall_pid_debug()) {
+//            flash_led(1,0,1,1);
+//        }
        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);
+            wait(0.5);
             if(state == RIGHT){
                 flash_led(0,0,1,1);
                 wait(0.25);
@@ -90,10 +83,7 @@
                 driver->resetEncoders();
                 count = 0;
             }else{
-                //TODO
-                //Add error handling
                 state = STRAIGHT;
-                
                 flash_led(1, 0, 0, 0);
             }
             
@@ -124,7 +114,6 @@
                 wait(0.25);
                 driver->resetEncoders();
                 driver->clear_pid();
-               // wait(0.25);
                 continue;
             }
         }
@@ -138,7 +127,6 @@
                 wait(0.25);
                 driver->resetEncoders();
                 driver->clear_pid();
-                //wait(0.25);
                 continue;
             }
         }
@@ -159,4 +147,128 @@
             }
         }
     }
+}
+
+void run_heuristic_wall_follower_algorithm() {
+    DriveControl * driver = new DriveControl (START_POS, END_POS);
+    driver->resetEncoders();
+    int state, count = 0;
+    bool finished_traverse_one_cell = true;
+    wait(2);
+    while(CONTROL) {
+       check_battery();
+       wait_ms(1);
+       if (finished_traverse_one_cell == true) {
+            if (!driver->has_right_wall() && !driver->has_left_wall()) {
+               wait(1);
+                if ((rand() % 3) == 1) {
+                    state = LEFT;
+                    driver->resetEncoders();
+                }
+                else {
+                    state = RIGHT;
+                    driver->resetEncoders();
+                }   
+            }
+            else if (!driver->has_right_wall()) {
+                wait(0.25);
+                state = RIGHT;  
+                driver->resetEncoders();
+            }
+            else if (!driver->has_left_wall()) {
+                wait(0.25);
+                state = LEFT;  
+                driver->resetEncoders();
+            }
+            else if(!driver->has_front_wall()){
+                state = STRAIGHT;
+                driver->resetEncoders();
+            }
+            else{
+                wait(0.25);
+                state = UTURN;
+                driver->resetEncoders();
+                count = 0;
+            }
+            finished_traverse_one_cell = false;
+        }
+        
+        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);
+                wait(0.5);
+                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);
+                state = STRAIGHT;
+                driver->resetEncoders();
+                driver->clear_pid();
+                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);
+                if (count == 1) {
+                    finished_traverse_one_cell = true;
+                }
+                else {
+                    state = LEFT;
+                    wait(0.25);
+                    count++;
+                }
+                continue;
+            }
+        }
+    }
+}
+
+int main() {
+    if (alg_pin) {
+        flash_led(1,0,1,0);
+        wait(2);
+        run_flood_fill_algorithm();
+    }
+    else {
+        flash_led(1,1,1,0);
+        wait(2);
+        run_heuristic_wall_follower_algorithm();
+    }    
 }
\ No newline at end of file