Revision daf6a575
ID | daf6a575208a562e4551f9111a98d0c3a636aceb |
Added position to published sonar messages
scout_avr/src/main.cpp | ||
---|---|---|
5 | 5 |
#include "range.h" |
6 | 6 |
#include "stepper.h" |
7 | 7 |
#include "orb.h" |
8 |
#include <std_msgs/Int16.h> // TODO remove |
|
9 | 8 |
#include <bom/bom.h> |
10 | 9 |
#include <sonar/sonar_distance.h> |
11 | 10 |
#include <sonar/sonar_toggle.h> |
... | ... | |
84 | 83 |
nh.advertise(bom_pub); |
85 | 84 |
|
86 | 85 |
/* Headlights (aka Orbs) */ |
87 |
orb_init(); |
|
86 |
//orb_init();
|
|
88 | 87 |
ros::Subscriber<headlights::set_headlights> orb_sub("set_headlights", |
89 | 88 |
orb_callback); |
90 | 89 |
nh.subscribe(orb_sub); |
... | ... | |
129 | 128 |
range_measure(ranges); |
130 | 129 |
range_msg.header.stamp = nh.now(); |
131 | 130 |
range_msg.header.seq++; |
132 |
range_msg.pos = 0; // TODO fill this
|
|
131 |
range_msg.pos = step_get_pos();
|
|
133 | 132 |
range_msg.distance0 = ranges[0]; |
134 | 133 |
range_msg.distance1 = ranges[1]; |
135 | 134 |
range_pub.publish(&range_msg); |
scout_avr/src/stepper.cpp | ||
---|---|---|
108 | 108 |
if((step.dir == 1) && (step.cw <= step.pos)) step_dir(-1); |
109 | 109 |
else if((step.dir == -1) && (step.ccw >= step.pos)) step_dir(1); |
110 | 110 |
} |
111 |
|
|
112 |
int step_get_pos() |
|
113 |
{ |
|
114 |
return step.pos; |
|
115 |
} |
scout_avr/src/stepper.h | ||
---|---|---|
25 | 25 |
void step_sweep(); |
26 | 26 |
void step_enable(); |
27 | 27 |
void step_disable(); |
28 |
int step_get_pos(); |
|
28 | 29 |
|
29 | 30 |
|
30 | 31 |
#endif |
Also available in: Unified diff