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