Revision f878b5f9

View differences:

scout/libscout/src/behaviors/maze_solve.cpp
48 48
    // TODO:first initialize map to all 0's
49 49
    solve(25,25, RIGHT);
50 50
}
51
bool maze_solve::solve(row, col, dir)
52
{   
51

  
52
bool maze_solve::solve(int row, int col, int dir)
53
{
54
    int intial_dir = dir;
55

  
53 56
    // use backtracking to solve the maze
54
    if (at_destination()) {
55
        // not really sure what to do next... just chill here
57
    if (at_destination())
56 58
        return true;
57
    }
58 59

  
59
    else
60
    // this function should fill the adjacent cells around me with
61
    // wall's or paths
62
    look_around(row, col, dir);
63

  
64
    /* try go up */
65
    if (map[row-1][col] != WALL && initial_dir != UP)
60 66
    {
61
        // this function should fill the adjacent cells around me with
62
        // wall's or paths
63
        look_around(row, col, dir);
64
        
65
        if (at_dead_end(row, col, dir))
67
        // Turn up.
68
        turn_from_to(dir, UP);
69
        line_follow();
70
        // Solve recursively.
71
        bool solved = solve(row-1, col, DOWN);
72
        if (solved)
66 73
        {
67
            // set the scout to a back tracking direction
68
            spot_turn();
69
            return false;
74
            return solved;
70 75
        }
71
        
72
        /* try go up */
73
        // no need to check range because we assume our map is big enough
74
        if (map[row-1][col] != WALL)
76
        else
75 77
        {
76
            turn_from_to(dir, UP);
77
            line_follow();
78
            // halts at intersection
79
            bool solved = solve(row-1, col)
80
            if (solved) {
81
                //yeah!!! don't really know what to do now...
82
                // hopefully send some message to some other scout
83
                // waiting for my valuable information
84
            }
85
            else {
86
                // whoops, that path did not work out
87
                // redo the move, since the recursive call has 
88
                // already put me into the right direction, I will just 
89
                // drive back and halt at intersection
90
                line_follow();
91
                // undo the turning
92
                turn_from_to()
93
            }            
78
            //Update where we are.
79
            dir = UP;
94 80
        }
95
        
96
        /* try right */
97
        if (map[row][col+1] != WALL)
81
    }
82
    /* try right */
83
    if (map[row][col+1] != WALL && initial_dir != RIGHT)
84
    {
85
        // Turn right.
86
        turn_from_to(dir, RIGHT);
87
        line_follow();
88
        // Solve recursively.
89
        bool solved = solve(row, col+1, LEFT);
90
        if (solved)
98 91
        {
99
            turn_from
92
            return solved;
93
        }
94
        else
95
        {
96
            //Update where we are.
97
            dir = RIGHT;
100 98
        }
101

  
102 99
    }
103

  
100
    /* try down */
101
    if (map[row+1][col] != WALL && initial_dir != DOWN)
102
    {
103
        // Turn down.
104
        turn_from_to(dir, DOWN);
105
        line_follow();
106
        // Solve recursively.
107
        bool solved = solve(row+1, col, UP);
108
        if (solved)
109
        {
110
            return solved;
111
        }
112
        else
113
        {
114
            //Update where we are.
115
            dir = DOWN;
116
        }
117
    }
118
    /* try left */
119
    if (map[row][col-1] != WALL && initial_dir != LEFT)
120
    {
121
        // Turn down.
122
        turn_from_to(dir, LEFT);
123
        line_follow();
124
        // Solve recursively.
125
        bool solved = solve(row, col-1, RIGHT);
126
        if (solved)
127
        {
128
            return solved;
129
        }
130
        else
131
        {
132
            //Update where we are.
133
            dir = LEFT;
134
        }
135
    }
136
    // we have exhausted all the options. This path is clearly a
137
    // dead end. go back to where we come from and return false.
138
    turn_from_to(dir, initial_dir);
139
    line_follow();
140
    return false;
104 141
}
105 142

  
106 143
// this function takes in the current direction and turns the scout
......
108 145
void maze_solve::turn_from_to(int current_dir, int intended_dir) {
109 146
    switch ((4 + intended_dir - current_dir) % 4) 
110 147
        case 0:
111
            turn_straight();
148
            spot_turn();
112 149
            break;
113 150
        case 1:
114 151
            turn_right();
115 152
            break;
116 153
        case 2:
154
            turn_straight();
117 155
            break;
118 156
        case 3:
119 157
            turn_left();
120 158
            break;
121 159
}
122 160

  
123
int maze_solve::look_around(row, col)
161
void maze_solve::look_around(int row, int col, int dir)
124 162
{
125 163
    // look around current place using sonar
126 164
    // store whether or not
127 165
    // there is a wall into the map
128 166
    // stores at row col 2 if point is critical, 1 otherwise
129
}
130

  
131
bool maze_solve::at_destination() 
132
{
133 167
    
168
    int* readings = get_sonar_readings();
169

  
170
    // Look to the right.
171
    int right_distance = (readings[1] + readings[0] + readings[47])/3;
172
    // Look to the front.
173
    int front_distance = (readings[11] + readings[12] + readings[13])/3;
174
    // Look to the left.
175
    int left_distance = (readings[23] + readings[24] + readings[25])/3;
176
    switch (dir)
177
    {
178
        case UP:
179
            // If the distance is less than 500, mark the area as a wall otherwise
180
            // mark it as seen.
181
            map[row][col+1] = (left_distance < 500)?WALL:SEEN;
182
            map[row+1][col] = (front_distance < 500)?WALL:SEEN;
183
            map[row][col-1] = (right_distance < 500)?WALL:SEEN;
184
            break;
185
        case RIGHT:
186
            // If the distance is less than 500, mark the area as a wall otherwise
187
            // mark it as seen.
188
            map[row+1][col] = (left_distance < 500)?WALL:SEEN;
189
            map[row][col-1] = (front_distance < 500)?WALL:SEEN;
190
            map[row-1][col] = (right_distance < 500)?WALL:SEEN;
191
            break;
192
        case DOWN:
193
            // If the distance is less than 500, mark the area as a wall otherwise
194
            // mark it as seen.
195
            map[row][col-1] = (left_distance < 500)?WALL:SEEN;
196
            map[row-1][col] = (front_distance < 500)?WALL:SEEN;
197
            map[row][col+1] = (right_distance < 500)?WALL:SEEN;
198
            break;
199
        case LEFT:
200
            // If the distance is less than 500, mark the area as a wall otherwise
201
            // mark it as seen.
202
            map[row-1][col] = (left_distance < 500)?WALL:SEEN;
203
            map[row][col+1] = (front_distance < 500)?WALL:SEEN;
204
            map[row+1][col] = (right_distance < 500)?WALL:SEEN;
205
            break;
206
    }
134 207
}
135 208

  
136
bool wall_in_direction()
209
bool maze_solve::at_destination() 
137 210
{
138
    
211
    vector<uint32_t> readings = linesensor->query();
212
    if ( readings[0] > 200 &&
213
         readings[1] < 55 &&
214
         readings[2] < 55 &&
215
         readings[3] > 200 &&
216
         readings[4] > 200 &&
217
         readings[5] < 55 &&
218
         readings[6] < 55 &&
219
         readings[7] > 200 )
220
    {
221
        return true;
222
    }
223
    return false;
139 224
}
scout/libscout/src/behaviors/maze_solve.h
34 34
        maze_solve(std::string scoutname, Sensors* sensors):
35 35
            line_follow(scoutname, "maze_solve", sensors) {};
36 36
        void run();
37
    private:
38
        bool solve(int row, int col, int dir);
39
        void turn_from_to(int current_dir, int intended_dir);
40
        void look_around(int row, int col, int dir);
41
        bool at_destination();
37 42
}
38 43
#endif
39 44

  

Also available in: Unified diff