Revision 2b0c2534
| ID | 2b0c2534204a4ecf1da101e08d9c51fbfb9e8062 |
Called spinOnce to update sonar readings.
| scout/libscout/src/behaviors/maze_solve.cpp | ||
|---|---|---|
| 46 | 46 |
void maze_solve::run(){
|
| 47 | 47 |
|
| 48 | 48 |
// TODO:first initialize map to all 0's |
| 49 |
solve(25,25, RIGHT); |
|
| 49 |
ROS_INFO("Starting to solve the maze");
|
|
| 50 |
// Go up to the first line. |
|
| 51 |
follow_line(); |
|
| 52 |
// Turn the sonar on. |
|
| 53 |
sonar->set_on(); |
|
| 54 |
sonar->set_range(0, 23); |
|
| 55 |
|
|
| 56 |
// Wait for the sonar to initialize. |
|
| 57 |
while(!look_around(25, 25, RIGHT) && ok()) |
|
| 58 |
{
|
|
| 59 |
spinOnce(); |
|
| 60 |
} |
|
| 61 |
|
|
| 62 |
// Solve the maze |
|
| 63 |
bool finished = solve(25,25, RIGHT); |
|
| 64 |
|
|
| 65 |
// Check and report final condition. |
|
| 66 |
if (finished) |
|
| 67 |
ROS_INFO("YAY! I have solved the maze");
|
|
| 68 |
else |
|
| 69 |
ROS_INFO("NO! The maze is unsolvable");
|
|
| 50 | 70 |
} |
| 51 | 71 |
|
| 52 | 72 |
bool maze_solve::solve(int row, int col, int dir) |
| ... | ... | |
| 59 | 79 |
|
| 60 | 80 |
// this function should fill the adjacent cells around me with |
| 61 | 81 |
// wall's or paths |
| 62 |
look_around(row, col, dir); |
|
| 82 |
while(!look_around(row, col, dir) && ok()) |
|
| 83 |
{
|
|
| 84 |
spinOnce(); |
|
| 85 |
} |
|
| 63 | 86 |
|
| 64 | 87 |
/* try go up */ |
| 88 |
ROS_INFO("GOING UP!");
|
|
| 65 | 89 |
if (map[row-1][col] != WALL && initial_dir != UP) |
| 66 | 90 |
{
|
| 67 | 91 |
// Turn up. |
| ... | ... | |
| 80 | 104 |
} |
| 81 | 105 |
} |
| 82 | 106 |
/* try right */ |
| 107 |
ROS_INFO("GOING RIGHT!");
|
|
| 83 | 108 |
if (map[row][col+1] != WALL && initial_dir != RIGHT) |
| 84 | 109 |
{
|
| 85 | 110 |
// Turn right. |
| ... | ... | |
| 98 | 123 |
} |
| 99 | 124 |
} |
| 100 | 125 |
/* try down */ |
| 126 |
ROS_INFO("GOING DOWN!");
|
|
| 101 | 127 |
if (map[row+1][col] != WALL && initial_dir != DOWN) |
| 102 | 128 |
{
|
| 103 | 129 |
// Turn down. |
| ... | ... | |
| 116 | 142 |
} |
| 117 | 143 |
} |
| 118 | 144 |
/* try left */ |
| 145 |
ROS_INFO("GOING LEFT!");
|
|
| 119 | 146 |
if (map[row][col-1] != WALL && initial_dir != LEFT) |
| 120 | 147 |
{
|
| 121 | 148 |
// Turn down. |
| ... | ... | |
| 160 | 187 |
} |
| 161 | 188 |
} |
| 162 | 189 |
|
| 163 |
void maze_solve::look_around(int row, int col, int dir)
|
|
| 190 |
bool maze_solve::look_around(int row, int col, int dir)
|
|
| 164 | 191 |
{
|
| 165 | 192 |
// look around current place using sonar |
| 166 | 193 |
// store whether or not |
| ... | ... | |
| 168 | 195 |
// stores at row col 2 if point is critical, 1 otherwise |
| 169 | 196 |
|
| 170 | 197 |
int* readings = sonar->get_sonar_readings(); |
| 198 |
spinOnce(); |
|
| 171 | 199 |
|
| 172 |
// Look to the right. |
|
| 173 |
int right_distance = (readings[1] + readings[0] + readings[47])/3; |
|
| 200 |
/* |
|
| 201 |
// Look to the left. |
|
| 202 |
int left_distance = (readings[1] + readings[0] + readings[47])/3; |
|
| 174 | 203 |
// Look to the front. |
| 175 |
int front_distance = (readings[11] + readings[12] + readings[13])/3; |
|
| 204 |
int front_distance = (readings[35] + readings[36] + readings[37])/3; |
|
| 205 |
// Look to the right. |
|
| 206 |
int right_distance = (readings[23] + readings[24] + readings[25])/3; |
|
| 207 |
*/ |
|
| 176 | 208 |
// Look to the left. |
| 177 |
int left_distance = (readings[23] + readings[24] + readings[25])/3; |
|
| 209 |
int left_distance = readings[0]; |
|
| 210 |
// Look to the front. |
|
| 211 |
int front_distance = readings[36]; |
|
| 212 |
// Look to the right. |
|
| 213 |
int right_distance = readings[24]; |
|
| 214 |
|
|
| 215 |
ROS_INFO("front: %d left: %d right: %d", front_distance, left_distance, right_distance);
|
|
| 216 |
|
|
| 217 |
if (right_distance == 0 || front_distance == 0 || left_distance == 0) |
|
| 218 |
return false; |
|
| 219 |
|
|
| 178 | 220 |
switch (dir) |
| 179 | 221 |
{
|
| 180 | 222 |
case UP: |
| ... | ... | |
| 206 | 248 |
map[row+1][col] = (right_distance < 500)?WALL:SEEN; |
| 207 | 249 |
break; |
| 208 | 250 |
} |
| 251 |
|
|
| 252 |
return true; |
|
| 209 | 253 |
} |
| 210 | 254 |
|
| 211 | 255 |
bool maze_solve::at_destination() |
Also available in: Unified diff