Statistics
| Branch: | Revision:

scoutos / scout_avr / src / main.cpp @ fd73d758

History | View | Annotate | Download (1.9 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

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

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

24
  ptest_out = &test_out;
25

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

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

36
  return 0;
37
}
38

39
#else ///////////////////////////////////////////////
40

    
41
extern "C"
42
{
43
#include <stdlib.h>
44
#include <string.h>
45
#include <avr/io.h>
46
#include <util/delay.h>
47
}
48

    
49
#include "Atmega128rfa1.h"
50
#include "range.h"
51
#include "bom.h"
52

    
53
Atmega128rfa1 avr;
54

    
55
void debug(const char *str)
56
{
57
  avr.puts(str);
58
}
59

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

    
110
#endif //////////////////////////////////////////////