Statistics
| Branch: | Revision:

root / scout / libscout / src / behaviors / maze_solve.cpp @ a69f6363

History | View | Annotate | Download (7.69 KB)

1 64ee446f Yuyang
/**
2
 * Copyright (c) 2011 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 */
25
26
#include "maze_solve.h"
27
28
using namespace std;
29
30
31
// want to have a minimal working thing, use a big enough 
32
// static array and start in the middle
33
// we assume we are facing right, that affects where we store
34
// wall information
35
// -1 for wall, 0 for unseen, 1 for traveled, 2 for critical
36
#define WALL -1
37
#define UNSEEN 0
38
#define SEEN 1
39
#define CRITICAL 2
40
// facings
41
#define UP 0
42
#define RIGHT 1
43
#define DOWN 2
44
#define LEFT 3
45 754da79f Priya
46 6ee555a3 Priya
Duration sonar_update_time(1.5);
47 c840fbe6 Priya
48 a69f6363 viki
void maze_solve::run()
49
{    
50 64ee446f Yuyang
    // TODO:first initialize map to all 0's
51 2b0c2534 Priya
    ROS_INFO("Starting to solve the maze");
52
    // Go up to the first line.
53
    follow_line();
54
    // Turn the sonar on.
55
    sonar->set_on();
56
    sonar->set_range(0, 23);
57
58
    // Wait for the sonar to initialize.
59
    while(!look_around(25, 25, RIGHT) && ok())
60
    {
61
      spinOnce();      
62
    }
63
64
    // Solve the maze
65
    bool finished = solve(25,25, RIGHT);
66
67
    // Check and report final condition.
68
    if (finished)
69
        ROS_INFO("YAY! I have solved the maze");
70
    else
71
        ROS_INFO("NO! The maze is unsolvable");
72 64ee446f Yuyang
}
73 f878b5f9 Priya
74
bool maze_solve::solve(int row, int col, int dir)
75
{
76 754da79f Priya
    int initial_dir = dir;
77 f878b5f9 Priya
78 c840fbe6 Priya
    ROS_INFO("I am at direction %d", dir);
79
80 64ee446f Yuyang
    // use backtracking to solve the maze
81 f878b5f9 Priya
    if (at_destination())
82 64ee446f Yuyang
        return true;
83
84 c840fbe6 Priya
    // Wait for sonar to update.
85
    sonar_update_time.sleep();
86
87 f878b5f9 Priya
    // this function should fill the adjacent cells around me with
88
    // wall's or paths
89 2b0c2534 Priya
    while(!look_around(row, col, dir) && ok())
90
    {
91
        spinOnce();
92
    }
93 f878b5f9 Priya
94
    /* try go up */
95
    if (map[row-1][col] != WALL && initial_dir != UP)
96 64ee446f Yuyang
    {
97 c840fbe6 Priya
    ROS_INFO("GOING UP!");
98 f878b5f9 Priya
        // Turn up.
99
        turn_from_to(dir, UP);
100 754da79f Priya
        follow_line();
101 f878b5f9 Priya
        // Solve recursively.
102
        bool solved = solve(row-1, col, DOWN);
103
        if (solved)
104 64ee446f Yuyang
        {
105 f878b5f9 Priya
            return solved;
106 64ee446f Yuyang
        }
107 f878b5f9 Priya
        else
108 64ee446f Yuyang
        {
109 f878b5f9 Priya
            //Update where we are.
110
            dir = UP;
111 64ee446f Yuyang
        }
112 f878b5f9 Priya
    }
113
    /* try right */
114
    if (map[row][col+1] != WALL && initial_dir != RIGHT)
115
    {
116 c840fbe6 Priya
    ROS_INFO("GOING RIGHT!");
117 f878b5f9 Priya
        // Turn right.
118
        turn_from_to(dir, RIGHT);
119 754da79f Priya
        follow_line();
120 f878b5f9 Priya
        // Solve recursively.
121
        bool solved = solve(row, col+1, LEFT);
122
        if (solved)
123 64ee446f Yuyang
        {
124 f878b5f9 Priya
            return solved;
125
        }
126
        else
127
        {
128
            //Update where we are.
129
            dir = RIGHT;
130 64ee446f Yuyang
        }
131
    }
132 f878b5f9 Priya
    /* try down */
133
    if (map[row+1][col] != WALL && initial_dir != DOWN)
134
    {
135 c840fbe6 Priya
    ROS_INFO("GOING DOWN!");
136 f878b5f9 Priya
        // Turn down.
137
        turn_from_to(dir, DOWN);
138 754da79f Priya
        follow_line();
139 f878b5f9 Priya
        // Solve recursively.
140
        bool solved = solve(row+1, col, UP);
141
        if (solved)
142
        {
143
            return solved;
144
        }
145
        else
146
        {
147
            //Update where we are.
148
            dir = DOWN;
149
        }
150
    }
151
    /* try left */
152
    if (map[row][col-1] != WALL && initial_dir != LEFT)
153
    {
154 c840fbe6 Priya
    ROS_INFO("GOING LEFT!");
155 f878b5f9 Priya
        // Turn down.
156
        turn_from_to(dir, LEFT);
157 754da79f Priya
        follow_line();
158 f878b5f9 Priya
        // Solve recursively.
159
        bool solved = solve(row, col-1, RIGHT);
160
        if (solved)
161
        {
162
            return solved;
163
        }
164
        else
165
        {
166
            //Update where we are.
167
            dir = LEFT;
168
        }
169
    }
170 6ee555a3 Priya
171
    ROS_INFO("DEAD END FOUND, TURNING BACK.");
172 f878b5f9 Priya
    // we have exhausted all the options. This path is clearly a
173
    // dead end. go back to where we come from and return false.
174
    turn_from_to(dir, initial_dir);
175 754da79f Priya
    follow_line();
176 f878b5f9 Priya
    return false;
177 64ee446f Yuyang
}
178
179
// this function takes in the current direction and turns the scout
180
// into it intended direction
181
void maze_solve::turn_from_to(int current_dir, int intended_dir) {
182
    switch ((4 + intended_dir - current_dir) % 4) 
183 754da79f Priya
    {
184 64ee446f Yuyang
        case 0:
185 f878b5f9 Priya
            spot_turn();
186 64ee446f Yuyang
            break;
187
        case 1:
188 c840fbe6 Priya
            turn_left();
189 64ee446f Yuyang
            break;
190
        case 2:
191 f878b5f9 Priya
            turn_straight();
192 64ee446f Yuyang
            break;
193
        case 3:
194 c840fbe6 Priya
            turn_right();
195 64ee446f Yuyang
            break;
196 754da79f Priya
    }
197 64ee446f Yuyang
}
198
199 2b0c2534 Priya
bool maze_solve::look_around(int row, int col, int dir)
200 64ee446f Yuyang
{
201
    // look around current place using sonar
202
    // store whether or not
203
    // there is a wall into the map
204
    // stores at row col 2 if point is critical, 1 otherwise
205
    
206 754da79f Priya
    int* readings = sonar->get_sonar_readings();
207 2b0c2534 Priya
    spinOnce();
208 f878b5f9 Priya
209 2b0c2534 Priya
/*
210
    // Look to the left.
211
    int left_distance = (readings[1] + readings[0] + readings[47])/3;
212 f878b5f9 Priya
    // Look to the front.
213 2b0c2534 Priya
    int front_distance = (readings[35] + readings[36] + readings[37])/3;
214
    // Look to the right.
215
    int right_distance = (readings[23] + readings[24] + readings[25])/3;
216
*/
217 f878b5f9 Priya
    // Look to the left.
218 2b0c2534 Priya
    int left_distance = readings[0];
219
    // Look to the front.
220
    int front_distance = readings[36];
221
    // Look to the right.
222
    int right_distance = readings[24];
223
224
    ROS_INFO("front: %d  left: %d  right: %d", front_distance, left_distance, right_distance);
225
226
    if (right_distance == 0 || front_distance == 0 || left_distance == 0)
227
      return false;
228
229 f878b5f9 Priya
    switch (dir)
230
    {
231
        case UP:
232
            // If the distance is less than 500, mark the area as a wall otherwise
233
            // mark it as seen.
234
            map[row][col+1] = (left_distance < 500)?WALL:SEEN;
235
            map[row+1][col] = (front_distance < 500)?WALL:SEEN;
236
            map[row][col-1] = (right_distance < 500)?WALL:SEEN;
237
            break;
238
        case RIGHT:
239
            // If the distance is less than 500, mark the area as a wall otherwise
240
            // mark it as seen.
241
            map[row+1][col] = (left_distance < 500)?WALL:SEEN;
242
            map[row][col-1] = (front_distance < 500)?WALL:SEEN;
243
            map[row-1][col] = (right_distance < 500)?WALL:SEEN;
244
            break;
245
        case DOWN:
246
            // If the distance is less than 500, mark the area as a wall otherwise
247
            // mark it as seen.
248
            map[row][col-1] = (left_distance < 500)?WALL:SEEN;
249
            map[row-1][col] = (front_distance < 500)?WALL:SEEN;
250
            map[row][col+1] = (right_distance < 500)?WALL:SEEN;
251
            break;
252
        case LEFT:
253
            // If the distance is less than 500, mark the area as a wall otherwise
254
            // mark it as seen.
255
            map[row-1][col] = (left_distance < 500)?WALL:SEEN;
256
            map[row][col+1] = (front_distance < 500)?WALL:SEEN;
257
            map[row+1][col] = (right_distance < 500)?WALL:SEEN;
258
            break;
259
    }
260 2b0c2534 Priya
261
    return true;
262 64ee446f Yuyang
}
263
264 f878b5f9 Priya
bool maze_solve::at_destination() 
265 64ee446f Yuyang
{
266 f878b5f9 Priya
    vector<uint32_t> readings = linesensor->query();
267
    if ( readings[0] > 200 &&
268
         readings[1] < 55 &&
269
         readings[2] < 55 &&
270
         readings[3] > 200 &&
271
         readings[4] > 200 &&
272
         readings[5] < 55 &&
273
         readings[6] < 55 &&
274
         readings[7] > 200 )
275
    {
276
        return true;
277
    }
278
    return false;
279 64ee446f Yuyang
}