Statistics
| Branch: | Revision:

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

History | View | Annotate | Download (3.84 KB)

1 64ee446f Yuyang
/**
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 4c9fb6ba viki
#define D_THRESH 600
29
#define max(x,y) ((x > y) ? x : y)
30 64ee446f Yuyang
31 4c9fb6ba viki
using namespace std;
32 754da79f Priya
33 4c9fb6ba viki
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 c840fbe6 Priya
44 a69f6363 viki
void maze_solve::run()
45
{    
46 2b0c2534 Priya
    ROS_INFO("Starting to solve the maze");
47 4c9fb6ba viki
48 2b0c2534 Priya
    // Go up to the first line.
49
    follow_line();
50 4c9fb6ba viki
51 2b0c2534 Priya
    // Turn the sonar on.
52
    sonar->set_on();
53
    sonar->set_range(0, 23);
54
55 4c9fb6ba viki
    ROS_INFO("Off we go!");
56 64ee446f Yuyang
57 4c9fb6ba viki
    while (!at_destination())
58 2b0c2534 Priya
    {
59 4c9fb6ba viki
        // 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 64ee446f Yuyang
        {
68 4c9fb6ba viki
            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 64ee446f Yuyang
        }
83 4c9fb6ba viki
        cout << endl;
84
85
        if (!readings_ok)
86 f878b5f9 Priya
        {
87 4c9fb6ba viki
            continue;
88 64ee446f Yuyang
        }
89 4c9fb6ba viki
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 f878b5f9 Priya
        {
98 4c9fb6ba viki
            ROS_INFO("Right.");
99
            turn_right();
100 f878b5f9 Priya
        }
101 4c9fb6ba viki
        else if (s_read > D_THRESH) // Straight
102 f878b5f9 Priya
        {
103 4c9fb6ba viki
            ROS_INFO("Straight.");
104
            turn_straight();
105 f878b5f9 Priya
        }
106 4c9fb6ba viki
        else if (l_read > D_THRESH)   // Left
107 f878b5f9 Priya
        {
108 4c9fb6ba viki
            ROS_INFO("Left.");
109
            turn_left();
110 f878b5f9 Priya
        }
111 4c9fb6ba viki
        else                                                        // Deadend
112 f878b5f9 Priya
        {
113 4c9fb6ba viki
            ROS_INFO("Dead end.");
114 f878b5f9 Priya
            spot_turn();
115 4c9fb6ba viki
        }
116 2b0c2534 Priya
117 4c9fb6ba viki
        follow_line();
118 f878b5f9 Priya
    }
119 64ee446f Yuyang
}
120
121 f878b5f9 Priya
bool maze_solve::at_destination() 
122 64ee446f Yuyang
{
123 f878b5f9 Priya
    vector<uint32_t> readings = linesensor->query();
124 4c9fb6ba viki
    //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 f878b5f9 Priya
    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 4c9fb6ba viki
        ROS_INFO("\n\nDESTINATION\n\n");
135 f878b5f9 Priya
        return true;
136
    }
137
    return false;
138 64ee446f Yuyang
}