Revision 7b5ea072
maze_solve now works. Need testing on other machines to confirm.
scout/libscout/src/test_behaviors/maze_solve.cpp | ||
---|---|---|
76 | 76 |
ROS_INFO("NO! The maze is unsolvable"); |
77 | 77 |
} |
78 | 78 |
|
79 |
void maze_solve::adjust() |
|
80 |
{ |
|
81 |
|
|
82 |
ROS_INFO("adjusting...!"); |
|
83 |
motors->set_sides(-30, -30, MOTOR_ABSOLUTE); |
|
84 |
ros::Duration(0.3).sleep(); |
|
85 |
halt(); |
|
86 |
/* int front_d = 0; |
|
87 |
vector<uint32_t> readings; |
|
88 |
int* sonar_readings = sonar->get_sonar_readings(); |
|
89 |
spinOnce(); |
|
90 |
int ideal_d = sonar_readings[36]/250*250; |
|
91 |
bool adjusted = false; |
|
92 |
while (ok() && !adjusted) |
|
93 |
{ |
|
94 |
sonar_readings = sonar->get_sonar_readings(); |
|
95 |
spinOnce(); |
|
96 |
front_d = sonar_readings[36]; |
|
97 |
ROS_INFO("adjusting from %d to %d", front_d, ideal_d); |
|
98 |
if (front_d > ideal_d) |
|
99 |
{ |
|
100 |
motors->set_sides(-10, -10, MOTOR_ABSOLUTE); |
|
101 |
readings = linesensor->query(); |
|
102 |
} |
|
103 |
else |
|
104 |
{ |
|
105 |
adjusted = true; |
|
106 |
halt(); |
|
107 |
} |
|
108 |
} */ |
|
109 |
} |
|
110 | 79 |
|
111 | 80 |
bool maze_solve::solve(int row, int col, int dir, bool root) |
112 | 81 |
{ |
... | ... | |
137 | 106 |
// Turn up. |
138 | 107 |
turn_from_to(dir, UP); |
139 | 108 |
follow_line(); |
140 |
//adjust(); |
|
141 | 109 |
// Solve recursively. |
142 | 110 |
bool solved = solve(row-2, col, DOWN, false); |
143 | 111 |
if (solved) |
... | ... | |
157 | 125 |
// Turn down. |
158 | 126 |
turn_from_to(dir, DOWN); |
159 | 127 |
follow_line(); |
160 |
//adjust(); |
|
161 | 128 |
// Solve recursively. |
162 | 129 |
bool solved = solve(row+2, col, UP, false); |
163 | 130 |
if (solved) |
... | ... | |
177 | 144 |
// Turn right. |
178 | 145 |
turn_from_to(dir, RIGHT); |
179 | 146 |
follow_line(); |
180 |
//adjust(); |
|
181 | 147 |
// Solve recursively. |
182 | 148 |
bool solved = solve(row, col+2, LEFT, false); |
183 | 149 |
if (solved) |
... | ... | |
197 | 163 |
// Turn down. |
198 | 164 |
turn_from_to(dir, LEFT); |
199 | 165 |
follow_line(); |
200 |
//adjust(); |
|
201 | 166 |
// Solve recursively. |
202 | 167 |
bool solved = solve(row, col-2, RIGHT, false); |
203 | 168 |
if (solved) |
... | ... | |
372 | 337 |
{ |
373 | 338 |
vector<uint32_t> readings = linesensor->query(); |
374 | 339 |
|
375 |
|
|
340 |
//changed values so it detects solution |
|
376 | 341 |
if ( readings[0] > 75 && |
377 | 342 |
readings[1] > 75 && |
378 | 343 |
readings[2] < 55 && |
Also available in: Unified diff