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