Statistics
| Branch: | Revision:

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

History | View | Annotate | Download (2.25 KB)

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

    
3
#include "ros.h"
4
#include "bom.h"
5

    
6
#include <std_msgs/Int16.h>
7

    
8
ros::Publisher *pbom_pub;
9

    
10
void debug(const char *str)
11
{
12
}
13

    
14
void callback(const std_msgs::Int16& msg)
15
{
16
}
17

    
18
int main()
19
{
20
  char i, id;
21
  ros::NodeHandle nh;
22
  bom::bom bom_msg;
23
  ros::Subscriber<std_msgs::Int16> test_in("test_in", callback);
24
  ros::Publisher bom_pub("bom", &bom_msg);
25

    
26
  pbom_pub = &bom_pub;
27

    
28
  nh.initNode();
29
  nh.subscribe(test_in);
30
  nh.advertise(bom_pub);
31

    
32
  id = 0;
33
  while (1)
34
  {
35
    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
    nh.spinOnce();
51
  }
52

    
53
  return 0;
54
}
55

    
56
#else ///////////////////////////////////////////////
57

    
58
extern "C"
59
{
60
#include <stdlib.h>
61
#include <string.h>
62
#include <avr/io.h>
63
#include <util/delay.h>
64
}
65

    
66
#include "Atmega128rfa1.h"
67
#include "range.h"
68
#include "bom.h"
69

    
70
Atmega128rfa1 avr;
71

    
72
void debug(const char *str)
73
{
74
  avr.puts(str);
75
}
76

    
77
int main()
78
{
79
  char buf[20];
80
  int i;
81
  char id = 0;
82
  unsigned long now, next = 0;
83
  avr.init();
84
  range_init();
85
  bom_init();
86
  avr.puts("Hello!\r\n");
87
  while (1)
88
  {
89
    now = avr.time();
90
    if (now > next) {
91
      next = now + 500;
92
      /*ultoa(now, buf, 10);
93
      avr.puts(buf);
94
      avr.puts("\r\n");*/
95
      id++;
96
      if (id == 0x40) {
97
        id = 0;
98
      }
99
      set_robot_id(id);
100
      /*utoa(range_get(0), buf, 10);
101
      avr.puts("Range: ");
102
      avr.puts(buf);
103
      avr.puts(", ");
104
      utoa(range_get(1), buf, 10);
105
      avr.puts(buf);*/
106
      for (i = 0; i < 4; i++) {
107
        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
          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
          avr.puts(buf);
119
          avr.puts(")\r\n");
120
        }
121
      }
122
    }
123
  }
124
  return 0;
125
}
126

    
127
#endif //////////////////////////////////////////////