Statistics
| Branch: | Revision:

root / scout_avr / src / main.cpp @ 6e7f0a98

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