Revision 64a67b32 scout/libscout/src/test_behaviors/ApproachEmitter.cpp

View differences:

scout/libscout/src/test_behaviors/ApproachEmitter.cpp
26 26
#include "ApproachEmitter.h"
27 27
#define TOP_BOM 0
28 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

  
29 41

  
30 42
using namespace std;
31 43

  
......
50 62

  
51 63
int ApproachEmitter::identifyPosition(
52 64
        vector<uint32_t> readings, vector<uint32_t> senders) {
53
    bool identified = 0;
65
    bool identified = SEARCH;
54 66
    int top_em_count = 0;
55 67
    int bot_em_count = 0;
56 68
    if (accumulate(&(readings[RIGHT.front()]),
......
60 72
            bot_em_count += (senders[i] == BOT_BOM);
61 73
        }
62 74
        if (top_em_count * bot_em_count != 0) {
63
            //ROS_INFO("HAND OVER TO HUIJUN!");
75
           return CENTRED;
64 76
        } else if (top_em_count > 0) {
65 77
            motors->set_sides(-50, -50, MOTOR_PERCENT);
66 78
        } else {
67 79
            motors->set_sides(50, 50, MOTOR_PERCENT);
68 80
        }
69
        identified = 1;
81
        identified = ADJUSTED;
70 82
    } else if (accumulate(&(readings[LEFT.front()]),
71 83
                   &(readings[LEFT.back()])+1, 0) >= 2) {
72 84

  
......
80 92
        } else {
81 93
            motors->set_sides(-50, -50, MOTOR_PERCENT);
82 94
        }
83
        identified = 1;
95
        identified = ADJUSTED;
84 96
    }
85 97
    return identified;
86 98
}
......
90 102
    BomReadings bomR;
91 103
    vector<uint32_t> r;
92 104
    vector<uint32_t> s;
105
    int identified = 0;
93 106
    while (ok()) {
94 107
        bomR = bom->query();
95 108
        r = bomR.readings;
96 109
        s = bomR.senders;
97
        if (identifyPosition(r, s)) {
98
            // do nothing
99
        } else {
100
            motors->set_sides(0, 50, MOTOR_PERCENT);
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);
101 116
        }
102 117
    }
103
    while (ok()) {}
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;
104 232
}

Also available in: Unified diff