Statistics
| Branch: | Revision:

root / scout_avr / src / main.cpp @ 47e26dee

History | View | Annotate | Download (2.34 KB)

1 6e7f0a98 Tom Mullins
#if 1 ///////////////////////////////////////////////
2 807483bf Tom Mullins
3 49090532 Tom Mullins
#include "ros.h"
4 6e7f0a98 Tom Mullins
#include "bom.h"
5 47e26dee Tom Mullins
#include "range.h"
6 cf115e3d Tom Mullins
#include <std_msgs/Int16.h>
7 47e26dee Tom Mullins
#include <util/delay.h>
8 88fb3a79 Tom Mullins
9 6e7f0a98 Tom Mullins
ros::Publisher *pbom_pub;
10 49090532 Tom Mullins
11 812788aa Tom Mullins
void debug(const char *str)
12
{
13 31f4a032 Tom Mullins
}
14
15 cf115e3d Tom Mullins
void callback(const std_msgs::Int16& msg)
16 49090532 Tom Mullins
{
17
}
18
19
int main()
20 88fb3a79 Tom Mullins
{
21 6e7f0a98 Tom Mullins
  char i, id;
22 88fb3a79 Tom Mullins
  ros::NodeHandle nh;
23 6e7f0a98 Tom Mullins
  bom::bom bom_msg;
24 cf115e3d Tom Mullins
  ros::Subscriber<std_msgs::Int16> test_in("test_in", callback);
25 6e7f0a98 Tom Mullins
  ros::Publisher bom_pub("bom", &bom_msg);
26 49090532 Tom Mullins
27 6e7f0a98 Tom Mullins
  pbom_pub = &bom_pub;
28 88fb3a79 Tom Mullins
29
  nh.initNode();
30 47e26dee Tom Mullins
  range_init();
31
  bom_init();
32 49090532 Tom Mullins
  nh.subscribe(test_in);
33 6e7f0a98 Tom Mullins
  nh.advertise(bom_pub);
34 88fb3a79 Tom Mullins
35 6e7f0a98 Tom Mullins
  id = 0;
36 88fb3a79 Tom Mullins
  while (1)
37
  {
38 6e7f0a98 Tom Mullins
    id++;
39
    if (id == 0x40) {
40
      id = 0;
41
    }
42
    set_robot_id(id);
43 47e26dee Tom Mullins
44
    bom_send(id & 1);
45 6e7f0a98 Tom Mullins
    for (i = 0; i < 4; i++) {
46
      int msg = bom_get(i);
47
      if (msg != BOM_NO_DATA) {
48
        bom_msg.sender = bom_msg_get_robot_id(msg);
49
        bom_msg.send_dir = bom_msg_get_dir(msg);
50
        bom_msg.recv_dir = i;
51
        bom_pub.publish(&bom_msg);
52
      }
53
    }
54 47e26dee Tom Mullins
55 88fb3a79 Tom Mullins
    nh.spinOnce();
56 47e26dee Tom Mullins
    _delay_ms(500);
57 88fb3a79 Tom Mullins
  }
58
59
  return 0;
60 49090532 Tom Mullins
}
61 88fb3a79 Tom Mullins
62 807483bf Tom Mullins
#else ///////////////////////////////////////////////
63
64
extern "C"
65 88fb3a79 Tom Mullins
{
66
#include <stdlib.h>
67
#include <string.h>
68 fd73d758 Tom Mullins
#include <avr/io.h>
69
#include <util/delay.h>
70 88fb3a79 Tom Mullins
}
71
72 49090532 Tom Mullins
#include "Atmega128rfa1.h"
73 1c3c96ce Tom Mullins
#include "range.h"
74 f115416e Tom Mullins
#include "bom.h"
75 49090532 Tom Mullins
76 31f4a032 Tom Mullins
Atmega128rfa1 avr;
77
78 812788aa Tom Mullins
void debug(const char *str)
79
{
80 31f4a032 Tom Mullins
  avr.puts(str);
81
}
82
83 88fb3a79 Tom Mullins
int main()
84
{
85 1c3c96ce Tom Mullins
  char buf[20];
86 f115416e Tom Mullins
  int i;
87 31f4a032 Tom Mullins
  char id = 0;
88 cf115e3d Tom Mullins
  unsigned long now, next = 0;
89 88fb3a79 Tom Mullins
  avr.init();
90 1c3c96ce Tom Mullins
  range_init();
91 f115416e Tom Mullins
  bom_init();
92 31f4a032 Tom Mullins
  avr.puts("Hello!\r\n");
93 88fb3a79 Tom Mullins
  while (1)
94
  {
95 49090532 Tom Mullins
    now = avr.time();
96
    if (now > next) {
97 6e7f0a98 Tom Mullins
      next = now + 500;
98 fd73d758 Tom Mullins
      /*ultoa(now, buf, 10);
99 812788aa Tom Mullins
      avr.puts(buf);
100 fd73d758 Tom Mullins
      avr.puts("\r\n");*/
101 31f4a032 Tom Mullins
      id++;
102
      if (id == 0x40) {
103
        id = 0;
104
      }
105
      set_robot_id(id);
106 f115416e Tom Mullins
      /*utoa(range_get(0), buf, 10);
107
      avr.puts("Range: ");
108
      avr.puts(buf);
109
      avr.puts(", ");
110 807483bf Tom Mullins
      utoa(range_get(1), buf, 10);
111 f115416e Tom Mullins
      avr.puts(buf);*/
112 fd73d758 Tom Mullins
      for (i = 0; i < 4; i++) {
113 f115416e Tom Mullins
        bom_send(i);
114
        int msg = bom_get(i);
115
        if (msg != BOM_NO_DATA) {
116
          avr.puts("BOM ");
117
          itoa(i, buf, 10);
118
          avr.puts(buf);
119
          avr.puts(": ");
120 31f4a032 Tom Mullins
          itoa(bom_msg_get_robot_id(msg), buf, 10);
121
          avr.puts(buf);
122
          avr.puts(" (");
123
          itoa(bom_msg_get_dir(msg), buf, 10);
124 f115416e Tom Mullins
          avr.puts(buf);
125 31f4a032 Tom Mullins
          avr.puts(")\r\n");
126 f115416e Tom Mullins
        }
127 fd73d758 Tom Mullins
      }
128 49090532 Tom Mullins
    }
129 88fb3a79 Tom Mullins
  }
130
  return 0;
131 cf115e3d Tom Mullins
}
132 807483bf Tom Mullins
133
#endif //////////////////////////////////////////////