Statistics
| Branch: | Revision:

root / scout_avr / src / main.cpp @ fd73d758

History | View | Annotate | Download (1.9 KB)

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

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