Statistics
| Branch: | Revision:

scoutos / scout_avr / src / main.cpp @ cc9ca04e

History | View | Annotate | Download (1.86 KB)

1
#if 1 ///////////////////////////////////////////////
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
}
46

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

    
51
Atmega128rfa1 avr;
52

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

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

    
108
#endif //////////////////////////////////////////////