Revision c840fbe6 scout/libscout/src/behaviors/maze_solve.cpp

View differences:

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