Revision 754da79f
ID | 754da79f59a021f22a2ce09dae007b7d85a2521e |
Added the behavior to behavior list and cmakelists and made things compile.
scout/libscout/src/behaviors/maze_solve.cpp | ||
---|---|---|
33 | 33 |
// we assume we are facing right, that affects where we store |
34 | 34 |
// wall information |
35 | 35 |
// -1 for wall, 0 for unseen, 1 for traveled, 2 for critical |
36 |
static int map[60][60]; |
|
37 | 36 |
#define WALL -1 |
38 | 37 |
#define UNSEEN 0 |
39 | 38 |
#define SEEN 1 |
... | ... | |
43 | 42 |
#define RIGHT 1 |
44 | 43 |
#define DOWN 2 |
45 | 44 |
#define LEFT 3 |
45 |
|
|
46 | 46 |
void maze_solve::run(){ |
47 | 47 |
|
48 | 48 |
// TODO:first initialize map to all 0's |
... | ... | |
51 | 51 |
|
52 | 52 |
bool maze_solve::solve(int row, int col, int dir) |
53 | 53 |
{ |
54 |
int intial_dir = dir; |
|
54 |
int initial_dir = dir;
|
|
55 | 55 |
|
56 | 56 |
// use backtracking to solve the maze |
57 | 57 |
if (at_destination()) |
... | ... | |
66 | 66 |
{ |
67 | 67 |
// Turn up. |
68 | 68 |
turn_from_to(dir, UP); |
69 |
line_follow();
|
|
69 |
follow_line();
|
|
70 | 70 |
// Solve recursively. |
71 | 71 |
bool solved = solve(row-1, col, DOWN); |
72 | 72 |
if (solved) |
... | ... | |
84 | 84 |
{ |
85 | 85 |
// Turn right. |
86 | 86 |
turn_from_to(dir, RIGHT); |
87 |
line_follow();
|
|
87 |
follow_line();
|
|
88 | 88 |
// Solve recursively. |
89 | 89 |
bool solved = solve(row, col+1, LEFT); |
90 | 90 |
if (solved) |
... | ... | |
102 | 102 |
{ |
103 | 103 |
// Turn down. |
104 | 104 |
turn_from_to(dir, DOWN); |
105 |
line_follow();
|
|
105 |
follow_line();
|
|
106 | 106 |
// Solve recursively. |
107 | 107 |
bool solved = solve(row+1, col, UP); |
108 | 108 |
if (solved) |
... | ... | |
120 | 120 |
{ |
121 | 121 |
// Turn down. |
122 | 122 |
turn_from_to(dir, LEFT); |
123 |
line_follow();
|
|
123 |
follow_line();
|
|
124 | 124 |
// Solve recursively. |
125 | 125 |
bool solved = solve(row, col-1, RIGHT); |
126 | 126 |
if (solved) |
... | ... | |
136 | 136 |
// we have exhausted all the options. This path is clearly a |
137 | 137 |
// dead end. go back to where we come from and return false. |
138 | 138 |
turn_from_to(dir, initial_dir); |
139 |
line_follow();
|
|
139 |
follow_line();
|
|
140 | 140 |
return false; |
141 | 141 |
} |
142 | 142 |
|
... | ... | |
144 | 144 |
// into it intended direction |
145 | 145 |
void maze_solve::turn_from_to(int current_dir, int intended_dir) { |
146 | 146 |
switch ((4 + intended_dir - current_dir) % 4) |
147 |
{ |
|
147 | 148 |
case 0: |
148 | 149 |
spot_turn(); |
149 | 150 |
break; |
... | ... | |
156 | 157 |
case 3: |
157 | 158 |
turn_left(); |
158 | 159 |
break; |
160 |
} |
|
159 | 161 |
} |
160 | 162 |
|
161 | 163 |
void maze_solve::look_around(int row, int col, int dir) |
... | ... | |
165 | 167 |
// there is a wall into the map |
166 | 168 |
// stores at row col 2 if point is critical, 1 otherwise |
167 | 169 |
|
168 |
int* readings = get_sonar_readings(); |
|
170 |
int* readings = sonar->get_sonar_readings();
|
|
169 | 171 |
|
170 | 172 |
// Look to the right. |
171 | 173 |
int right_distance = (readings[1] + readings[0] + readings[47])/3; |
Also available in: Unified diff