Revision fb61cb17
Add first stage of charging behavoir
scout/libscout/src/test_behaviors/ApproachEmitter.cpp | ||
---|---|---|
24 | 24 |
*/ |
25 | 25 |
|
26 | 26 |
#include "ApproachEmitter.h" |
27 |
#define BOM_W1 5 |
|
28 |
#define BOM_W2 10 |
|
29 |
#define BOM_W3 15 |
|
30 |
#define BOM_W4 20 |
|
31 |
#define BOM_W5 30 |
|
27 |
#define TOP_BOM 0 |
|
28 |
#define BOT_BOM 1 |
|
32 | 29 |
|
33 | 30 |
using namespace std; |
34 | 31 |
|
35 |
ApproachEmitter::ApproachEmitter(string scoutname, Sensors* sensors)
|
|
32 |
ApproachEmitter::ApproachEmitter(string scoutname, Sensors* sensors) |
|
36 | 33 |
: Behavior(scoutname, "Approach Emitter", sensors) { |
37 | 34 |
name = scoutname; |
38 | 35 |
scout_pos = new pos; |
36 |
|
|
37 |
// initallize the side index values :D UGLY!!! But best solution so far |
|
38 |
FRONT.push_back(0); |
|
39 |
LEFT.push_back(1); |
|
40 |
LEFT.push_back(2); |
|
41 |
LEFT.push_back(3); |
|
42 |
BACK.push_back(4); |
|
43 |
BACK.push_back(5); |
|
44 |
RIGHT.push_back(6); |
|
45 |
RIGHT.push_back(7); |
|
46 |
RIGHT.push_back(8); |
|
47 |
FRONT.push_back(9); |
|
48 |
|
|
49 |
} |
|
50 |
|
|
51 |
int ApproachEmitter::identifyPosition( |
|
52 |
vector<uint32_t> readings, vector<uint32_t> senders) { |
|
53 |
bool identified = 0; |
|
54 |
int top_em_count = 0; |
|
55 |
int bot_em_count = 0; |
|
56 |
if (accumulate(&(readings[RIGHT.front()]), |
|
57 |
&(readings[RIGHT.back()])+1, 0) >= 2) { |
|
58 |
for (int i=RIGHT.front(); i<RIGHT.back(); i++) { |
|
59 |
top_em_count += (senders[i] == TOP_BOM); |
|
60 |
bot_em_count += (senders[i] == BOT_BOM); |
|
61 |
} |
|
62 |
if (top_em_count * bot_em_count != 0) { |
|
63 |
//ROS_INFO("HAND OVER TO HUIJUN!"); |
|
64 |
} else if (top_em_count > 0) { |
|
65 |
motors->set_sides(-50, -50, MOTOR_PERCENT); |
|
66 |
} else { |
|
67 |
motors->set_sides(50, 50, MOTOR_PERCENT); |
|
68 |
} |
|
69 |
identified = 1; |
|
70 |
} else if (accumulate(&(readings[LEFT.front()]), |
|
71 |
&(readings[LEFT.back()])+1, 0) >= 2) { |
|
72 |
|
|
73 |
for (int i=LEFT.front(); i<LEFT.back(); i++) { |
|
74 |
top_em_count += (senders[i] == TOP_BOM); |
|
75 |
bot_em_count += (senders[i] == BOT_BOM); |
|
76 |
} |
|
77 |
if (top_em_count * bot_em_count != 0) { |
|
78 |
} else if (top_em_count > 0) { |
|
79 |
motors->set_sides(50, 50, MOTOR_PERCENT); |
|
80 |
} else { |
|
81 |
motors->set_sides(-50, -50, MOTOR_PERCENT); |
|
82 |
} |
|
83 |
identified = 1; |
|
84 |
} |
|
85 |
return identified; |
|
39 | 86 |
} |
40 | 87 |
|
41 | 88 |
void ApproachEmitter::run() { |
42 | 89 |
ROS_INFO("BOM Main Running"); |
43 | 90 |
BomReadings bomR; |
44 |
int r_weighted, l_weighted; |
|
45 |
int speed = 20; |
|
46 | 91 |
vector<uint32_t> r; |
47 | 92 |
vector<uint32_t> s; |
48 | 93 |
while (ok()) { |
49 | 94 |
bomR = bom->query(); |
50 | 95 |
r = bomR.readings; |
51 | 96 |
s = bomR.senders; |
52 |
ROS_INFO("BOM Main Running"); |
|
53 |
if (accumulate(r.begin(), r.end(), 0)) { |
|
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); |
|
97 |
if (identifyPosition(r, s)) { |
|
98 |
// do nothing |
|
99 |
} else { |
|
100 |
motors->set_sides(0, 50, MOTOR_PERCENT); |
|
60 | 101 |
} |
61 | 102 |
} |
103 |
while (ok()) {} |
|
62 | 104 |
} |
Also available in: Unified diff