Revision b71f5bca
Working on ApproachEmitter
scout/libscout/src/test_behaviors/ApproachEmitter.cpp | ||
---|---|---|
39 | 39 |
} |
40 | 40 |
|
41 | 41 |
void ApproachEmitter::run() { |
42 |
ROS_INFO("BOM Main Running"); |
|
42 | 43 |
BomReadings bomR; |
43 | 44 |
int r_weighted, l_weighted; |
44 | 45 |
int speed = 20; |
... | ... | |
48 | 49 |
bomR = bom->query(); |
49 | 50 |
r = bomR.readings; |
50 | 51 |
s = bomR.senders; |
52 |
ROS_INFO("BOM Main Running"); |
|
51 | 53 |
if (accumulate(r.begin(), r.end(), 0)) { |
52 |
l_weighted = r[0] *BOM_W1 + r[1]*BOM_W2 + r[2]*BOM_W3 + r[3]*BOM_W4 + r[4]*BOM_W5; |
|
53 |
r_weighted = r[9] *BOM_W1 + r[8]*BOM_W2 + r[7]*BOM_W3 + r[6]*BOM_W4 - r[5]*BOM_W5; |
|
54 |
motors->set_sides(min(r_weighted+speed, 100), |
|
55 |
min(l_weighted+speed, 102), MOTOR_PERCENT); |
|
54 |
for (int i=0; i<10; ++i) { |
|
55 |
ROS_INFO("BOM %d reading is %d sender is %d", i, r[i], s[i]); |
|
56 |
} |
|
57 |
|
|
58 |
//motors->set_sides(min(r_weighted+speed, 100), |
|
59 |
// min(l_weighted+speed, 102), MOTOR_PERCENT); |
|
56 | 60 |
} |
57 | 61 |
} |
58 | 62 |
} |
scout/libscout/src/test_behaviors/ApproachEmitter_launch_instruction.txt | ||
---|---|---|
1 |
rosrun scoutsim scoutsim_node chargetest |
|
2 |
rosservice call /spawn_em 3.5 1.07 2.618 "em_0" |
|
3 |
rosservice call /spawn_em 3.5 1.14 3.667 "em_1" |
Also available in: Unified diff