Revision 86b48573
Range sensors work now! :D
scout_avr/src/main.cpp | ||
---|---|---|
12 | 12 |
#include <util/delay.h> |
13 | 13 |
|
14 | 14 |
/* Period of main loop in ms */ |
15 |
#define MAINLOOP_PERIOD 50 |
|
15 |
#define MAINLOOP_PERIOD 1000//50
|
|
16 | 16 |
|
17 | 17 |
char range_enabled = 0; |
18 | 18 |
|
... | ... | |
55 | 55 |
/* Range */ |
56 | 56 |
range_init(); |
57 | 57 |
sonar::sonar_distance range_msg; |
58 |
ros::Publisher range_distance("sonar_distance", &range_msg);
|
|
58 |
ros::Publisher range_pub("sonar_distance", &range_msg);
|
|
59 | 59 |
ros::ServiceServer |
60 | 60 |
<sonar::sonar_toggleRequest, sonar::sonar_toggleResponse> |
61 | 61 |
range_toggle("sonar_toggle", range_toggle_cb); |
62 | 62 |
ros::ServiceServer |
63 | 63 |
<sonar::sonar_set_scanRequest, sonar::sonar_set_scanResponse> |
64 | 64 |
range_set_scan("sonar_set_scan", range_set_scan_cb); |
65 |
nh.advertise(range_pub); |
|
65 | 66 |
|
66 | 67 |
/* BOM */ |
67 | 68 |
bom_init(); |
... | ... | |
111 | 112 |
range_msg.pos = 0; // TODO fill this |
112 | 113 |
range_msg.distance0 = ranges[0]; |
113 | 114 |
range_msg.distance1 = ranges[1]; |
115 |
range_pub.publish(&range_msg); |
|
114 | 116 |
|
115 | 117 |
} |
116 | 118 |
|
... | ... | |
142 | 144 |
int main() |
143 | 145 |
{ |
144 | 146 |
char buf[20]; |
145 |
int i; |
|
146 |
char id = 0; |
|
147 |
//int i;
|
|
148 |
//char id = 0;
|
|
147 | 149 |
unsigned long now, next = 0; |
150 |
unsigned int ranges[2]; |
|
148 | 151 |
avr.init(); |
149 | 152 |
range_init(); |
150 | 153 |
bom_init(); |
151 |
func step_sweep_cb = step_init(10); |
|
154 |
/*func step_sweep_cb = step_init(10);
|
|
152 | 155 |
step_sweep_bounds(-26, 26); |
153 | 156 |
step_dir(1); |
154 |
step_sweep_speed(50); |
|
157 |
step_sweep_speed(50);*/
|
|
155 | 158 |
avr.puts("Hello!\r\n"); |
156 | 159 |
while (1) |
157 | 160 |
{ |
158 |
step_sweep_cb(); |
|
159 |
_delay_ms(10); |
|
160 |
/*now = avr.time();
|
|
161 |
/*step_sweep_cb();
|
|
162 |
_delay_ms(10);*/
|
|
163 |
now = avr.time(); |
|
161 | 164 |
if (now > next) { |
162 |
next = now + 50;*/
|
|
165 |
next = now + 500;
|
|
163 | 166 |
//step_sweep_cb(); |
164 | 167 |
/*ultoa(now, buf, 10); |
165 | 168 |
avr.puts(buf); |
... | ... | |
169 | 172 |
id = 0; |
170 | 173 |
} |
171 | 174 |
set_robot_id(id);*/ |
172 |
/*utoa(range_get(0), buf, 10); |
|
175 |
range_measure(ranges); |
|
176 |
utoa(ranges[0], buf, 10); |
|
173 | 177 |
avr.puts("Range: "); |
174 | 178 |
avr.puts(buf); |
175 | 179 |
avr.puts(", "); |
176 |
utoa(range_get(1), buf, 10); |
|
177 |
avr.puts(buf);*/ |
|
180 |
utoa(ranges[1], buf, 10); |
|
181 |
avr.puts(buf); |
|
182 |
avr.puts("\r\n"); |
|
178 | 183 |
/*for (i = 0; i < 4; i++) { |
179 | 184 |
bom_send(i); |
180 | 185 |
int msg = bom_get(i); |
... | ... | |
191 | 196 |
avr.puts(")\r\n"); |
192 | 197 |
} |
193 | 198 |
}*/ |
194 |
//}
|
|
199 |
} |
|
195 | 200 |
} |
196 | 201 |
return 0; |
197 | 202 |
} |
Also available in: Unified diff