Revision c840fbe6
Changed linefollowing so that turns start working once again. Also fixed some bugs with maze solving.
scout/libscout/src/behaviors/maze_solve.cpp | ||
---|---|---|
43 | 43 |
#define DOWN 2 |
44 | 44 |
#define LEFT 3 |
45 | 45 |
|
46 |
Duration sonar_update_time(1.0); |
|
47 |
|
|
46 | 48 |
void maze_solve::run(){ |
47 | 49 |
|
48 | 50 |
// TODO:first initialize map to all 0's |
... | ... | |
73 | 75 |
{ |
74 | 76 |
int initial_dir = dir; |
75 | 77 |
|
78 |
ROS_INFO("I am at direction %d", dir); |
|
79 |
|
|
76 | 80 |
// use backtracking to solve the maze |
77 | 81 |
if (at_destination()) |
78 | 82 |
return true; |
79 | 83 |
|
84 |
// Wait for sonar to update. |
|
85 |
sonar_update_time.sleep(); |
|
86 |
|
|
80 | 87 |
// this function should fill the adjacent cells around me with |
81 | 88 |
// wall's or paths |
82 | 89 |
while(!look_around(row, col, dir) && ok()) |
... | ... | |
85 | 92 |
} |
86 | 93 |
|
87 | 94 |
/* try go up */ |
88 |
ROS_INFO("GOING UP!"); |
|
89 | 95 |
if (map[row-1][col] != WALL && initial_dir != UP) |
90 | 96 |
{ |
97 |
ROS_INFO("GOING UP!"); |
|
91 | 98 |
// Turn up. |
92 | 99 |
turn_from_to(dir, UP); |
93 | 100 |
follow_line(); |
... | ... | |
104 | 111 |
} |
105 | 112 |
} |
106 | 113 |
/* try right */ |
107 |
ROS_INFO("GOING RIGHT!"); |
|
108 | 114 |
if (map[row][col+1] != WALL && initial_dir != RIGHT) |
109 | 115 |
{ |
116 |
ROS_INFO("GOING RIGHT!"); |
|
110 | 117 |
// Turn right. |
111 | 118 |
turn_from_to(dir, RIGHT); |
112 | 119 |
follow_line(); |
... | ... | |
123 | 130 |
} |
124 | 131 |
} |
125 | 132 |
/* try down */ |
126 |
ROS_INFO("GOING DOWN!"); |
|
127 | 133 |
if (map[row+1][col] != WALL && initial_dir != DOWN) |
128 | 134 |
{ |
135 |
ROS_INFO("GOING DOWN!"); |
|
129 | 136 |
// Turn down. |
130 | 137 |
turn_from_to(dir, DOWN); |
131 | 138 |
follow_line(); |
... | ... | |
142 | 149 |
} |
143 | 150 |
} |
144 | 151 |
/* try left */ |
145 |
ROS_INFO("GOING LEFT!"); |
|
146 | 152 |
if (map[row][col-1] != WALL && initial_dir != LEFT) |
147 | 153 |
{ |
154 |
ROS_INFO("GOING LEFT!"); |
|
148 | 155 |
// Turn down. |
149 | 156 |
turn_from_to(dir, LEFT); |
150 | 157 |
follow_line(); |
... | ... | |
176 | 183 |
spot_turn(); |
177 | 184 |
break; |
178 | 185 |
case 1: |
179 |
turn_right();
|
|
186 |
turn_left();
|
|
180 | 187 |
break; |
181 | 188 |
case 2: |
182 | 189 |
turn_straight(); |
183 | 190 |
break; |
184 | 191 |
case 3: |
185 |
turn_left();
|
|
192 |
turn_right();
|
|
186 | 193 |
break; |
187 | 194 |
} |
188 | 195 |
} |
Also available in: Unified diff