Statistics
| Branch: | Revision:

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

History | View | Annotate | Download (2.34 KB)

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

    
3
#include "ros.h"
4
#include "bom.h"
5
#include "range.h"
6
#include <std_msgs/Int16.h>
7
#include <util/delay.h>
8

    
9
ros::Publisher *pbom_pub;
10

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

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

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

    
27
  pbom_pub = &bom_pub;
28

    
29
  nh.initNode();
30
  range_init();
31
  bom_init();
32
  nh.subscribe(test_in);
33
  nh.advertise(bom_pub);
34

    
35
  id = 0;
36
  while (1)
37
  {
38
    id++;
39
    if (id == 0x40) {
40
      id = 0;
41
    }
42
    set_robot_id(id);
43

    
44
    bom_send(id & 1);
45
    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

    
55
    nh.spinOnce();
56
    _delay_ms(500);
57
  }
58

    
59
  return 0;
60
}
61

    
62
#else ///////////////////////////////////////////////
63

    
64
extern "C"
65
{
66
#include <stdlib.h>
67
#include <string.h>
68
#include <avr/io.h>
69
#include <util/delay.h>
70
}
71

    
72
#include "Atmega128rfa1.h"
73
#include "range.h"
74
#include "bom.h"
75

    
76
Atmega128rfa1 avr;
77

    
78
void debug(const char *str)
79
{
80
  avr.puts(str);
81
}
82

    
83
int main()
84
{
85
  char buf[20];
86
  int i;
87
  char id = 0;
88
  unsigned long now, next = 0;
89
  avr.init();
90
  range_init();
91
  bom_init();
92
  avr.puts("Hello!\r\n");
93
  while (1)
94
  {
95
    now = avr.time();
96
    if (now > next) {
97
      next = now + 500;
98
      /*ultoa(now, buf, 10);
99
      avr.puts(buf);
100
      avr.puts("\r\n");*/
101
      id++;
102
      if (id == 0x40) {
103
        id = 0;
104
      }
105
      set_robot_id(id);
106
      /*utoa(range_get(0), buf, 10);
107
      avr.puts("Range: ");
108
      avr.puts(buf);
109
      avr.puts(", ");
110
      utoa(range_get(1), buf, 10);
111
      avr.puts(buf);*/
112
      for (i = 0; i < 4; i++) {
113
        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
          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
          avr.puts(buf);
125
          avr.puts(")\r\n");
126
        }
127
      }
128
    }
129
  }
130
  return 0;
131
}
132

    
133
#endif //////////////////////////////////////////////