Revision 7b5ea072 scout/libscout/src/test_behaviors/maze_solve.cpp

View differences:

scout/libscout/src/test_behaviors/maze_solve.cpp
76 76
        ROS_INFO("NO! The maze is unsolvable");
77 77
}
78 78

  
79
void maze_solve::adjust()
80
{
81

  
82
    ROS_INFO("adjusting...!");   
83
    motors->set_sides(-30, -30, MOTOR_ABSOLUTE);
84
    ros::Duration(0.3).sleep();
85
    halt();    
86
/*    int front_d = 0;
87
    vector<uint32_t> readings;
88
    int* sonar_readings = sonar->get_sonar_readings();
89
    spinOnce(); 
90
    int ideal_d = sonar_readings[36]/250*250;
91
    bool adjusted = false;   
92
    while (ok() && !adjusted)
93
    {
94
        sonar_readings = sonar->get_sonar_readings();
95
        spinOnce();
96
        front_d = sonar_readings[36];
97
        ROS_INFO("adjusting from %d to %d", front_d, ideal_d);
98
        if (front_d > ideal_d)
99
        {
100
            motors->set_sides(-10, -10, MOTOR_ABSOLUTE);
101
            readings = linesensor->query();
102
        }
103
        else
104
        {
105
            adjusted = true;
106
            halt();
107
        }
108
    }    */
109
}
110 79

  
111 80
bool maze_solve::solve(int row, int col, int dir, bool root)
112 81
{
......
137 106
        // Turn up.
138 107
        turn_from_to(dir, UP);
139 108
        follow_line();
140
        //adjust();
141 109
        // Solve recursively.
142 110
        bool solved = solve(row-2, col, DOWN, false);
143 111
        if (solved)
......
157 125
        // Turn down.
158 126
        turn_from_to(dir, DOWN);
159 127
        follow_line();
160
        //adjust();
161 128
        // Solve recursively.
162 129
        bool solved = solve(row+2, col, UP, false);
163 130
        if (solved)
......
177 144
        // Turn right.
178 145
        turn_from_to(dir, RIGHT);
179 146
        follow_line();
180
        //adjust();
181 147
        // Solve recursively.
182 148
        bool solved = solve(row, col+2, LEFT, false);
183 149
        if (solved)
......
197 163
        // Turn down.
198 164
        turn_from_to(dir, LEFT);
199 165
        follow_line();
200
        //adjust();
201 166
        // Solve recursively.
202 167
        bool solved = solve(row, col-2, RIGHT, false);
203 168
        if (solved)
......
372 337
{
373 338
    vector<uint32_t> readings = linesensor->query();
374 339

  
375

  
340
    //changed values so it detects solution
376 341
    if ( readings[0] > 75 &&
377 342
         readings[1] > 75 &&
378 343
         readings[2] < 55 &&

Also available in: Unified diff