maze_solver.cpp

00001 #include "maze_solver.h"
00002 DriveControl * driver;
00003 int prev_mouse_dir_print = 5;
00004 int prev_mouse_dir = NORTH;
00005 
00006 Maze:: Maze() {
00007     unsigned char goal1 = MAZE_SIZE / 2;
00008     unsigned char goal2 = (MAZE_SIZE - 1) / 2;
00009     for (unsigned char i = 0; i < MAZE_SIZE; i++) {
00010         for (unsigned char j = 0; j < MAZE_SIZE; j++) {
00011             unsigned char min = min4(manhattan_dist(i, goal1, j, goal1),
00012                                      manhattan_dist(i, goal1, j, goal2),
00013                                      manhattan_dist(i, goal2, j, goal1),
00014                                      manhattan_dist(i, goal2, j, goal2));
00015             maze[i][j] = new Cell(i, j, min);
00016             if (i == MAZE_SIZE - 1) {
00017                 maze[i][j]->top_wall = true;
00018             }
00019             if (j == MAZE_SIZE - 1) {
00020                 maze[i][j]->right_wall = true;
00021             }
00022         }
00023     }
00024 }
00025 
00026 unsigned char Maze:: manhattan_dist(unsigned char x1, unsigned char x2,
00027                                     unsigned char y1, unsigned char y2) {
00028 
00029     return abs(x1 - x2) + abs(y1 - y2);
00030 }
00031 
00032 // Function that takes the minimum of the four given distances
00033 unsigned char Maze:: min4(unsigned char a, unsigned char b, unsigned char c, unsigned char d) {
00034     unsigned char min;
00035     (a < b) ? min = a : min = b;
00036     if (c < min) min = c;
00037     if (d < min) min = d;
00038     return min;
00039 }
00040 
00041 bool Maze:: is_center(unsigned char x, unsigned char y) {
00042     unsigned char goal1 =  MAZE_SIZE / 2;
00043     unsigned char goal2 = (MAZE_SIZE - 1) / 2;
00044     if (manhattan_dist(y, goal1, x, goal1) == 0 ||
00045             manhattan_dist(y, goal1, x, goal2) == 0 ||
00046             manhattan_dist(y, goal2, x, goal1) == 0 ||
00047             manhattan_dist(y, goal2, x, goal2) == 0) {
00048         return true;
00049     }
00050     return false;
00051 }
00052 
00053 //============================================================
00054 unsigned char Mouse:: get_direction() {
00055     return direction;
00056 }
00057 
00058 void Mouse:: set_direction(unsigned char dir) {
00059     direction = dir;
00060 }
00061 
00062 
00063 bool Mouse:: get_front_sensor_value(){
00064     bool has_wall;
00065     if(direction == NORTH){
00066         if(mouse_x == MAZE_SIZE-1){ //if the mouse is at the topest row;
00067             return 1;
00068         }
00069         // TODO: change to check front sensor value.
00070         // TODO: has_wall = reference_maze->maze[mouse_x][mouse_y]->top_wall;
00071         has_wall = driver->has_front_wall();
00072         detected_maze->maze[mouse_x][mouse_y]->top_wall = has_wall;
00073     }
00074     else if(direction == SOUTH){
00075         if(mouse_x == 0){
00076             return 1;
00077         }
00078         // TODO: has_wall = reference_maze->maze[mouse_x-1][mouse_y]->top_wall; //its bottom cell's upper wall
00079         has_wall = driver->has_front_wall();
00080         detected_maze->maze[mouse_x-1][mouse_y]->top_wall = has_wall;
00081     }
00082     else if(direction == EAST){
00083         if(mouse_y == MAZE_SIZE-1)
00084             return 1;
00085         has_wall = driver->has_front_wall();
00086         detected_maze->maze[mouse_x][mouse_y]->right_wall = has_wall;
00087     }       
00088     else{ //WEST
00089         if(mouse_y == 0)
00090             return 1;
00091         has_wall = driver->has_front_wall();
00092         detected_maze->maze[mouse_x][mouse_y-1]->right_wall = has_wall;
00093     }
00094     return has_wall;
00095 }
00096 
00097 bool Mouse:: get_left_sensor_value(){
00098     bool has_wall;
00099 
00100     if(direction == NORTH){
00101         if(mouse_y == 0) //if the mouse is at the left_most column
00102             return 1;
00103         
00104         // TODO: has_wall = reference_maze->maze[mouse_x][mouse_y-1]->right_wall;
00105         has_wall = driver->has_left_wall();
00106         detected_maze->maze[mouse_x][mouse_y-1]->right_wall = has_wall;
00107 
00108     }
00109     else if(direction == SOUTH){
00110         if(mouse_y == MAZE_SIZE-1)
00111             return 1;
00112 
00113         // TODO: has_wall = reference_maze->maze[mouse_x][mouse_y]->right_wall;
00114         has_wall = driver->has_left_wall();
00115         detected_maze->maze[mouse_x][mouse_y]->right_wall = has_wall;
00116 
00117     }
00118     else if(direction == EAST){
00119         if(mouse_x == MAZE_SIZE-1)
00120             return 1;
00121 
00122         // TODO: has_wall = reference_maze->maze[mouse_x][mouse_y]->top_wall;
00123         has_wall = driver->has_left_wall();
00124         detected_maze->maze[mouse_x][mouse_y]->top_wall = has_wall;
00125 
00126     }
00127     else{
00128         if(mouse_x == 0)
00129             return 1;
00130         // TODO: has_wall = reference_maze->maze[mouse_x-1][mouse_y]->top_wall;
00131         has_wall = driver->has_left_wall();
00132         detected_maze->maze[mouse_x-1][mouse_y]->top_wall = has_wall;
00133     }
00134 
00135     return has_wall;
00136 }
00137 
00138 bool Mouse:: get_right_sensor_value(){
00139 
00140     bool has_wall;
00141 
00142     if(direction == NORTH){
00143         if(mouse_y == MAZE_SIZE-1) //if the mouse is at the right_most column
00144             return 1;
00145 
00146         // TODO: has_wall = reference_maze->maze[mouse_x][mouse_y]->right_wall;
00147         has_wall = driver->has_right_wall();
00148         detected_maze->maze[mouse_x][mouse_y]->right_wall = has_wall;
00149 
00150     }
00151     else if(direction == SOUTH){
00152         if(mouse_y == 0)
00153             return 1;
00154 
00155         // TODO: has_wall = reference_maze->maze[mouse_x][mouse_y-1]->right_wall;
00156         has_wall = driver->has_right_wall();
00157         detected_maze->maze[mouse_x][mouse_y-1]->right_wall = has_wall;
00158 
00159     }
00160     else if(direction == EAST){
00161         if(mouse_x == 0)
00162             return 1;
00163 
00164         // TODO: has_wall = reference_maze->maze[mouse_x-1][mouse_y]->top_wall;
00165         has_wall = driver->has_right_wall();
00166         detected_maze->maze[mouse_x-1][mouse_y]->top_wall = has_wall;
00167     }
00168     else{
00169         if(mouse_x == MAZE_SIZE-1)
00170             return 1;
00171 
00172         // TODO: has_wall = reference_maze->maze[mouse_x][mouse_y]->top_wall;
00173         has_wall = driver->has_right_wall();
00174         detected_maze->maze[mouse_x][mouse_y]->top_wall = has_wall;
00175 
00176     }
00177 
00178     return has_wall;
00179 
00180 }
00181 
00182 
00183 // the front sensor is 0, then the mouse can move in this direction
00184 bool Mouse:: can_move() {
00185     //in the real world, instead of using 0, we need to set some constant
00186     //as a threshold for the sensor value;
00187     return (get_front_sensor_value()==0);
00188 }
00189 
00190 
00191 //Assumption: the maze[0][y] and maze[x][0] has left wall and bottom wall
00192 void Mouse:: check_open_neighbor() {
00193     right_sensor = get_right_sensor_value();
00194     front_sensor = get_front_sensor_value();
00195     left_sensor = get_left_sensor_value();
00196 
00197 
00198     if(direction == NORTH) {
00199         if(prev == SOUTH) {
00200             south_open = 1;
00201         }
00202 
00203         north_open = !front_sensor;
00204         east_open = !right_sensor;
00205         west_open = !left_sensor;
00206     } else if(direction == SOUTH) {
00207         if(prev == NORTH) {
00208             north_open = 1;
00209         }
00210 
00211         south_open = !front_sensor;
00212         east_open = !left_sensor;
00213         west_open = !right_sensor;
00214     } else if(direction == EAST) {
00215         if(prev == WEST) {
00216             west_open = 1;
00217         }
00218 
00219         south_open = !right_sensor;
00220         east_open = !front_sensor;
00221         north_open = !left_sensor;
00222     } else { //WEST
00223         if(prev == EAST) {
00224             east_open = 1;
00225         }
00226 
00227         south_open = !left_sensor;
00228         west_open = !front_sensor;
00229         north_open = !right_sensor;
00230     }
00231   
00232 }
00233 
00234 unsigned char Mouse:: min_open_neighbor(vector<Cell*> cells) {
00235     int min = MAX;
00236     for (vector<Cell *>::iterator it = cells.begin(); it != cells.end(); it++) {
00237         if ((*it)->dist < min) {
00238             min = (*it)->dist;
00239         }
00240     }
00241     return min;
00242 }
00243 
00244 void Mouse:: update_distance() {
00245     Cell * curr;
00246     vector<Cell*> neighbor;
00247     vector<Cell*> open_neighbor;
00248     unsigned char min_dist;
00249     unsigned char x, y;
00250     unsigned char count = 0;
00251     stk.push_back(detected_maze->maze[mouse_x][mouse_y]);
00252 
00253     while(!stk.empty()) {
00254         curr = stk.back();
00255         stk.pop_back();
00256         x = curr->x;
00257         y = curr->y;
00258 
00259         if(x < MAZE_SIZE-1) { //top cell
00260             neighbor.push_back(detected_maze->maze[x+1][y]);
00261             if(!curr->top_wall) {
00262                 open_neighbor.push_back(detected_maze->maze[x + 1][y]);
00263             }
00264         }
00265 
00266         if(y > 0) { //left cell
00267             neighbor.push_back(detected_maze->maze[x][y-1]);
00268 
00269             if (!(detected_maze->maze[x][y - 1]->right_wall)) {
00270                 open_neighbor.push_back(detected_maze->maze[x][y - 1]);
00271             }
00272         }
00273 
00274         if(x > 0) { //bottom cell
00275             neighbor.push_back(detected_maze->maze[x-1][y]);
00276             if (!(detected_maze->maze[x-1][y]->top_wall)) {
00277                 open_neighbor.push_back(detected_maze->maze[x-1][y]);
00278             }
00279         }
00280 
00281         if(y < MAZE_SIZE -1) { //right_cell
00282             neighbor.push_back(detected_maze->maze[x][y+1]);
00283             if(!curr->right_wall) {
00284                 open_neighbor.push_back(detected_maze->maze[x][y+1]);
00285             }
00286         }
00287         if (open_neighbor.empty()) {
00288             neighbor.clear();
00289             continue;
00290         }
00291         min_dist = min_open_neighbor(open_neighbor);
00292         open_neighbor.clear();
00293 
00294         if (curr->dist - 1 != min_dist) {
00295             curr->dist = min_dist + 1;
00296             for (vector<Cell *>::iterator it = neighbor.begin(); it != neighbor.end(); it++) {
00297                 if (!detected_maze->is_center((*it)->x, (*it)->y)) {
00298                     stk.push_back(*it);
00299                 }
00300             }
00301             neighbor.clear();
00302         }
00303         count++;
00304     }
00305 }
00306 
00307 void Mouse::clear_stack() {
00308     stk.clear();
00309 }
00310 
00311 void Mouse:: move_one_cell() {
00312     check_open_neighbor();
00313     update_distance();
00314     int min_dist = MAX;
00315     unsigned char temp;
00316     if(north_open) {
00317         temp = detected_maze->maze[mouse_x+1][mouse_y]->dist;
00318         if (temp < min_dist) {
00319             min_dist = temp;
00320             direction = NORTH;
00321             prev = SOUTH;
00322         }
00323     }
00324 
00325 
00326     if(east_open) {
00327         temp = detected_maze->maze[mouse_x][mouse_y+1]->dist;
00328         if (temp < min_dist) {
00329             min_dist = temp;
00330             direction = EAST;
00331             prev = WEST;
00332         }
00333     }
00334 
00335     if(south_open) {
00336         temp = detected_maze->maze[mouse_x-1][mouse_y]->dist;
00337         if (temp < min_dist) {
00338             min_dist = temp;
00339             direction = SOUTH;
00340             prev = NORTH;
00341         }
00342     }
00343     if(west_open) {
00344         temp = detected_maze->maze[mouse_x][mouse_y-1]->dist;
00345         if (temp < min_dist) {
00346             min_dist = temp;
00347             direction = WEST;
00348             prev = EAST;
00349         }
00350     }
00351     //Move
00352     if(direction == NORTH) {
00353         mouse_x += 1;
00354     } else if(direction == SOUTH) {
00355         mouse_x -= 1;
00356     } else if (direction == EAST) {
00357         mouse_y += 1;
00358     } else { //WEST
00359         mouse_y -= 1;
00360     }
00361     north_open = south_open = east_open = west_open = 0;
00362 }
00363 
00364 bool Mouse::center_found() {
00365     return detected_maze->is_center(mouse_x, mouse_y);
00366 }
00367 
00368 int Mouse::get_next_cell_direction() {
00369     return direction;    
00370 }
00371 
00372 int Mouse::get_prev_direction() {
00373     return prev_mouse_dir_print;
00374 }
00375 
00376 int Mouse:: solve_maze() {
00377     const unsigned char STRAIGHT = 0, LEFT = 1, RIGHT = 2, UTURN = 3;
00378     
00379     prev_mouse_dir_print = prev_mouse_dir;
00380     
00381   //  
00382 
00383     //while the mouse has not find the center of the Maze
00384     move_one_cell();
00385     unsigned char state = 5;
00386     
00387     
00388     if (prev_mouse_dir == direction) {
00389         if (prev_mouse_dir == NORTH && direction == NORTH) {
00390             can_reset_mouse = true;
00391         }
00392         state = STRAIGHT;
00393     }
00394     // Mouse is facing south
00395     else if (prev_mouse_dir == SOUTH && direction == NORTH) {
00396         state = UTURN;
00397         prev_mouse_dir = NORTH;
00398     } else if (prev_mouse_dir == SOUTH && direction == EAST) {
00399         state = LEFT;
00400         prev_mouse_dir = EAST;
00401     } else if (prev_mouse_dir == SOUTH && direction == WEST) {
00402         state = RIGHT;
00403         prev_mouse_dir = WEST;
00404     }
00405     // Mouse is facing north
00406     else if (prev_mouse_dir == NORTH && direction == EAST) {
00407         state = RIGHT;
00408         prev_mouse_dir = EAST;
00409     } else if (prev_mouse_dir == NORTH && direction == WEST) {
00410         state = LEFT;
00411         prev_mouse_dir = WEST;
00412     } else if (prev_mouse_dir == NORTH && direction == SOUTH) {
00413         state = UTURN;
00414         prev_mouse_dir = SOUTH;
00415     }
00416     // Mouse is facing west
00417     else if (prev_mouse_dir == WEST && direction == NORTH) {
00418         state = RIGHT;
00419         prev_mouse_dir = NORTH;
00420     } else if (prev_mouse_dir == WEST && direction == SOUTH) {
00421         state = LEFT;
00422         prev_mouse_dir = SOUTH;
00423     } else if (prev_mouse_dir == WEST && direction == EAST) {
00424         state = UTURN;
00425         prev_mouse_dir = EAST;
00426     }
00427     // Mouse is facing west
00428     else if (prev_mouse_dir == EAST && direction == NORTH) {
00429         state = LEFT;
00430         prev_mouse_dir = NORTH;
00431     } else if (prev_mouse_dir == EAST && direction == SOUTH) {
00432         state = RIGHT;
00433         prev_mouse_dir = SOUTH;
00434     } else if (prev_mouse_dir == EAST && direction == WEST) {
00435         state = UTURN;
00436         prev_mouse_dir = WEST;
00437     }
00438 
00439     
00440     return state;
00441 }
00442 
00443 
00444 
00445 Mouse:: Mouse(DriveControl * prev_driver) {
00446     can_reset_mouse = false;
00447     driver = prev_driver;
00448     detected_maze = new Maze();
00449     prev = (unsigned char) 5;
00450     direction = NORTH;
00451     mouse_x = mouse_y = 0;
00452     north_open = south_open = east_open = west_open = 0;
00453 }