Revision 6ad8ef06
Improved reverse parking adjustment in ApproachEmitter. Made end line in chargetest easier to detect
scout/libscout/src/test_behaviors/ApproachEmitter.cpp | ||
---|---|---|
32 | 32 |
|
33 | 33 |
#define LEFTSIDE 1 |
34 | 34 |
#define RIGHTSIDE 2 |
35 |
#define REAR 3 |
|
35 | 36 |
#define NEITHER 0 |
36 | 37 |
|
37 | 38 |
#define CENTRED 2 |
... | ... | |
62 | 63 |
|
63 | 64 |
int ApproachEmitter::identifyPosition( |
64 | 65 |
vector<uint32_t> readings, vector<uint32_t> senders) { |
65 |
bool identified = SEARCH;
|
|
66 |
int identified = SEARCH;
|
|
66 | 67 |
int top_em_count = 0; |
67 | 68 |
int bot_em_count = 0; |
68 | 69 |
if (accumulate(&(readings[RIGHT.front()]), |
... | ... | |
110 | 111 |
s = bomR.senders; |
111 | 112 |
identified = identifyPosition(r, s); |
112 | 113 |
if (identified == CENTRED) { |
113 |
ROS_INFO("break"); |
|
114 |
//ROS_INFO("break");
|
|
114 | 115 |
break; |
115 | 116 |
} else if (identified == SEARCH) { |
116 | 117 |
motors->set_sides(10, 40, MOTOR_PERCENT); |
... | ... | |
163 | 164 |
checkBOM(5, RIGHT_EMITTER_ID)) { |
164 | 165 |
|
165 | 166 |
motors->set_sides(-20, -20, MOTOR_PERCENT); |
166 |
if (at_destination()) { |
|
167 |
return false; |
|
167 |
} |
|
168 |
else if (get_side() == REAR) { |
|
169 |
if (checkBOM(4,LEFT_EMITTER_ID) && |
|
170 |
!checkBOM(4, RIGHT_EMITTER_ID)) { //reverse right |
|
171 |
//ROS_INFO("correct left"); |
|
172 |
motors->set_sides(-20, -10, MOTOR_PERCENT); |
|
173 |
} |
|
174 |
else if (!checkBOM(4, LEFT_EMITTER_ID) && |
|
175 |
checkBOM(5, RIGHT_EMITTER_ID)) { //reverse left |
|
176 |
//ROS_INFO("correct right"); |
|
177 |
motors->set_sides(-10, -20, MOTOR_PERCENT); |
|
178 |
} |
|
179 |
else { |
|
180 |
motors->set_sides(-20, -20, MOTOR_PERCENT); |
|
168 | 181 |
} |
169 | 182 |
} |
170 | 183 |
else { |
... | ... | |
176 | 189 |
|
177 | 190 |
} |
178 | 191 |
} |
192 |
|
|
193 |
if (at_destination()) { |
|
194 |
return false; |
|
195 |
} |
|
179 | 196 |
return true; |
180 | 197 |
} |
181 | 198 |
|
... | ... | |
189 | 206 |
(readings[1] == 0 && readings[2] == 0 && readings[3] == 0)) { |
190 | 207 |
return RIGHTSIDE; |
191 | 208 |
} |
209 |
else if ((readings[4] > 0) || (readings[5] > 0)) { |
|
210 |
return REAR; |
|
211 |
} |
|
192 | 212 |
else { |
193 | 213 |
return NEITHER; |
194 | 214 |
} |
Also available in: Unified diff