Revision 31f4a032 scout_avr/src/main.cpp
| 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