Statistics
| Branch: | Revision:

scoutos / scout_avr / src / main.cpp @ 86b48573

History | View | Annotate | Download (4.37 KB)

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

    
3
#include "ros.h"
4
#include "bom.h"
5
#include "range.h"
6
#include "stepper.h"
7
#include <std_msgs/Int16.h> // TODO remove
8
#include <bom/bom.h>
9
#include <sonar/sonar_distance.h>
10
#include <sonar/sonar_toggle.h>
11
#include <sonar/sonar_set_scan.h>
12
#include <util/delay.h>
13

    
14
/* Period of main loop in ms */
15
#define MAINLOOP_PERIOD 1000//50
16

    
17
char range_enabled = 0;
18

    
19
void debug(const char *str)
20
{
21
}
22

    
23
void callback(const std_msgs::Int16& msg)
24
{
25
}
26

    
27
/* dammit, Priya, this capitalization just looks ridiculous */
28
void range_toggle_cb(const sonar::sonar_toggleRequest& req,
29
    sonar::sonar_toggleResponse& resp)
30
{
31
  range_enabled = req.set_on;
32
  resp.ack = true;
33
}
34

    
35
void range_set_scan_cb(const sonar::sonar_set_scanRequest& req,
36
    sonar::sonar_set_scanResponse& resp)
37
{
38
  step_sweep_bounds(req.stop_l, req.stop_r);
39
  resp.ack = true;
40
}
41

    
42
int main()
43
{
44
  unsigned long now, next;
45
  unsigned int ranges[2];
46
  char i, id;
47

    
48
  ros::NodeHandle nh;
49
  nh.initNode();
50

    
51
  /* To be removed later; just an example of a subscirber */
52
  ros::Subscriber<std_msgs::Int16> test_in("test_in", callback);
53
  nh.subscribe(test_in);
54

    
55
  /* Range */
56
  range_init();
57
  sonar::sonar_distance range_msg;
58
  ros::Publisher range_pub("sonar_distance", &range_msg);
59
  ros::ServiceServer
60
    <sonar::sonar_toggleRequest, sonar::sonar_toggleResponse>
61
    range_toggle("sonar_toggle", range_toggle_cb);
62
  ros::ServiceServer
63
    <sonar::sonar_set_scanRequest, sonar::sonar_set_scanResponse>
64
    range_set_scan("sonar_set_scan", range_set_scan_cb);
65
  nh.advertise(range_pub);
66

    
67
  /* BOM */
68
  bom_init();
69
  bom::bom bom_msg;
70
  ros::Publisher bom_pub("bom", &bom_msg);
71
  nh.advertise(bom_pub);
72

    
73
  id = 0;
74
  next = 0;
75
  while (1)
76
  {
77
    nh.spinOnce();
78

    
79
    /* Skip loop if the loop period hasn't passed yet */
80
    /* TODO if we need more exact timing, we can enter a tight loop when now
81
     * gets close to next, and avoid the uncertainty of nh.spinOnce() */
82
    now = nh.getHardware()->time();
83
    if (now < next) {
84
      continue;
85
    }
86
    next = now + MAINLOOP_PERIOD;
87

    
88
    /* Temporary, for BOM testing */
89
    id++;
90
    if (id == 0x40) {
91
      id = 0;
92
    }
93
    set_robot_id(id);
94
    bom_send(id & 1);
95

    
96
    /* BOM */
97
    for (i = 0; i < 4; i++) {
98
      int msg = bom_get(i);
99
      if (msg != BOM_NO_DATA) {
100
        bom_msg.sender = bom_msg_get_robot_id(msg);
101
        bom_msg.send_dir = bom_msg_get_dir(msg);
102
        bom_msg.recv_dir = i;
103
        bom_pub.publish(&bom_msg);
104
      }
105
    }
106

    
107
    /* Stepper / range sensor */
108
    // TODO sweep stepper
109
    range_measure(ranges);
110
    range_msg.header.stamp = nh.now();
111
    range_msg.header.seq++;
112
    range_msg.pos = 0; // TODO fill this
113
    range_msg.distance0 = ranges[0];
114
    range_msg.distance1 = ranges[1];
115
    range_pub.publish(&range_msg);
116

    
117
  }
118

    
119
  return 0;
120
}
121

    
122
#else ///////////////////////////////////////////////
123

    
124
extern "C"
125
{
126
#include <stdlib.h>
127
#include <string.h>
128
#include <avr/io.h>
129
#include <util/delay.h>
130
}
131

    
132
#include "Atmega128rfa1.h"
133
#include "range.h"
134
#include "bom.h"
135
#include "stepper.h"
136

    
137
Atmega128rfa1 avr;
138

    
139
void debug(const char *str)
140
{
141
  avr.puts(str);
142
}
143

    
144
int main()
145
{
146
  char buf[20];
147
  //int i;
148
  //char id = 0;
149
  unsigned long now, next = 0;
150
  unsigned int ranges[2];
151
  avr.init();
152
  range_init();
153
  bom_init();
154
  /*func step_sweep_cb = step_init(10);
155
  step_sweep_bounds(-26, 26);
156
  step_dir(1);
157
  step_sweep_speed(50);*/
158
  avr.puts("Hello!\r\n");
159
  while (1)
160
  {
161
    /*step_sweep_cb();
162
    _delay_ms(10);*/
163
    now = avr.time();
164
    if (now > next) {
165
      next = now + 500;
166
      //step_sweep_cb();
167
      /*ultoa(now, buf, 10);
168
      avr.puts(buf);
169
      avr.puts("\r\n");*/
170
      /*id++;
171
      if (id == 0x40) {
172
        id = 0;
173
      }
174
      set_robot_id(id);*/
175
      range_measure(ranges);
176
      utoa(ranges[0], buf, 10);
177
      avr.puts("Range: ");
178
      avr.puts(buf);
179
      avr.puts(", ");
180
      utoa(ranges[1], buf, 10);
181
      avr.puts(buf);
182
      avr.puts("\r\n");
183
      /*for (i = 0; i < 4; i++) {
184
        bom_send(i);
185
        int msg = bom_get(i);
186
        if (msg != BOM_NO_DATA) {
187
          avr.puts("BOM ");
188
          itoa(i, buf, 10);
189
          avr.puts(buf);
190
          avr.puts(": ");
191
          itoa(bom_msg_get_robot_id(msg), buf, 10);
192
          avr.puts(buf);
193
          avr.puts(" (");
194
          itoa(bom_msg_get_dir(msg), buf, 10);
195
          avr.puts(buf);
196
          avr.puts(")\r\n");
197
        }
198
      }*/
199
    }
200
  }
201
  return 0;
202
}
203

    
204
#endif //////////////////////////////////////////////