Statistics
| Branch: | Revision:

root / scout_avr / src / main.cpp @ f115416e

History | View | Annotate | Download (1.48 KB)

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

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

6
ros::Publisher *ptest_out;
7

8
void callback(const std_msgs::Int16& msg)
9
{
10
  ptest_out->publish(&msg);
11
}
12

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

20
  ptest_out = &test_out;
21

22
  msg.data = 0;
23
  nh.initNode();
24
  nh.subscribe(test_in);
25
  nh.advertise(test_out);
26

27
  while (1)
28
  {
29
    nh.spinOnce();
30
  }
31

32
  return 0;
33
}
34

35
#else ///////////////////////////////////////////////
36

    
37
extern "C"
38
{
39
#include <stdlib.h>
40
#include <string.h>
41
}
42

    
43
#include "Atmega128rfa1.h"
44
#include "range.h"
45
#include "bom.h"
46

    
47
int main()
48
{
49
  char buf[20];
50
  int i;
51
  unsigned long now, next = 0;
52
  Atmega128rfa1 avr;
53
  avr.init();
54
  range_init();
55
  bom_init();
56
  set_robot_id(0x2a);
57
  while (1)
58
  {
59
    now = avr.time();
60
    if (now > next) {
61
      next = now + 100;
62
      /*utoa(range_get(0), buf, 10);
63
      avr.puts("Range: ");
64
      avr.puts(buf);
65
      avr.puts(", ");
66
      utoa(range_get(1), buf, 10);
67
      avr.puts(buf);*/
68
      for (i = 0; i < 4; i++) {
69
        bom_send(i);
70
        int msg = bom_get(i);
71
        if (msg != BOM_NO_DATA) {
72
          avr.puts("BOM ");
73
          itoa(i, buf, 10);
74
          avr.puts(buf);
75
          avr.puts(": ");
76
          itoa(msg, buf, 10);
77
          avr.puts(buf);
78
        }
79
      }
80
      avr.puts("\r\n");
81
    }
82
  }
83
  return 0;
84
}
85

    
86
#endif //////////////////////////////////////////////