Statistics
| Branch: | Revision:

root / scout / libscout / src / test_behaviors / ApproachEmitter.cpp @ 64a67b32

History | View | Annotate | Download (6.18 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 "ApproachEmitter.h"
27
#define TOP_BOM 0
28
#define BOT_BOM 1
29
#define LEFT_EMITTER_ID 1
30
#define RIGHT_EMITTER_ID 0
31

    
32

    
33
#define LEFTSIDE 1
34
#define RIGHTSIDE 2
35
#define NEITHER 0
36

    
37
#define CENTRED 2
38
#define SEARCH 0
39
#define ADJUSTED 1
40

    
41

    
42
using namespace std;
43

    
44
ApproachEmitter::ApproachEmitter(string scoutname, Sensors* sensors)
45
    : Behavior(scoutname, "Approach Emitter", sensors) {
46
    name = scoutname;
47
    scout_pos = new pos;
48

    
49
    // initallize the side index values :D UGLY!!! But best solution so far
50
    FRONT.push_back(0);
51
    LEFT.push_back(1);
52
    LEFT.push_back(2);
53
    LEFT.push_back(3);
54
    BACK.push_back(4);
55
    BACK.push_back(5);
56
    RIGHT.push_back(6);
57
    RIGHT.push_back(7);
58
    RIGHT.push_back(8);
59
    FRONT.push_back(9);
60

    
61
}
62

    
63
int ApproachEmitter::identifyPosition(
64
        vector<uint32_t> readings, vector<uint32_t> senders) {
65
    bool identified = SEARCH;
66
    int top_em_count = 0;
67
    int bot_em_count = 0;
68
    if (accumulate(&(readings[RIGHT.front()]),
69
                   &(readings[RIGHT.back()])+1, 0) >= 2) {
70
        for (int i=RIGHT.front(); i<RIGHT.back(); i++) {
71
            top_em_count += (senders[i] == TOP_BOM);
72
            bot_em_count += (senders[i] == BOT_BOM);
73
        }
74
        if (top_em_count * bot_em_count != 0) {
75
           return CENTRED;
76
        } else if (top_em_count > 0) {
77
            motors->set_sides(-50, -50, MOTOR_PERCENT);
78
        } else {
79
            motors->set_sides(50, 50, MOTOR_PERCENT);
80
        }
81
        identified = ADJUSTED;
82
    } else if (accumulate(&(readings[LEFT.front()]),
83
                   &(readings[LEFT.back()])+1, 0) >= 2) {
84

    
85
        for (int i=LEFT.front(); i<LEFT.back(); i++) {
86
            top_em_count += (senders[i] == TOP_BOM);
87
            bot_em_count += (senders[i] == BOT_BOM);
88
        }
89
        if (top_em_count * bot_em_count != 0) {
90
        } else if (top_em_count > 0) {
91
            motors->set_sides(50, 50, MOTOR_PERCENT);
92
        } else {
93
            motors->set_sides(-50, -50, MOTOR_PERCENT);
94
        }
95
        identified = ADJUSTED;
96
    }
97
    return identified;
98
}
99

    
100
void ApproachEmitter::run() {
101
    ROS_INFO("BOM Main Running");
102
    BomReadings bomR;
103
    vector<uint32_t> r;
104
    vector<uint32_t> s;
105
    int identified = 0;
106
    while (ok()) {
107
        bomR = bom->query();
108
        r = bomR.readings;
109
        s = bomR.senders;
110
        identified = identifyPosition(r, s);
111
        if (identified == CENTRED) {
112
            ROS_INFO("break");
113
            break;
114
        } else if (identified == SEARCH) {
115
            motors->set_sides(10, 40, MOTOR_PERCENT);
116
        }
117
    }
118

    
119
    update_readings();
120
    start_side = get_side();
121

    
122
    while (ok() && ((start_side == LEFTSIDE && checkBOM(3, RIGHT_EMITTER_ID)) ||
123
           (start_side == RIGHTSIDE && checkBOM(6, LEFT_EMITTER_ID)))) {
124
       motors->set_sides(25, 25, MOTOR_PERCENT);
125
       update_readings();
126

    
127
     }
128

    
129
    motors->set_sides(0, 0, MOTOR_PERCENT);
130

    
131

    
132
    while(ok() && reverse_park()) {
133
        update_readings();
134
    }
135

    
136

    
137
    //motors->set_sides(0,0,MOTOR_PERCENT);
138
    ROS_INFO("done");
139
/*
140
    while(ok()) {
141
        update_readings();
142
        if (at_destination()) {
143
              motors->set_sides(0, 0, MOTOR_PERCENT);
144
          ROS_INFO("done");
145
          break;
146
        }
147
        else {
148
          motors->set_sides(-20,-20,MOTOR_PERCENT);
149

150
        }
151
    }
152
*/
153
}
154

    
155
bool ApproachEmitter::reverse_park() {
156
    if(checkBOM(5, LEFT_EMITTER_ID) ||
157
       checkBOM(4, RIGHT_EMITTER_ID)) { //move forward
158

    
159
        motors->set_sides(10, 10, MOTOR_PERCENT);
160
    }
161
    else if(checkBOM(4, LEFT_EMITTER_ID) &&
162
            checkBOM(5, RIGHT_EMITTER_ID)) {
163

    
164
        motors->set_sides(-20, -20, MOTOR_PERCENT);
165
            if (at_destination()) {
166
          return false;
167
        }
168
    }
169
    else {
170
        if (start_side == RIGHTSIDE) { //reverseright
171
            motors->set_sides(-40, -10, MOTOR_PERCENT);
172
        }
173
        else { //reverseleft
174
            motors->set_sides(-10, -40, MOTOR_PERCENT);
175

    
176
        }
177
    }
178
    return true;
179
}
180

    
181
int ApproachEmitter::get_side() {
182

    
183
    if ((readings[1] > 0 || readings[2] > 0|| readings[3] > 0) &&
184
        (readings[6] == 0 && readings[7] == 0 && readings[8] == 0)) {
185
        return LEFTSIDE;
186
    }
187
    else if ((readings[6] > 0 || readings[7] > 0|| readings[8] > 0) &&
188
        (readings[1] == 0 && readings[2] == 0 && readings[3] == 0)) {
189
        return RIGHTSIDE;
190
    }
191
    else {
192
        return NEITHER;
193
    }
194
}
195

    
196
bool ApproachEmitter::checkBOM(int bNum, unsigned int ID) {
197
    if(readings[bNum] == true) {
198
        //ROS_INFO("Emitter ID: %d", senders[bNum]);
199
        return senders[bNum] == ID;
200
    }
201
    return false;
202

    
203
}
204

    
205
void ApproachEmitter::update_readings() {
206
    BomReadings bomR = bom->query();
207
    readings = bomR.readings;
208
    senders = bomR.senders;
209

    
210
}
211

    
212
bool ApproachEmitter::at_destination()
213
{
214
    vector<uint32_t> readings = linesensor->query();
215

    
216
/*   ROS_INFO("%d %d %d %d %d %d %d %d", readings[0], readings[1],
217
        readings[2],readings[3],readings[4],readings[5],readings[6],readings[7]);
218
*/
219
    //changed values so it detects solution
220
    if ( readings[0] > 75 ||
221
         readings[1] > 75 ||
222
         readings[2] > 75 ||
223
         readings[3] > 75 ||
224
         readings[4] > 75 ||
225
         readings[5] > 75 ||
226
         readings[6] > 75 ||
227
         readings[7] > 75 )
228
    {
229
        return true;
230
    }
231
    return false;
232
}