Statistics
| Branch: | Revision:

root / scout_avr / src / main.cpp @ 31f4a032

History | View | Annotate | Download (1.79 KB)

1
#if 0 ///////////////////////////////////////////////
2

3
#include "ros.h"
4
#include <std_msgs/Int16.h>
5

6
ros::Publisher *ptest_out;
7

8
void debug(const char *str) {
9
}
10

11
void callback(const std_msgs::Int16& msg)
12
{
13
  ptest_out->publish(&msg);
14
}
15

16
int main()
17
{
18
  ros::NodeHandle nh;
19
  std_msgs::Int16 msg;
20
  ros::Subscriber<std_msgs::Int16> test_in("test_in", callback);
21
  ros::Publisher test_out("test_out", &msg);
22

23
  ptest_out = &test_out;
24

25
  msg.data = 0;
26
  nh.initNode();
27
  nh.subscribe(test_in);
28
  nh.advertise(test_out);
29

30
  while (1)
31
  {
32
    nh.spinOnce();
33
  }
34

35
  return 0;
36
}
37

38
#else ///////////////////////////////////////////////
39

    
40
extern "C"
41
{
42
#include <stdlib.h>
43
#include <string.h>
44
}
45

    
46
#include "Atmega128rfa1.h"
47
#include "range.h"
48
#include "bom.h"
49

    
50
Atmega128rfa1 avr;
51

    
52
void debug(const char *str) {
53
  avr.puts(str);
54
}
55

    
56
int main()
57
{
58
  char buf[20];
59
  int i;
60
  char id = 0;
61
  unsigned long now, next = 0;
62
  avr.init();
63
  range_init();
64
  bom_init();
65
  avr.puts("Hello!\r\n");
66
  while (1)
67
  {
68
    now = avr.time();
69
    if (now > next) {
70
      next = now + 100;
71
      id++;
72
      if (id == 0x40) {
73
        id = 0;
74
      }
75
      set_robot_id(id);
76
      /*utoa(range_get(0), buf, 10);
77
      avr.puts("Range: ");
78
      avr.puts(buf);
79
      avr.puts(", ");
80
      utoa(range_get(1), buf, 10);
81
      avr.puts(buf);*/
82
      for (i = 0; i < 4; i++) {
83
        bom_send(i);
84
        int msg = bom_get(i);
85
        if (msg != BOM_NO_DATA) {
86
          avr.puts("BOM ");
87
          itoa(i, buf, 10);
88
          avr.puts(buf);
89
          avr.puts(": ");
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);
94
          avr.puts(buf);
95
          avr.puts(")\r\n");
96
        }
97
      }
98
    }
99
  }
100
  return 0;
101
}
102

    
103
#endif //////////////////////////////////////////////