Statistics
| Branch: | Revision:

root / scout_avr / src / main.cpp @ 812788aa

History | View | Annotate | Download (1.86 KB)

1 807483bf Tom Mullins
#if 0 ///////////////////////////////////////////////
2

3 49090532 Tom Mullins
#include "ros.h"
4 cf115e3d Tom Mullins
#include <std_msgs/Int16.h>
5 88fb3a79 Tom Mullins

6 49090532 Tom Mullins
ros::Publisher *ptest_out;
7

8 812788aa Tom Mullins
void debug(const char *str)
9
{
10 31f4a032 Tom Mullins
}
11

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

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

24
  ptest_out = &test_out;
25 88fb3a79 Tom Mullins

26 49090532 Tom Mullins
  msg.data = 0;
27 88fb3a79 Tom Mullins
  nh.initNode();
28 49090532 Tom Mullins
  nh.subscribe(test_in);
29
  nh.advertise(test_out);
30 88fb3a79 Tom Mullins

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

36
  return 0;
37 49090532 Tom Mullins
}
38 88fb3a79 Tom Mullins

39 807483bf Tom Mullins
#else ///////////////////////////////////////////////
40
41
extern "C"
42 88fb3a79 Tom Mullins
{
43
#include <stdlib.h>
44
#include <string.h>
45
}
46
47 49090532 Tom Mullins
#include "Atmega128rfa1.h"
48 1c3c96ce Tom Mullins
#include "range.h"
49 f115416e Tom Mullins
#include "bom.h"
50 49090532 Tom Mullins
51 31f4a032 Tom Mullins
Atmega128rfa1 avr;
52
53 812788aa Tom Mullins
void debug(const char *str)
54
{
55 31f4a032 Tom Mullins
  avr.puts(str);
56
}
57
58 88fb3a79 Tom Mullins
int main()
59
{
60 1c3c96ce Tom Mullins
  char buf[20];
61 f115416e Tom Mullins
  int i;
62 31f4a032 Tom Mullins
  char id = 0;
63 cf115e3d Tom Mullins
  unsigned long now, next = 0;
64 88fb3a79 Tom Mullins
  avr.init();
65 1c3c96ce Tom Mullins
  range_init();
66 f115416e Tom Mullins
  bom_init();
67 31f4a032 Tom Mullins
  avr.puts("Hello!\r\n");
68 88fb3a79 Tom Mullins
  while (1)
69
  {
70 49090532 Tom Mullins
    now = avr.time();
71
    if (now > next) {
72 1c3c96ce Tom Mullins
      next = now + 100;
73 812788aa Tom Mullins
      ultoa(now, buf, 10);
74
      avr.puts(buf);
75
      avr.puts("\r\n");
76 31f4a032 Tom Mullins
      id++;
77
      if (id == 0x40) {
78
        id = 0;
79
      }
80
      set_robot_id(id);
81 f115416e Tom Mullins
      /*utoa(range_get(0), buf, 10);
82
      avr.puts("Range: ");
83
      avr.puts(buf);
84
      avr.puts(", ");
85 807483bf Tom Mullins
      utoa(range_get(1), buf, 10);
86 f115416e Tom Mullins
      avr.puts(buf);*/
87 812788aa Tom Mullins
      /*for (i = 0; i < 4; i++) {
88 f115416e Tom Mullins
        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 31f4a032 Tom Mullins
          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 f115416e Tom Mullins
          avr.puts(buf);
100 31f4a032 Tom Mullins
          avr.puts(")\r\n");
101 f115416e Tom Mullins
        }
102 812788aa Tom Mullins
      }*/
103 49090532 Tom Mullins
    }
104 88fb3a79 Tom Mullins
  }
105
  return 0;
106 cf115e3d Tom Mullins
}
107 807483bf Tom Mullins
108
#endif //////////////////////////////////////////////