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