TomYumBoys / Mbed 2 deprecated MM2017

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Maze/maze_solver.cpp@31:f7a8a9b82bc1, 2017-05-28 (annotated)

Committer:
kolanery
Date:
Sun May 28 09:54:40 2017 +0000
Revision:
31:f7a8a9b82bc1
Parent:
30:daf286ac049f 
Update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryan_whr 25:7155cb993870 1 #include "maze_solver.h"
szh66 27:b980fce784ea 2 DriveControl * driver;
kolanery 28:600932142201 3 int prev_mouse_dir_print = 5;
kolanery 28:600932142201 4 int prev_mouse_dir = NORTH;
ryan_whr 25:7155cb993870 5
szh66 27:b980fce784ea 6 Maze:: Maze() {
szh66 27:b980fce784ea 7 unsigned char goal1 = MAZE_SIZE / 2;
szh66 27:b980fce784ea 8 unsigned char goal2 = (MAZE_SIZE - 1) / 2;
szh66 27:b980fce784ea 9 for (unsigned char i = 0; i < MAZE_SIZE; i++) {
szh66 27:b980fce784ea 10 for (unsigned char j = 0; j < MAZE_SIZE; j++) {
szh66 27:b980fce784ea 11 unsigned char min = min4(manhattan_dist(i, goal1, j, goal1),
szh66 27:b980fce784ea 12 manhattan_dist(i, goal1, j, goal2),
szh66 27:b980fce784ea 13 manhattan_dist(i, goal2, j, goal1),
szh66 27:b980fce784ea 14 manhattan_dist(i, goal2, j, goal2));
ryan_whr 25:7155cb993870 15 maze[i][j] = new Cell(i, j, min);
ryan_whr 25:7155cb993870 16 if (i == MAZE_SIZE - 1) {
ryan_whr 25:7155cb993870 17 maze[i][j]->top_wall = true;
ryan_whr 25:7155cb993870 18 }
ryan_whr 25:7155cb993870 19 if (j == MAZE_SIZE - 1) {
ryan_whr 25:7155cb993870 20 maze[i][j]->right_wall = true;
ryan_whr 25:7155cb993870 21 }
ryan_whr 25:7155cb993870 22 }
ryan_whr 25:7155cb993870 23 }
ryan_whr 25:7155cb993870 24 }
ryan_whr 25:7155cb993870 25
szh66 27:b980fce784ea 26 unsigned char Maze:: manhattan_dist(unsigned char x1, unsigned char x2,
szh66 27:b980fce784ea 27 unsigned char y1, unsigned char y2) {
szh66 27:b980fce784ea 28
ryan_whr 25:7155cb993870 29 return abs(x1 - x2) + abs(y1 - y2);
ryan_whr 25:7155cb993870 30 }
ryan_whr 25:7155cb993870 31
ryan_whr 25:7155cb993870 32 // Function that takes the minimum of the four given distances
szh66 27:b980fce784ea 33 unsigned char Maze:: min4(unsigned char a, unsigned char b, unsigned char c, unsigned char d) {
szh66 27:b980fce784ea 34 unsigned char min;
ryan_whr 25:7155cb993870 35 (a < b) ? min = a : min = b;
ryan_whr 25:7155cb993870 36 if (c < min) min = c;
ryan_whr 25:7155cb993870 37 if (d < min) min = d;
ryan_whr 25:7155cb993870 38 return min;
ryan_whr 25:7155cb993870 39 }
ryan_whr 25:7155cb993870 40
szh66 27:b980fce784ea 41 bool Maze:: is_center(unsigned char x, unsigned char y) {
kolanery 30:daf286ac049f 42 unsigned char goal1 = MAZE_SIZE / 2;
szh66 27:b980fce784ea 43 unsigned char goal2 = (MAZE_SIZE - 1) / 2;
ryan_whr 25:7155cb993870 44 if (manhattan_dist(y, goal1, x, goal1) == 0 ||
szh66 27:b980fce784ea 45 manhattan_dist(y, goal1, x, goal2) == 0 ||
szh66 27:b980fce784ea 46 manhattan_dist(y, goal2, x, goal1) == 0 ||
szh66 27:b980fce784ea 47 manhattan_dist(y, goal2, x, goal2) == 0) {
ryan_whr 25:7155cb993870 48 return true;
ryan_whr 25:7155cb993870 49 }
ryan_whr 25:7155cb993870 50 return false;
ryan_whr 25:7155cb993870 51 }
ryan_whr 25:7155cb993870 52
ryan_whr 25:7155cb993870 53 //============================================================
szh66 27:b980fce784ea 54 unsigned char Mouse:: get_direction() {
ryan_whr 25:7155cb993870 55 return direction;
ryan_whr 25:7155cb993870 56 }
ryan_whr 25:7155cb993870 57
szh66 27:b980fce784ea 58 void Mouse:: set_direction(unsigned char dir) {
ryan_whr 25:7155cb993870 59 direction = dir;
ryan_whr 25:7155cb993870 60 }
ryan_whr 25:7155cb993870 61
szh66 27:b980fce784ea 62
ryan_whr 25:7155cb993870 63 bool Mouse:: get_front_sensor_value(){
ryan_whr 25:7155cb993870 64 bool has_wall;
ryan_whr 25:7155cb993870 65 if(direction == NORTH){
ryan_whr 25:7155cb993870 66 if(mouse_x == MAZE_SIZE-1){ //if the mouse is at the topest row;
ryan_whr 25:7155cb993870 67 return 1;
ryan_whr 25:7155cb993870 68 }
ryan_whr 25:7155cb993870 69 // TODO: change to check front sensor value.
ryan_whr 25:7155cb993870 70 // TODO: has_wall = reference_maze->maze[mouse_x][mouse_y]->top_wall;
ryan_whr 25:7155cb993870 71 has_wall = driver->has_front_wall();
ryan_whr 25:7155cb993870 72 detected_maze->maze[mouse_x][mouse_y]->top_wall = has_wall;
ryan_whr 25:7155cb993870 73 }
ryan_whr 25:7155cb993870 74 else if(direction == SOUTH){
ryan_whr 25:7155cb993870 75 if(mouse_x == 0){
ryan_whr 25:7155cb993870 76 return 1;
ryan_whr 25:7155cb993870 77 }
ryan_whr 25:7155cb993870 78 // TODO: has_wall = reference_maze->maze[mouse_x-1][mouse_y]->top_wall; //its bottom cell's upper wall
ryan_whr 25:7155cb993870 79 has_wall = driver->has_front_wall();
ryan_whr 25:7155cb993870 80 detected_maze->maze[mouse_x-1][mouse_y]->top_wall = has_wall;
ryan_whr 25:7155cb993870 81 }
ryan_whr 25:7155cb993870 82 else if(direction == EAST){
ryan_whr 25:7155cb993870 83 if(mouse_y == MAZE_SIZE-1)
ryan_whr 25:7155cb993870 84 return 1;
ryan_whr 25:7155cb993870 85 has_wall = driver->has_front_wall();
ryan_whr 25:7155cb993870 86 detected_maze->maze[mouse_x][mouse_y]->right_wall = has_wall;
ryan_whr 25:7155cb993870 87 }
ryan_whr 25:7155cb993870 88 else{ //WEST
ryan_whr 25:7155cb993870 89 if(mouse_y == 0)
ryan_whr 25:7155cb993870 90 return 1;
ryan_whr 25:7155cb993870 91 has_wall = driver->has_front_wall();
ryan_whr 25:7155cb993870 92 detected_maze->maze[mouse_x][mouse_y-1]->right_wall = has_wall;
ryan_whr 25:7155cb993870 93 }
ryan_whr 25:7155cb993870 94 return has_wall;
ryan_whr 25:7155cb993870 95 }
ryan_whr 25:7155cb993870 96
ryan_whr 25:7155cb993870 97 bool Mouse:: get_left_sensor_value(){
ryan_whr 25:7155cb993870 98 bool has_wall;
ryan_whr 25:7155cb993870 99
ryan_whr 25:7155cb993870 100 if(direction == NORTH){
ryan_whr 25:7155cb993870 101 if(mouse_y == 0) //if the mouse is at the left_most column
ryan_whr 25:7155cb993870 102 return 1;
ryan_whr 25:7155cb993870 103
ryan_whr 25:7155cb993870 104 // TODO: has_wall = reference_maze->maze[mouse_x][mouse_y-1]->right_wall;
ryan_whr 25:7155cb993870 105 has_wall = driver->has_left_wall();
ryan_whr 25:7155cb993870 106 detected_maze->maze[mouse_x][mouse_y-1]->right_wall = has_wall;
ryan_whr 25:7155cb993870 107
ryan_whr 25:7155cb993870 108 }
ryan_whr 25:7155cb993870 109 else if(direction == SOUTH){
ryan_whr 25:7155cb993870 110 if(mouse_y == MAZE_SIZE-1)
ryan_whr 25:7155cb993870 111 return 1;
ryan_whr 25:7155cb993870 112
ryan_whr 25:7155cb993870 113 // TODO: has_wall = reference_maze->maze[mouse_x][mouse_y]->right_wall;
ryan_whr 25:7155cb993870 114 has_wall = driver->has_left_wall();
ryan_whr 25:7155cb993870 115 detected_maze->maze[mouse_x][mouse_y]->right_wall = has_wall;
ryan_whr 25:7155cb993870 116
ryan_whr 25:7155cb993870 117 }
ryan_whr 25:7155cb993870 118 else if(direction == EAST){
ryan_whr 25:7155cb993870 119 if(mouse_x == MAZE_SIZE-1)
ryan_whr 25:7155cb993870 120 return 1;
ryan_whr 25:7155cb993870 121
ryan_whr 25:7155cb993870 122 // TODO: has_wall = reference_maze->maze[mouse_x][mouse_y]->top_wall;
ryan_whr 25:7155cb993870 123 has_wall = driver->has_left_wall();
ryan_whr 25:7155cb993870 124 detected_maze->maze[mouse_x][mouse_y]->top_wall = has_wall;
ryan_whr 25:7155cb993870 125
ryan_whr 25:7155cb993870 126 }
ryan_whr 25:7155cb993870 127 else{
ryan_whr 25:7155cb993870 128 if(mouse_x == 0)
ryan_whr 25:7155cb993870 129 return 1;
ryan_whr 25:7155cb993870 130 // TODO: has_wall = reference_maze->maze[mouse_x-1][mouse_y]->top_wall;
ryan_whr 25:7155cb993870 131 has_wall = driver->has_left_wall();
ryan_whr 25:7155cb993870 132 detected_maze->maze[mouse_x-1][mouse_y]->top_wall = has_wall;
ryan_whr 25:7155cb993870 133 }
ryan_whr 25:7155cb993870 134
ryan_whr 25:7155cb993870 135 return has_wall;
ryan_whr 25:7155cb993870 136 }
ryan_whr 25:7155cb993870 137
ryan_whr 25:7155cb993870 138 bool Mouse:: get_right_sensor_value(){
ryan_whr 25:7155cb993870 139
ryan_whr 25:7155cb993870 140 bool has_wall;
ryan_whr 25:7155cb993870 141
ryan_whr 25:7155cb993870 142 if(direction == NORTH){
ryan_whr 25:7155cb993870 143 if(mouse_y == MAZE_SIZE-1) //if the mouse is at the right_most column
ryan_whr 25:7155cb993870 144 return 1;
ryan_whr 25:7155cb993870 145
ryan_whr 25:7155cb993870 146 // TODO: has_wall = reference_maze->maze[mouse_x][mouse_y]->right_wall;
ryan_whr 25:7155cb993870 147 has_wall = driver->has_right_wall();
ryan_whr 25:7155cb993870 148 detected_maze->maze[mouse_x][mouse_y]->right_wall = has_wall;
ryan_whr 25:7155cb993870 149
ryan_whr 25:7155cb993870 150 }
ryan_whr 25:7155cb993870 151 else if(direction == SOUTH){
ryan_whr 25:7155cb993870 152 if(mouse_y == 0)
ryan_whr 25:7155cb993870 153 return 1;
ryan_whr 25:7155cb993870 154
ryan_whr 25:7155cb993870 155 // TODO: has_wall = reference_maze->maze[mouse_x][mouse_y-1]->right_wall;
ryan_whr 25:7155cb993870 156 has_wall = driver->has_right_wall();
ryan_whr 25:7155cb993870 157 detected_maze->maze[mouse_x][mouse_y-1]->right_wall = has_wall;
ryan_whr 25:7155cb993870 158
ryan_whr 25:7155cb993870 159 }
ryan_whr 25:7155cb993870 160 else if(direction == EAST){
ryan_whr 25:7155cb993870 161 if(mouse_x == 0)
ryan_whr 25:7155cb993870 162 return 1;
ryan_whr 25:7155cb993870 163
ryan_whr 25:7155cb993870 164 // TODO: has_wall = reference_maze->maze[mouse_x-1][mouse_y]->top_wall;
ryan_whr 25:7155cb993870 165 has_wall = driver->has_right_wall();
ryan_whr 25:7155cb993870 166 detected_maze->maze[mouse_x-1][mouse_y]->top_wall = has_wall;
ryan_whr 25:7155cb993870 167 }
ryan_whr 25:7155cb993870 168 else{
ryan_whr 25:7155cb993870 169 if(mouse_x == MAZE_SIZE-1)
ryan_whr 25:7155cb993870 170 return 1;
ryan_whr 25:7155cb993870 171
ryan_whr 25:7155cb993870 172 // TODO: has_wall = reference_maze->maze[mouse_x][mouse_y]->top_wall;
ryan_whr 25:7155cb993870 173 has_wall = driver->has_right_wall();
ryan_whr 25:7155cb993870 174 detected_maze->maze[mouse_x][mouse_y]->top_wall = has_wall;
ryan_whr 25:7155cb993870 175
ryan_whr 25:7155cb993870 176 }
ryan_whr 25:7155cb993870 177
ryan_whr 25:7155cb993870 178 return has_wall;
ryan_whr 25:7155cb993870 179
ryan_whr 25:7155cb993870 180 }
ryan_whr 25:7155cb993870 181
ryan_whr 25:7155cb993870 182
ryan_whr 25:7155cb993870 183 // the front sensor is 0, then the mouse can move in this direction
szh66 27:b980fce784ea 184 bool Mouse:: can_move() {
ryan_whr 25:7155cb993870 185 //in the real world, instead of using 0, we need to set some constant
szh66 27:b980fce784ea 186 //as a threshold for the sensor value;
ryan_whr 25:7155cb993870 187 return (get_front_sensor_value()==0);
ryan_whr 25:7155cb993870 188 }
ryan_whr 25:7155cb993870 189
ryan_whr 25:7155cb993870 190
ryan_whr 25:7155cb993870 191 //Assumption: the maze[0][y] and maze[x][0] has left wall and bottom wall
szh66 27:b980fce784ea 192 void Mouse:: check_open_neighbor() {
ryan_whr 25:7155cb993870 193 right_sensor = get_right_sensor_value();
ryan_whr 25:7155cb993870 194 front_sensor = get_front_sensor_value();
ryan_whr 25:7155cb993870 195 left_sensor = get_left_sensor_value();
ryan_whr 25:7155cb993870 196
ryan_whr 25:7155cb993870 197
szh66 27:b980fce784ea 198 if(direction == NORTH) {
szh66 27:b980fce784ea 199 if(prev == SOUTH) {
ryan_whr 25:7155cb993870 200 south_open = 1;
ryan_whr 25:7155cb993870 201 }
ryan_whr 25:7155cb993870 202
ryan_whr 25:7155cb993870 203 north_open = !front_sensor;
ryan_whr 25:7155cb993870 204 east_open = !right_sensor;
ryan_whr 25:7155cb993870 205 west_open = !left_sensor;
szh66 27:b980fce784ea 206 } else if(direction == SOUTH) {
szh66 27:b980fce784ea 207 if(prev == NORTH) {
ryan_whr 25:7155cb993870 208 north_open = 1;
ryan_whr 25:7155cb993870 209 }
ryan_whr 25:7155cb993870 210
ryan_whr 25:7155cb993870 211 south_open = !front_sensor;
ryan_whr 25:7155cb993870 212 east_open = !left_sensor;
ryan_whr 25:7155cb993870 213 west_open = !right_sensor;
szh66 27:b980fce784ea 214 } else if(direction == EAST) {
szh66 27:b980fce784ea 215 if(prev == WEST) {
ryan_whr 25:7155cb993870 216 west_open = 1;
ryan_whr 25:7155cb993870 217 }
ryan_whr 25:7155cb993870 218
ryan_whr 25:7155cb993870 219 south_open = !right_sensor;
ryan_whr 25:7155cb993870 220 east_open = !front_sensor;
ryan_whr 25:7155cb993870 221 north_open = !left_sensor;
szh66 27:b980fce784ea 222 } else { //WEST
szh66 27:b980fce784ea 223 if(prev == EAST) {
ryan_whr 25:7155cb993870 224 east_open = 1;
ryan_whr 25:7155cb993870 225 }
ryan_whr 25:7155cb993870 226
ryan_whr 25:7155cb993870 227 south_open = !left_sensor;
ryan_whr 25:7155cb993870 228 west_open = !front_sensor;
ryan_whr 25:7155cb993870 229 north_open = !right_sensor;
ryan_whr 25:7155cb993870 230 }
szh66 27:b980fce784ea 231
ryan_whr 25:7155cb993870 232 }
ryan_whr 25:7155cb993870 233
szh66 27:b980fce784ea 234 unsigned char Mouse:: min_open_neighbor(vector<Cell*> cells) {
ryan_whr 25:7155cb993870 235 int min = MAX;
ryan_whr 25:7155cb993870 236 for (vector<Cell *>::iterator it = cells.begin(); it != cells.end(); it++) {
ryan_whr 25:7155cb993870 237 if ((*it)->dist < min) {
ryan_whr 25:7155cb993870 238 min = (*it)->dist;
ryan_whr 25:7155cb993870 239 }
ryan_whr 25:7155cb993870 240 }
ryan_whr 25:7155cb993870 241 return min;
ryan_whr 25:7155cb993870 242 }
ryan_whr 25:7155cb993870 243
szh66 27:b980fce784ea 244 void Mouse:: update_distance() {
ryan_whr 25:7155cb993870 245 Cell * curr;
ryan_whr 25:7155cb993870 246 vector<Cell*> neighbor;
ryan_whr 25:7155cb993870 247 vector<Cell*> open_neighbor;
szh66 27:b980fce784ea 248 unsigned char min_dist;
szh66 27:b980fce784ea 249 unsigned char x, y;
szh66 27:b980fce784ea 250 unsigned char count = 0;
ryan_whr 25:7155cb993870 251 stk.push_back(detected_maze->maze[mouse_x][mouse_y]);
ryan_whr 25:7155cb993870 252
szh66 27:b980fce784ea 253 while(!stk.empty()) {
ryan_whr 25:7155cb993870 254 curr = stk.back();
ryan_whr 25:7155cb993870 255 stk.pop_back();
ryan_whr 25:7155cb993870 256 x = curr->x;
ryan_whr 25:7155cb993870 257 y = curr->y;
ryan_whr 25:7155cb993870 258
szh66 27:b980fce784ea 259 if(x < MAZE_SIZE-1) { //top cell
ryan_whr 25:7155cb993870 260 neighbor.push_back(detected_maze->maze[x+1][y]);
szh66 27:b980fce784ea 261 if(!curr->top_wall) {
ryan_whr 25:7155cb993870 262 open_neighbor.push_back(detected_maze->maze[x + 1][y]);
ryan_whr 25:7155cb993870 263 }
ryan_whr 25:7155cb993870 264 }
ryan_whr 25:7155cb993870 265
szh66 27:b980fce784ea 266 if(y > 0) { //left cell
szh66 27:b980fce784ea 267 neighbor.push_back(detected_maze->maze[x][y-1]);
ryan_whr 25:7155cb993870 268
ryan_whr 25:7155cb993870 269 if (!(detected_maze->maze[x][y - 1]->right_wall)) {
ryan_whr 25:7155cb993870 270 open_neighbor.push_back(detected_maze->maze[x][y - 1]);
ryan_whr 25:7155cb993870 271 }
ryan_whr 25:7155cb993870 272 }
ryan_whr 25:7155cb993870 273
szh66 27:b980fce784ea 274 if(x > 0) { //bottom cell
ryan_whr 25:7155cb993870 275 neighbor.push_back(detected_maze->maze[x-1][y]);
ryan_whr 25:7155cb993870 276 if (!(detected_maze->maze[x-1][y]->top_wall)) {
ryan_whr 25:7155cb993870 277 open_neighbor.push_back(detected_maze->maze[x-1][y]);
ryan_whr 25:7155cb993870 278 }
ryan_whr 25:7155cb993870 279 }
ryan_whr 25:7155cb993870 280
szh66 27:b980fce784ea 281 if(y < MAZE_SIZE -1) { //right_cell
ryan_whr 25:7155cb993870 282 neighbor.push_back(detected_maze->maze[x][y+1]);
szh66 27:b980fce784ea 283 if(!curr->right_wall) {
ryan_whr 25:7155cb993870 284 open_neighbor.push_back(detected_maze->maze[x][y+1]);
ryan_whr 25:7155cb993870 285 }
ryan_whr 25:7155cb993870 286 }
ryan_whr 25:7155cb993870 287 if (open_neighbor.empty()) {
ryan_whr 25:7155cb993870 288 neighbor.clear();
ryan_whr 25:7155cb993870 289 continue;
ryan_whr 25:7155cb993870 290 }
ryan_whr 25:7155cb993870 291 min_dist = min_open_neighbor(open_neighbor);
ryan_whr 25:7155cb993870 292 open_neighbor.clear();
ryan_whr 25:7155cb993870 293
ryan_whr 25:7155cb993870 294 if (curr->dist - 1 != min_dist) {
ryan_whr 25:7155cb993870 295 curr->dist = min_dist + 1;
ryan_whr 25:7155cb993870 296 for (vector<Cell *>::iterator it = neighbor.begin(); it != neighbor.end(); it++) {
szh66 27:b980fce784ea 297 if (!detected_maze->is_center((*it)->x, (*it)->y)) {
ryan_whr 25:7155cb993870 298 stk.push_back(*it);
ryan_whr 25:7155cb993870 299 }
ryan_whr 25:7155cb993870 300 }
ryan_whr 25:7155cb993870 301 neighbor.clear();
ryan_whr 25:7155cb993870 302 }
ryan_whr 25:7155cb993870 303 count++;
ryan_whr 25:7155cb993870 304 }
ryan_whr 25:7155cb993870 305 }
ryan_whr 25:7155cb993870 306
kolanery 30:daf286ac049f 307 void Mouse::clear_stack() {
kolanery 30:daf286ac049f 308 stk.clear();
kolanery 30:daf286ac049f 309 }
kolanery 30:daf286ac049f 310
szh66 27:b980fce784ea 311 void Mouse:: move_one_cell() {
ryan_whr 25:7155cb993870 312 check_open_neighbor();
ryan_whr 25:7155cb993870 313 update_distance();
ryan_whr 25:7155cb993870 314 int min_dist = MAX;
szh66 27:b980fce784ea 315 unsigned char temp;
szh66 27:b980fce784ea 316 if(north_open) {
ryan_whr 25:7155cb993870 317 temp = detected_maze->maze[mouse_x+1][mouse_y]->dist;
ryan_whr 25:7155cb993870 318 if (temp < min_dist) {
ryan_whr 25:7155cb993870 319 min_dist = temp;
ryan_whr 25:7155cb993870 320 direction = NORTH;
ryan_whr 25:7155cb993870 321 prev = SOUTH;
ryan_whr 25:7155cb993870 322 }
ryan_whr 25:7155cb993870 323 }
ryan_whr 25:7155cb993870 324
ryan_whr 25:7155cb993870 325
szh66 27:b980fce784ea 326 if(east_open) {
ryan_whr 25:7155cb993870 327 temp = detected_maze->maze[mouse_x][mouse_y+1]->dist;
szh66 27:b980fce784ea 328 if (temp < min_dist) {
ryan_whr 25:7155cb993870 329 min_dist = temp;
ryan_whr 25:7155cb993870 330 direction = EAST;
ryan_whr 25:7155cb993870 331 prev = WEST;
ryan_whr 25:7155cb993870 332 }
ryan_whr 25:7155cb993870 333 }
szh66 27:b980fce784ea 334
szh66 27:b980fce784ea 335 if(south_open) {
ryan_whr 25:7155cb993870 336 temp = detected_maze->maze[mouse_x-1][mouse_y]->dist;
szh66 27:b980fce784ea 337 if (temp < min_dist) {
ryan_whr 25:7155cb993870 338 min_dist = temp;
ryan_whr 25:7155cb993870 339 direction = SOUTH;
ryan_whr 25:7155cb993870 340 prev = NORTH;
ryan_whr 25:7155cb993870 341 }
ryan_whr 25:7155cb993870 342 }
szh66 27:b980fce784ea 343 if(west_open) {
ryan_whr 25:7155cb993870 344 temp = detected_maze->maze[mouse_x][mouse_y-1]->dist;
ryan_whr 25:7155cb993870 345 if (temp < min_dist) {
ryan_whr 25:7155cb993870 346 min_dist = temp;
ryan_whr 25:7155cb993870 347 direction = WEST;
ryan_whr 25:7155cb993870 348 prev = EAST;
ryan_whr 25:7155cb993870 349 }
ryan_whr 25:7155cb993870 350 }
ryan_whr 25:7155cb993870 351 //Move
szh66 27:b980fce784ea 352 if(direction == NORTH) {
ryan_whr 25:7155cb993870 353 mouse_x += 1;
szh66 27:b980fce784ea 354 } else if(direction == SOUTH) {
ryan_whr 25:7155cb993870 355 mouse_x -= 1;
szh66 27:b980fce784ea 356 } else if (direction == EAST) {
ryan_whr 25:7155cb993870 357 mouse_y += 1;
szh66 27:b980fce784ea 358 } else { //WEST
ryan_whr 25:7155cb993870 359 mouse_y -= 1;
ryan_whr 25:7155cb993870 360 }
szh66 27:b980fce784ea 361 north_open = south_open = east_open = west_open = 0;
szh66 27:b980fce784ea 362 }
ryan_whr 25:7155cb993870 363
szh66 27:b980fce784ea 364 bool Mouse::center_found() {
szh66 27:b980fce784ea 365 return detected_maze->is_center(mouse_x, mouse_y);
ryan_whr 25:7155cb993870 366 }
ryan_whr 25:7155cb993870 367
kolanery 28:600932142201 368 int Mouse::get_next_cell_direction() {
kolanery 28:600932142201 369 return direction;
kolanery 28:600932142201 370 }
kolanery 28:600932142201 371
kolanery 28:600932142201 372 int Mouse::get_prev_direction() {
kolanery 28:600932142201 373 return prev_mouse_dir_print;
kolanery 28:600932142201 374 }
kolanery 28:600932142201 375
szh66 27:b980fce784ea 376 int Mouse:: solve_maze() {
szh66 27:b980fce784ea 377 const unsigned char STRAIGHT = 0, LEFT = 1, RIGHT = 2, UTURN = 3;
kolanery 28:600932142201 378
kolanery 28:600932142201 379 prev_mouse_dir_print = prev_mouse_dir;
kolanery 28:600932142201 380
kolanery 28:600932142201 381 //
ryan_whr 25:7155cb993870 382
ryan_whr 25:7155cb993870 383 //while the mouse has not find the center of the Maze
kolanery 28:600932142201 384 move_one_cell();
kolanery 28:600932142201 385 unsigned char state = 5;
kolanery 28:600932142201 386
kolanery 28:600932142201 387
kolanery 28:600932142201 388 if (prev_mouse_dir == direction) {
kolanery 30:daf286ac049f 389 if (prev_mouse_dir == NORTH && direction == NORTH) {
kolanery 30:daf286ac049f 390 can_reset_mouse = true;
kolanery 30:daf286ac049f 391 }
kolanery 28:600932142201 392 state = STRAIGHT;
kolanery 28:600932142201 393 }
kolanery 28:600932142201 394 // Mouse is facing south
kolanery 28:600932142201 395 else if (prev_mouse_dir == SOUTH && direction == NORTH) {
kolanery 28:600932142201 396 state = UTURN;
kolanery 28:600932142201 397 prev_mouse_dir = NORTH;
kolanery 28:600932142201 398 } else if (prev_mouse_dir == SOUTH && direction == EAST) {
kolanery 28:600932142201 399 state = LEFT;
kolanery 28:600932142201 400 prev_mouse_dir = EAST;
kolanery 28:600932142201 401 } else if (prev_mouse_dir == SOUTH && direction == WEST) {
kolanery 28:600932142201 402 state = RIGHT;
kolanery 28:600932142201 403 prev_mouse_dir = WEST;
kolanery 28:600932142201 404 }
kolanery 28:600932142201 405 // Mouse is facing north
kolanery 28:600932142201 406 else if (prev_mouse_dir == NORTH && direction == EAST) {
kolanery 28:600932142201 407 state = RIGHT;
kolanery 28:600932142201 408 prev_mouse_dir = EAST;
kolanery 28:600932142201 409 } else if (prev_mouse_dir == NORTH && direction == WEST) {
kolanery 28:600932142201 410 state = LEFT;
kolanery 28:600932142201 411 prev_mouse_dir = WEST;
kolanery 28:600932142201 412 } else if (prev_mouse_dir == NORTH && direction == SOUTH) {
kolanery 28:600932142201 413 state = UTURN;
kolanery 28:600932142201 414 prev_mouse_dir = SOUTH;
kolanery 28:600932142201 415 }
kolanery 28:600932142201 416 // Mouse is facing west
kolanery 28:600932142201 417 else if (prev_mouse_dir == WEST && direction == NORTH) {
kolanery 28:600932142201 418 state = RIGHT;
kolanery 28:600932142201 419 prev_mouse_dir = NORTH;
kolanery 28:600932142201 420 } else if (prev_mouse_dir == WEST && direction == SOUTH) {
kolanery 28:600932142201 421 state = LEFT;
kolanery 28:600932142201 422 prev_mouse_dir = SOUTH;
kolanery 28:600932142201 423 } else if (prev_mouse_dir == WEST && direction == EAST) {
kolanery 28:600932142201 424 state = UTURN;
kolanery 28:600932142201 425 prev_mouse_dir = EAST;
kolanery 28:600932142201 426 }
kolanery 28:600932142201 427 // Mouse is facing west
kolanery 28:600932142201 428 else if (prev_mouse_dir == EAST && direction == NORTH) {
kolanery 28:600932142201 429 state = LEFT;
kolanery 28:600932142201 430 prev_mouse_dir = NORTH;
kolanery 28:600932142201 431 } else if (prev_mouse_dir == EAST && direction == SOUTH) {
kolanery 28:600932142201 432 state = RIGHT;
kolanery 28:600932142201 433 prev_mouse_dir = SOUTH;
kolanery 28:600932142201 434 } else if (prev_mouse_dir == EAST && direction == WEST) {
kolanery 28:600932142201 435 state = UTURN;
kolanery 28:600932142201 436 prev_mouse_dir = WEST;
kolanery 28:600932142201 437 }
kolanery 28:600932142201 438
kolanery 28:600932142201 439
kolanery 28:600932142201 440 return state;
szh66 27:b980fce784ea 441 }
szh66 27:b980fce784ea 442
kolanery 28:600932142201 443
kolanery 28:600932142201 444
szh66 27:b980fce784ea 445 Mouse:: Mouse(DriveControl * prev_driver) {
kolanery 30:daf286ac049f 446 can_reset_mouse = false;
szh66 27:b980fce784ea 447 driver = prev_driver;
szh66 27:b980fce784ea 448 detected_maze = new Maze();
szh66 27:b980fce784ea 449 prev = (unsigned char) 5;
szh66 27:b980fce784ea 450 direction = NORTH;
szh66 27:b980fce784ea 451 mouse_x = mouse_y = 0;
szh66 27:b980fce784ea 452 north_open = south_open = east_open = west_open = 0;
kolanery 28:600932142201 453 }