Revision ac55b507
basic drive towards BOM working
scout/libscout/src/test_behaviors/ApproachEmitter.cpp | ||
---|---|---|
34 | 34 |
} |
35 | 35 |
|
36 | 36 |
void ApproachEmitter::run() { |
37 |
vector<uint32_t> r; |
|
38 |
int r_weighted, l_weighted; |
|
39 |
int speed = 20; |
|
37 | 40 |
while (ok()) { |
38 |
vector<uint32_t> readings; |
|
39 |
readings = bom->query(); |
|
40 |
ROS_INFO("front BOM readings %d", readings[0]); |
|
41 |
r = bom->query(); |
|
42 |
if (accumulate(r.begin(), r.end(), 0)) { |
|
43 |
l_weighted = r[0] *5 + r[1]*10 + r[2]*15 + r[3]*20 + r[4]*30; |
|
44 |
r_weighted = r[9] *5 + r[8]*10 + r[7]*15 + r[6]*20 - r[5]*30; |
|
45 |
motors->set_sides(min(r_weighted+speed, 100), |
|
46 |
min(l_weighted+speed, 100), MOTOR_PERCENT); |
|
47 |
} |
|
41 | 48 |
} |
42 | 49 |
} |
Also available in: Unified diff