Revision 31f4a032
ID | 31f4a0323d38938bb40b2da834fa73fc15003faf |
Some fixes to BOM, which is now tested and it works
scout_avr/src/main.cpp | ||
---|---|---|
5 | 5 |
|
6 | 6 |
ros::Publisher *ptest_out; |
7 | 7 |
|
8 |
void debug(const char *str) { |
|
9 |
} |
|
10 |
|
|
8 | 11 |
void callback(const std_msgs::Int16& msg) |
9 | 12 |
{ |
10 | 13 |
ptest_out->publish(&msg); |
... | ... | |
44 | 47 |
#include "range.h" |
45 | 48 |
#include "bom.h" |
46 | 49 |
|
50 |
Atmega128rfa1 avr; |
|
51 |
|
|
52 |
void debug(const char *str) { |
|
53 |
avr.puts(str); |
|
54 |
} |
|
55 |
|
|
47 | 56 |
int main() |
48 | 57 |
{ |
49 | 58 |
char buf[20]; |
50 | 59 |
int i; |
60 |
char id = 0; |
|
51 | 61 |
unsigned long now, next = 0; |
52 |
Atmega128rfa1 avr; |
|
53 | 62 |
avr.init(); |
54 | 63 |
range_init(); |
55 | 64 |
bom_init(); |
56 |
set_robot_id(0x2a);
|
|
65 |
avr.puts("Hello!\r\n");
|
|
57 | 66 |
while (1) |
58 | 67 |
{ |
59 | 68 |
now = avr.time(); |
60 | 69 |
if (now > next) { |
61 | 70 |
next = now + 100; |
71 |
id++; |
|
72 |
if (id == 0x40) { |
|
73 |
id = 0; |
|
74 |
} |
|
75 |
set_robot_id(id); |
|
62 | 76 |
/*utoa(range_get(0), buf, 10); |
63 | 77 |
avr.puts("Range: "); |
64 | 78 |
avr.puts(buf); |
... | ... | |
73 | 87 |
itoa(i, buf, 10); |
74 | 88 |
avr.puts(buf); |
75 | 89 |
avr.puts(": "); |
76 |
itoa(msg, buf, 10); |
|
90 |
itoa(bom_msg_get_robot_id(msg), buf, 10); |
|
91 |
avr.puts(buf); |
|
92 |
avr.puts(" ("); |
|
93 |
itoa(bom_msg_get_dir(msg), buf, 10); |
|
77 | 94 |
avr.puts(buf); |
95 |
avr.puts(")\r\n"); |
|
78 | 96 |
} |
79 | 97 |
} |
80 |
avr.puts("\r\n"); |
|
81 | 98 |
} |
82 | 99 |
} |
83 | 100 |
return 0; |
Also available in: Unified diff