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