1 |
|
#if 1 ///////////////////////////////////////////////
|
2 |
|
|
3 |
1 |
#include "ros.h"
|
4 |
2 |
#include "bom.h"
|
5 |
3 |
#include "range.h"
|
... | ... | |
19 |
17 |
|
20 |
18 |
char range_enabled = 0;
|
21 |
19 |
|
22 |
|
void debug(const char *str)
|
23 |
|
{
|
24 |
|
}
|
25 |
|
|
26 |
20 |
void orb_callback(const messages::set_headlights& msg)
|
27 |
21 |
{
|
28 |
22 |
orb_set0(msg.left_red, msg.left_green, msg.left_blue);
|
... | ... | |
61 |
55 |
// TODO ROS messages to set bounds
|
62 |
56 |
step_init();
|
63 |
57 |
step_dir(1);
|
64 |
|
step_set_size(STEP_WHOLE);
|
|
58 |
step_set_size(STEP_HALF);
|
65 |
59 |
step_sweep_bounds(-26, 26);
|
66 |
60 |
|
67 |
61 |
/* Range */
|
... | ... | |
85 |
79 |
nh.advertise(bom_pub);
|
86 |
80 |
|
87 |
81 |
/* Headlights (aka Orbs) */
|
88 |
|
//orb_init();
|
|
82 |
//orb_init(); TODO debug orbs
|
89 |
83 |
ros::Subscriber<messages::set_headlights> orb_sub("set_headlights",
|
90 |
84 |
orb_callback);
|
91 |
85 |
nh.subscribe(orb_sub);
|
... | ... | |
158 |
152 |
|
159 |
153 |
return 0;
|
160 |
154 |
}
|
161 |
|
|
162 |
|
#else ///////////////////////////////////////////////
|
163 |
|
|
164 |
|
extern "C"
|
165 |
|
{
|
166 |
|
#include <stdlib.h>
|
167 |
|
#include <string.h>
|
168 |
|
#include <avr/io.h>
|
169 |
|
#include <util/delay.h>
|
170 |
|
}
|
171 |
|
|
172 |
|
#include "Atmega128rfa1.h"
|
173 |
|
#include "range.h"
|
174 |
|
#include "bom.h"
|
175 |
|
#include "stepper.h"
|
176 |
|
|
177 |
|
Atmega128rfa1 avr;
|
178 |
|
|
179 |
|
void debug(const char *str)
|
180 |
|
{
|
181 |
|
avr.puts(str);
|
182 |
|
}
|
183 |
|
|
184 |
|
int main()
|
185 |
|
{
|
186 |
|
char buf[20];
|
187 |
|
//int i;
|
188 |
|
//char id = 0;
|
189 |
|
/*unsigned long now, next = 0;
|
190 |
|
unsigned int ranges[2];*/
|
191 |
|
avr.init();
|
192 |
|
range_init();
|
193 |
|
bom_init();
|
194 |
|
func step_sweep_cb = step_init(10);
|
195 |
|
step_sweep_bounds(-26, 26);
|
196 |
|
step_dir(1);
|
197 |
|
step_sweep_speed(50);
|
198 |
|
PORTF |= _BV(PF4);
|
199 |
|
_delay_ms(500);
|
200 |
|
PORTF = (PORTF & ~_BV(PF4)) | _BV(PF5);
|
201 |
|
_delay_ms(500);
|
202 |
|
PORTF &= ~_BV(PF5);
|
203 |
|
while (1)
|
204 |
|
{
|
205 |
|
step_sweep_cb();
|
206 |
|
_delay_ms(10);
|
207 |
|
/*now = avr.time();
|
208 |
|
if (now > next) {
|
209 |
|
next = now + 500;*/
|
210 |
|
/*ultoa(now, buf, 10);
|
211 |
|
avr.puts(buf);
|
212 |
|
avr.puts("\r\n");*/
|
213 |
|
/*id++;
|
214 |
|
if (id == 0x40) {
|
215 |
|
id = 0;
|
216 |
|
}
|
217 |
|
set_robot_id(id);*/
|
218 |
|
/*range_measure(ranges);
|
219 |
|
utoa(ranges[0], buf, 10);
|
220 |
|
avr.puts("Range: ");
|
221 |
|
avr.puts(buf);
|
222 |
|
avr.puts(", ");
|
223 |
|
utoa(ranges[1], buf, 10);
|
224 |
|
avr.puts(buf);
|
225 |
|
avr.puts("\r\n");*/
|
226 |
|
/*for (i = 0; i < 4; i++) {
|
227 |
|
bom_send(i);
|
228 |
|
int msg = bom_get(i);
|
229 |
|
if (msg != BOM_NO_DATA) {
|
230 |
|
avr.puts("BOM ");
|
231 |
|
itoa(i, buf, 10);
|
232 |
|
avr.puts(buf);
|
233 |
|
avr.puts(": ");
|
234 |
|
itoa(bom_msg_get_robot_id(msg), buf, 10);
|
235 |
|
avr.puts(buf);
|
236 |
|
avr.puts(" (");
|
237 |
|
itoa(bom_msg_get_dir(msg), buf, 10);
|
238 |
|
avr.puts(buf);
|
239 |
|
avr.puts(")\r\n");
|
240 |
|
}
|
241 |
|
}*/
|
242 |
|
//}
|
243 |
|
}
|
244 |
|
return 0;
|
245 |
|
}
|
246 |
|
|
247 |
|
#endif //////////////////////////////////////////////
|