Revision 4bdd00ba
Changed cliff_status_changed to a singe int8 bitmask
scout_avr/src/main.cpp | ||
---|---|---|
52 | 52 |
{ |
53 | 53 |
unsigned long now, next; |
54 | 54 |
unsigned int ranges[2]; |
55 |
char i, id; |
|
56 |
int cliff_front, cliff_left, cliff_right; |
|
55 |
char i, id, cliff_status; |
|
57 | 56 |
|
58 | 57 |
ros::NodeHandle nh; |
59 | 58 |
nh.initNode(); |
... | ... | |
86 | 85 |
nh.advertise(bom_pub); |
87 | 86 |
|
88 | 87 |
/* Headlights (aka Orbs) */ |
89 |
orb_init(); |
|
90 |
/*ros::Subscriber<messages::set_headlights> orb_sub("set_headlights",
|
|
88 |
//orb_init();
|
|
89 |
ros::Subscriber<messages::set_headlights> orb_sub("set_headlights", |
|
91 | 90 |
orb_callback); |
92 |
nh.subscribe(orb_sub);*/
|
|
91 |
nh.subscribe(orb_sub); |
|
93 | 92 |
|
94 | 93 |
/* Cliff sensors */ |
95 | 94 |
cliffSensor_init(); |
... | ... | |
143 | 142 |
range_pub.publish(&range_msg); |
144 | 143 |
} |
145 | 144 |
|
146 |
/* TODO remove raw values and have single bitmask */ |
|
147 |
cliff_front = read_cliffSensor_front(); |
|
148 |
cliff_left = read_cliffSensor_left(); |
|
149 |
cliff_right = read_cliffSensor_right(); |
|
150 |
if (cliff_front != cliff_msg.front_raw || cliff_left != cliff_msg.left_raw |
|
151 |
|| cliff_right != cliff_msg.right_raw) { |
|
152 |
cliff_msg.front_raw = cliff_front; |
|
153 |
cliff_msg.left_raw = cliff_left; |
|
154 |
cliff_msg.right_raw = cliff_right; |
|
155 |
cliff_msg.cliff_status = cliff_front || cliff_left || cliff_right; |
|
145 |
cliff_status = 0; |
|
146 |
if (read_cliffSensor_front()) |
|
147 |
cliff_status |= messages::cliff_status_changed::CLIFF_FRONT; |
|
148 |
if (read_cliffSensor_left()) |
|
149 |
cliff_status |= messages::cliff_status_changed::CLIFF_LEFT; |
|
150 |
if (read_cliffSensor_right()) |
|
151 |
cliff_status |= messages::cliff_status_changed::CLIFF_RIGHT; |
|
152 |
if (cliff_status != cliff_msg.cliff_status) { |
|
153 |
cliff_msg.cliff_status = cliff_status; |
|
156 | 154 |
cliff_pub.publish(&cliff_msg); |
157 | 155 |
} |
158 | 156 |
|
Also available in: Unified diff