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