Statistics
| Branch: | Revision:

root / scout / libscout / src / test_behaviors / maze_solve.cpp @ 4c9fb6ba

History | View | Annotate | Download (3.84 KB)

1
/**
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
#define D_THRESH 600
29
#define max(x,y) ((x > y) ? x : y)
30

    
31
using namespace std;
32

    
33
void maze_solve::spin_for(double duration)
34
{
35
    ros::Rate r(100);     // 100 hz
36
    int ticks = int(duration * 100);
37
    for (int i = 0; i < ticks; i++)
38
    {
39
        spinOnce();
40
        r.sleep();
41
    }
42
}
43

    
44
void maze_solve::run()
45
{    
46
    ROS_INFO("Starting to solve the maze");
47

    
48
    // Go up to the first line.
49
    follow_line();
50

    
51
    // Turn the sonar on.
52
    sonar->set_on();
53
    sonar->set_range(0, 23);
54

    
55
    ROS_INFO("Off we go!");
56

    
57
    while (!at_destination())
58
    {
59
        // Wait for sonar to update.
60
        spin_for(1.5);
61
        int* readings = sonar->get_sonar_readings();
62
        
63
        // Wait until the sonar gives us real values.
64
        bool readings_ok = true;
65
        cout << "[";
66
        for (int i = 0; i < 48; i++)
67
        {
68
            if (i == 24 || i == 36 || i == 0)
69
            {
70
                cout << "(" << readings[i] << ") ";
71
            }
72
            else
73
            {
74
                cout << readings[i] << " ";
75
            }
76
            if (readings[i] == 0)
77
            {
78
                ROS_INFO("Waiting. readings[%d] == 0.", i);
79
                readings_ok = false;
80
                break;
81
            }
82
        }
83
        cout << endl;
84

    
85
        if (!readings_ok)
86
        {
87
            continue;
88
        }
89

    
90
        int r_read = max(readings[23], max(readings[24], readings[25]));
91
        int s_read = max(readings[35], max(readings[36], readings[37]));
92
        int l_read = max(readings[47], max(readings[0],  readings[1]));
93

    
94
        ROS_INFO("Left: %d. Straight: %d. Right: %d.", l_read, s_read, r_read);
95

    
96
        if (r_read > D_THRESH)      // Right
97
        {
98
            ROS_INFO("Right.");
99
            turn_right();
100
        }
101
        else if (s_read > D_THRESH) // Straight
102
        {
103
            ROS_INFO("Straight.");
104
            turn_straight();
105
        }
106
        else if (l_read > D_THRESH)   // Left
107
        {
108
            ROS_INFO("Left.");
109
            turn_left();
110
        }
111
        else                                                        // Deadend
112
        {
113
            ROS_INFO("Dead end.");
114
            spot_turn();
115
        }
116

    
117
        follow_line();
118
    }
119
}
120

    
121
bool maze_solve::at_destination() 
122
{
123
    vector<uint32_t> readings = linesensor->query();
124
    //ROS_INFO("Readings: %d %d %d %d %d %d %d %d.", readings[0], readings[1], readings[2], readings[3], readings[4], readings[5], readings[6], readings[7]);
125
    if ( readings[0] > 200 &&
126
         readings[1] < 55 &&
127
         readings[2] < 55 &&
128
         readings[3] > 200 &&
129
         readings[4] > 200 &&
130
         readings[5] < 55 &&
131
         readings[6] < 55 &&
132
         readings[7] > 200 )
133
    {
134
        ROS_INFO("\n\nDESTINATION\n\n");
135
        return true;
136
    }
137
    return false;
138
}