Statistics
| Branch: | Revision:

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

History | View | Annotate | Download (4.37 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 6c9146d5 Tom Mullins
#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 47e26dee Tom Mullins
#include <util/delay.h>
13 88fb3a79 Tom Mullins
14 6c9146d5 Tom Mullins
/* Period of main loop in ms */
15 86b48573 Tom Mullins
#define MAINLOOP_PERIOD 1000//50
16 6c9146d5 Tom Mullins
17
char range_enabled = 0;
18 49090532 Tom Mullins
19 812788aa Tom Mullins
void debug(const char *str)
20
{
21 31f4a032 Tom Mullins
}
22
23 cf115e3d Tom Mullins
void callback(const std_msgs::Int16& msg)
24 49090532 Tom Mullins
{
25
}
26
27 6c9146d5 Tom Mullins
/* 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 49090532 Tom Mullins
int main()
43 88fb3a79 Tom Mullins
{
44 6c9146d5 Tom Mullins
  unsigned long now, next;
45
  unsigned int ranges[2];
46 6e7f0a98 Tom Mullins
  char i, id;
47 6c9146d5 Tom Mullins
48 88fb3a79 Tom Mullins
  ros::NodeHandle nh;
49 6c9146d5 Tom Mullins
  nh.initNode();
50 49090532 Tom Mullins
51 6c9146d5 Tom Mullins
  /* 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 88fb3a79 Tom Mullins
55 6c9146d5 Tom Mullins
  /* Range */
56 47e26dee Tom Mullins
  range_init();
57 6c9146d5 Tom Mullins
  sonar::sonar_distance range_msg;
58 86b48573 Tom Mullins
  ros::Publisher range_pub("sonar_distance", &range_msg);
59 6c9146d5 Tom Mullins
  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 86b48573 Tom Mullins
  nh.advertise(range_pub);
66 6c9146d5 Tom Mullins
67
  /* BOM */
68 47e26dee Tom Mullins
  bom_init();
69 6c9146d5 Tom Mullins
  bom::bom bom_msg;
70
  ros::Publisher bom_pub("bom", &bom_msg);
71 6e7f0a98 Tom Mullins
  nh.advertise(bom_pub);
72 88fb3a79 Tom Mullins
73 6e7f0a98 Tom Mullins
  id = 0;
74 6c9146d5 Tom Mullins
  next = 0;
75 88fb3a79 Tom Mullins
  while (1)
76
  {
77 6c9146d5 Tom Mullins
    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 6e7f0a98 Tom Mullins
    id++;
90
    if (id == 0x40) {
91
      id = 0;
92
    }
93
    set_robot_id(id);
94 47e26dee Tom Mullins
    bom_send(id & 1);
95 6c9146d5 Tom Mullins
96
    /* BOM */
97 6e7f0a98 Tom Mullins
    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 47e26dee Tom Mullins
107 6c9146d5 Tom Mullins
    /* 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 86b48573 Tom Mullins
    range_pub.publish(&range_msg);
116 6c9146d5 Tom Mullins
117 88fb3a79 Tom Mullins
  }
118
119
  return 0;
120 49090532 Tom Mullins
}
121 88fb3a79 Tom Mullins
122 807483bf Tom Mullins
#else ///////////////////////////////////////////////
123
124
extern "C"
125 88fb3a79 Tom Mullins
{
126
#include <stdlib.h>
127
#include <string.h>
128 fd73d758 Tom Mullins
#include <avr/io.h>
129
#include <util/delay.h>
130 88fb3a79 Tom Mullins
}
131
132 49090532 Tom Mullins
#include "Atmega128rfa1.h"
133 1c3c96ce Tom Mullins
#include "range.h"
134 f115416e Tom Mullins
#include "bom.h"
135 6c9146d5 Tom Mullins
#include "stepper.h"
136 49090532 Tom Mullins
137 31f4a032 Tom Mullins
Atmega128rfa1 avr;
138
139 812788aa Tom Mullins
void debug(const char *str)
140
{
141 31f4a032 Tom Mullins
  avr.puts(str);
142
}
143
144 88fb3a79 Tom Mullins
int main()
145
{
146 1c3c96ce Tom Mullins
  char buf[20];
147 86b48573 Tom Mullins
  //int i;
148
  //char id = 0;
149 cf115e3d Tom Mullins
  unsigned long now, next = 0;
150 86b48573 Tom Mullins
  unsigned int ranges[2];
151 88fb3a79 Tom Mullins
  avr.init();
152 1c3c96ce Tom Mullins
  range_init();
153 f115416e Tom Mullins
  bom_init();
154 86b48573 Tom Mullins
  /*func step_sweep_cb = step_init(10);
155 6c9146d5 Tom Mullins
  step_sweep_bounds(-26, 26);
156
  step_dir(1);
157 86b48573 Tom Mullins
  step_sweep_speed(50);*/
158 31f4a032 Tom Mullins
  avr.puts("Hello!\r\n");
159 88fb3a79 Tom Mullins
  while (1)
160
  {
161 86b48573 Tom Mullins
    /*step_sweep_cb();
162
    _delay_ms(10);*/
163
    now = avr.time();
164 49090532 Tom Mullins
    if (now > next) {
165 86b48573 Tom Mullins
      next = now + 500;
166 6c9146d5 Tom Mullins
      //step_sweep_cb();
167 fd73d758 Tom Mullins
      /*ultoa(now, buf, 10);
168 812788aa Tom Mullins
      avr.puts(buf);
169 fd73d758 Tom Mullins
      avr.puts("\r\n");*/
170 6c9146d5 Tom Mullins
      /*id++;
171 31f4a032 Tom Mullins
      if (id == 0x40) {
172
        id = 0;
173
      }
174 6c9146d5 Tom Mullins
      set_robot_id(id);*/
175 86b48573 Tom Mullins
      range_measure(ranges);
176
      utoa(ranges[0], buf, 10);
177 f115416e Tom Mullins
      avr.puts("Range: ");
178
      avr.puts(buf);
179
      avr.puts(", ");
180 86b48573 Tom Mullins
      utoa(ranges[1], buf, 10);
181
      avr.puts(buf);
182
      avr.puts("\r\n");
183 6c9146d5 Tom Mullins
      /*for (i = 0; i < 4; i++) {
184 f115416e Tom Mullins
        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 31f4a032 Tom Mullins
          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 f115416e Tom Mullins
          avr.puts(buf);
196 31f4a032 Tom Mullins
          avr.puts(")\r\n");
197 f115416e Tom Mullins
        }
198 6c9146d5 Tom Mullins
      }*/
199 86b48573 Tom Mullins
    }
200 88fb3a79 Tom Mullins
  }
201
  return 0;
202 cf115e3d Tom Mullins
}
203 807483bf Tom Mullins
204
#endif //////////////////////////////////////////////