main.cpp

00001 #include "drivecontrol.h"
00002 #include "io_modules.h"
00003 #include "mbed.h"
00004 #include "maze_solver.h"
00005 //// PIN ASSIGNMENTS
00006 AnalogIn battery(PA_3);
00007 DigitalOut led_1(PB_12);
00008 DigitalOut led_2(PB_13);
00009 DigitalOut led_3(PB_14);
00010 DigitalOut led_4(PB_15);
00011 Serial serial (PA_9, PA_10);
00012 
00013 // Algorithm Switch
00014 DigitalIn alg_pin (PC_10);
00015 
00016 // CONTROL CONSTANTS
00017 const unsigned char  STRAIGHT = 0, LEFT = 1, RIGHT = 2, UTURN = 3;
00018 const int START_POS = 0, END_POS = 0;
00019 const int CONTROL = 1;
00020 unsigned char state; 
00021 int move = 0;
00022 
00023 void check_battery () {
00024     // pc.baud(9600);
00025     // using the serial functions will have an impact on timing.
00026     // serial.printf("voltage value is: %3.3f%%\r\n", battery.read()*100.0f);
00027     // serial.printf("normalized: 0x%04X \r\n", battery.read_u16());
00028     if (battery.read() < 0.70f){
00029         // flash led
00030         led_1 = 1;
00031         led_2 = 1;
00032         led_3 = 1;
00033         led_4 = 1;
00034     }
00035 }
00036 
00037 void flash_led (int _led_1, int _led_2, int _led_3, int _led_4) {
00038     led_1 = _led_1;
00039     led_2 = _led_2;
00040     led_3 = _led_3;
00041     led_4 = _led_4;
00042 }
00043 
00044 //void print_debug_message(){
00045 //    serial.printf("Next mouse position <%u,%u> \r\n", my_mouse->mouse_x, my_mouse->mouse_y);
00046 //    serial.printf("next state: %u \r\n", state);
00047 //    serial.printf("previous mouse direction <%d> \r\n", my_mouse->get_prev_direction());
00048 //    serial.printf("next cell direction <%d> \r\n", my_mouse->get_next_cell_direction());
00049 //    move++;
00050 //}
00051 
00052 void run_flood_fill_algorithm() {
00053     DriveControl * driver = new DriveControl (START_POS, END_POS);
00054     Mouse * my_mouse = new Mouse(driver);
00055     driver->resetEncoders();
00056     int count = 0;
00057     bool finished_traverse_one_cell = true;
00058     wait(1);
00059     int count_move = 0;
00060     
00061     while(CONTROL) {
00062         check_battery();
00063         wait_ms(1);
00064 //        if(driver->right_wall_pid_debug()) {
00065 //            flash_led(0,1,1,0);
00066 //        }
00067 //       if (my_mouse -> center_found()){
00068 //         flash_led(1,1,1,1);
00069 //         //wait(2);
00070 //         //my_mouse = new Mouse(driver);
00071 //       }
00072        if (finished_traverse_one_cell == true) {
00073 //            if (!driver->has_right_wall() && !driver->has_left_wall()){
00074 //                flash_led(0,0,0,0);    
00075 //            }
00076 //            else if (!driver->has_left_wall()){
00077 //                flash_led(1,0,0,0);
00078 //            }
00079 //            else if (!driver->has_right_wall()){
00080 //                flash_led(0,1,1,1);
00081 //            }
00082 //            else{
00083 //                flash_led(0,0,0,0);
00084 //            }
00085 
00086             if (my_mouse->center_found()) {
00087                 flash_led(1,1,1,1);
00088             }
00089             //count_move++;
00090 //            if (count_move > 18 && my_mouse->can_reset_mouse) {
00091 //                my_mouse->clear_stack();
00092 //                delete my_mouse;
00093 //                count_move = 0;
00094 //                flash_led(0,1,1,1);
00095 //                wait(2);
00096 //                run_flood_fill_algorithm();
00097 //            }
00098 //            if (count_move > 30) {
00099 //                my_mouse->clear_stack();
00100 //                count_move = 0;
00101 //                flash_led(0,1,1,1);
00102 //                wait(2);
00103 //            }
00104             state = my_mouse->solve_maze();
00105             wait(0.5);
00106             if(state == RIGHT){
00107                 //flash_led(0,0,1,1);
00108                 wait(0.25);
00109                 driver->resetEncoders();
00110             }
00111             else if(state == LEFT){
00112                 //flash_led(1,1,0,0);
00113                 wait(0.25);
00114                 driver->resetEncoders();
00115             }
00116             else if(state == UTURN){
00117                 //flash_led(1,1,1,0);
00118                 wait(0.25);
00119                 driver->resetEncoders();
00120                 count = 0;
00121             }else{
00122                 state = STRAIGHT;
00123                 //flash_led(1, 0, 0, 0);
00124             }
00125             
00126             finished_traverse_one_cell = false;
00127         }
00128         
00129         if (state == STRAIGHT) {
00130             if (!driver->should_finish_drive_forward() && !driver-> has_front_wall()) {
00131                 driver->drive_forward();
00132                
00133             }
00134             else {
00135                 driver->stop();
00136                 wait(0.25);
00137                 driver->resetEncoders();
00138                 finished_traverse_one_cell = true;
00139                 continue;
00140             }
00141         }
00142         if (state == RIGHT) {
00143             if (!driver->should_finish_turn_right()) {
00144                 driver->turn_right();
00145             }
00146             else {
00147                 driver->stop();
00148                 state = STRAIGHT;
00149                 wait(0.25);
00150                 driver->resetEncoders();
00151                 driver->clear_pid();
00152                 continue;
00153             }
00154         }
00155         if (state == LEFT) {
00156             if (!driver->should_finish_turn_left()) {
00157                 driver->turn_left();
00158             }
00159             else {
00160                 driver->stop();
00161                 state = STRAIGHT;
00162                 wait(0.25);
00163                 driver->resetEncoders();
00164                 driver->clear_pid();
00165                 continue;
00166             }
00167         }
00168         
00169         if (state == UTURN){
00170             if (!driver->should_finish_turn_left()) {
00171                 driver->turn_left();
00172             }
00173             else {
00174                 driver->stop();
00175                 driver->resetEncoders();
00176                 wait(0.25);
00177                 if(count == 1) {
00178                     state = STRAIGHT;
00179                 }
00180                 count ++;//
00181                 continue;
00182             }
00183         }
00184     }
00185 }
00186 
00187 void run_heuristic_wall_follower_algorithm() {
00188     DriveControl * driver = new DriveControl (START_POS, END_POS);
00189     driver->set_wall_follower_speed();
00190     driver->set_wall_follower_sensor_thres();
00191     driver->resetEncoders();
00192     int state, count = 0;
00193     int heuristic = 0;
00194     bool finished_traverse_one_cell = true;
00195     wait(1);
00196     while(CONTROL) {
00197        check_battery();
00198        wait_ms(1);
00199        if (finished_traverse_one_cell == true) {
00200             heuristic ++;
00201             if (heuristic % 10 == 0) {
00202                 if (!driver->has_front_wall()){
00203                     wait(0.25);
00204                     state = STRAIGHT;
00205                     driver->resetEncoders();
00206                 }
00207             }
00208             if (!driver->has_right_wall() && !driver->has_left_wall()) {
00209                 if ((rand() % 3) == 1) {
00210                     state = LEFT;
00211                     driver->resetEncoders();
00212                 }
00213                 else {
00214                     state = RIGHT;
00215                     driver->resetEncoders();
00216                 }
00217                 wait(0.25);   
00218             }
00219             else if (!driver->has_right_wall()) {
00220                 wait(0.25);
00221                 state = RIGHT;  
00222                 driver->resetEncoders();
00223             }
00224             else if (!driver->has_left_wall()) {
00225                 wait(0.25);
00226                 state = LEFT;  
00227                 driver->resetEncoders();
00228             }
00229             else if(!driver->has_front_wall()){
00230                 state = STRAIGHT;
00231                 driver->resetEncoders();
00232             }
00233             else{
00234                 wait(0.25);
00235                 state = UTURN;
00236                 driver->resetEncoders();
00237                 count = 0;
00238             }
00239             finished_traverse_one_cell = false;
00240         }
00241         
00242         if (state == STRAIGHT) {
00243             if (!driver->should_finish_drive_forward() && !driver-> has_front_wall()) {
00244                 driver->drive_forward();
00245                 flash_led(1, 0, 0, 0);
00246             }
00247             else {
00248                 driver->stop();
00249                 driver->resetEncoders();
00250                 flash_led (0, 0, 0, 0);
00251                 finished_traverse_one_cell = true;
00252                 continue;
00253             }
00254         }
00255         if (state == RIGHT) {
00256             if (!driver->should_finish_turn_right()) {
00257                 driver->turn_right();
00258                 flash_led (0, 1, 0, 0);
00259             }
00260             else {
00261                 driver->stop();
00262                 flash_led (0, 0, 0, 0);
00263                 state = STRAIGHT;
00264                 driver->resetEncoders();
00265                 driver->clear_pid();
00266                 continue;
00267             }
00268         }
00269         if (state == LEFT) {
00270             if (!driver->should_finish_turn_left()) {
00271                 driver->turn_left();
00272                 flash_led (0, 0, 1, 0);
00273             }
00274             else {
00275                 driver->stop();
00276                 flash_led (0, 0, 0, 0);
00277                 state = STRAIGHT;
00278                 driver->resetEncoders();
00279                 driver->clear_pid();
00280                 continue;
00281             }
00282         }
00283         
00284         if (state == UTURN){
00285             if (!driver->should_finish_turn_left()) {
00286                 driver->turn_left();
00287                 flash_led (0, 1, 1, 0);
00288             }
00289             else {
00290                 driver->stop();
00291                 driver->resetEncoders();
00292                 flash_led (0, 0, 0, 0);
00293                 if (count == 1) {
00294                     finished_traverse_one_cell = true;
00295                 }
00296                 else {
00297                     state = LEFT;
00298                     wait(0.25);
00299                     count++;
00300                 }
00301                 continue;
00302             }
00303         }
00304     }
00305 }
00306 
00307 int main() {
00308     if (alg_pin) {
00309         flash_led(1,0,1,0);
00310         wait(2);
00311         run_flood_fill_algorithm();
00312     }
00313     else {
00314         flash_led(1,1,1,0);
00315         wait(2);
00316         run_heuristic_wall_follower_algorithm();
00317     }    
00318 }