Revision 2b0c2534 scout/libscout/src/behaviors/maze_solve.cpp

View differences:

scout/libscout/src/behaviors/maze_solve.cpp
46 46
void maze_solve::run(){
47 47
    
48 48
    // TODO:first initialize map to all 0's
49
    solve(25,25, RIGHT);
49
    ROS_INFO("Starting to solve the maze");
50
    // Go up to the first line.
51
    follow_line();
52
    // Turn the sonar on.
53
    sonar->set_on();
54
    sonar->set_range(0, 23);
55

  
56
    // Wait for the sonar to initialize.
57
    while(!look_around(25, 25, RIGHT) && ok())
58
    {
59
      spinOnce();      
60
    }
61

  
62
    // Solve the maze
63
    bool finished = solve(25,25, RIGHT);
64

  
65
    // Check and report final condition.
66
    if (finished)
67
        ROS_INFO("YAY! I have solved the maze");
68
    else
69
        ROS_INFO("NO! The maze is unsolvable");
50 70
}
51 71

  
52 72
bool maze_solve::solve(int row, int col, int dir)
......
59 79

  
60 80
    // this function should fill the adjacent cells around me with
61 81
    // wall's or paths
62
    look_around(row, col, dir);
82
    while(!look_around(row, col, dir) && ok())
83
    {
84
        spinOnce();
85
    }
63 86

  
64 87
    /* try go up */
88
    ROS_INFO("GOING UP!");
65 89
    if (map[row-1][col] != WALL && initial_dir != UP)
66 90
    {
67 91
        // Turn up.
......
80 104
        }
81 105
    }
82 106
    /* try right */
107
    ROS_INFO("GOING RIGHT!");
83 108
    if (map[row][col+1] != WALL && initial_dir != RIGHT)
84 109
    {
85 110
        // Turn right.
......
98 123
        }
99 124
    }
100 125
    /* try down */
126
    ROS_INFO("GOING DOWN!");
101 127
    if (map[row+1][col] != WALL && initial_dir != DOWN)
102 128
    {
103 129
        // Turn down.
......
116 142
        }
117 143
    }
118 144
    /* try left */
145
    ROS_INFO("GOING LEFT!");
119 146
    if (map[row][col-1] != WALL && initial_dir != LEFT)
120 147
    {
121 148
        // Turn down.
......
160 187
    }
161 188
}
162 189

  
163
void maze_solve::look_around(int row, int col, int dir)
190
bool maze_solve::look_around(int row, int col, int dir)
164 191
{
165 192
    // look around current place using sonar
166 193
    // store whether or not
......
168 195
    // stores at row col 2 if point is critical, 1 otherwise
169 196
    
170 197
    int* readings = sonar->get_sonar_readings();
198
    spinOnce();
171 199

  
172
    // Look to the right.
173
    int right_distance = (readings[1] + readings[0] + readings[47])/3;
200
/*
201
    // Look to the left.
202
    int left_distance = (readings[1] + readings[0] + readings[47])/3;
174 203
    // Look to the front.
175
    int front_distance = (readings[11] + readings[12] + readings[13])/3;
204
    int front_distance = (readings[35] + readings[36] + readings[37])/3;
205
    // Look to the right.
206
    int right_distance = (readings[23] + readings[24] + readings[25])/3;
207
*/
176 208
    // Look to the left.
177
    int left_distance = (readings[23] + readings[24] + readings[25])/3;
209
    int left_distance = readings[0];
210
    // Look to the front.
211
    int front_distance = readings[36];
212
    // Look to the right.
213
    int right_distance = readings[24];
214

  
215
    ROS_INFO("front: %d  left: %d  right: %d", front_distance, left_distance, right_distance);
216

  
217
    if (right_distance == 0 || front_distance == 0 || left_distance == 0)
218
      return false;
219

  
178 220
    switch (dir)
179 221
    {
180 222
        case UP:
......
206 248
            map[row+1][col] = (right_distance < 500)?WALL:SEEN;
207 249
            break;
208 250
    }
251

  
252
    return true;
209 253
}
210 254

  
211 255
bool maze_solve::at_destination() 

Also available in: Unified diff