Statistics
| Branch: | Revision:

root / scout_avr / src / main.cpp @ daf6a575

History | View | Annotate | Download (4.91 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 066b08bb Tom Mullins
#include "orb.h"
8 6c9146d5 Tom Mullins
#include <bom/bom.h>
9
#include <sonar/sonar_distance.h>
10
#include <sonar/sonar_toggle.h>
11
#include <sonar/sonar_set_scan.h>
12 066b08bb Tom Mullins
#include <headlights/set_headlights.h>
13 47e26dee Tom Mullins
#include <util/delay.h>
14 88fb3a79 Tom Mullins
15 6c9146d5 Tom Mullins
/* Period of main loop in ms */
16 2853d46b Tom Mullins
#define MAINLOOP_PERIOD 100//50
17 6c9146d5 Tom Mullins
18
char range_enabled = 0;
19 49090532 Tom Mullins
20 812788aa Tom Mullins
void debug(const char *str)
21
{
22 31f4a032 Tom Mullins
}
23
24 066b08bb Tom Mullins
void orb_callback(const headlights::set_headlights& msg)
25 49090532 Tom Mullins
{
26 066b08bb Tom Mullins
  orb_set0(msg.left_red, msg.left_green, msg.left_blue);
27
  orb_set1(msg.right_red, msg.right_green, msg.right_blue);
28 49090532 Tom Mullins
}
29
30 6c9146d5 Tom Mullins
/* dammit, Priya, this capitalization just looks ridiculous */
31
void range_toggle_cb(const sonar::sonar_toggleRequest& req,
32
    sonar::sonar_toggleResponse& resp)
33
{
34
  range_enabled = req.set_on;
35 64aea12e Tom Mullins
  if (range_enabled)
36
    step_enable();
37
  else
38
    step_disable();
39 6c9146d5 Tom Mullins
  resp.ack = true;
40
}
41
42
void range_set_scan_cb(const sonar::sonar_set_scanRequest& req,
43
    sonar::sonar_set_scanResponse& resp)
44
{
45
  step_sweep_bounds(req.stop_l, req.stop_r);
46
  resp.ack = true;
47
}
48
49 49090532 Tom Mullins
int main()
50 88fb3a79 Tom Mullins
{
51 6c9146d5 Tom Mullins
  unsigned long now, next;
52
  unsigned int ranges[2];
53 6e7f0a98 Tom Mullins
  char i, id;
54 6c9146d5 Tom Mullins
55 88fb3a79 Tom Mullins
  ros::NodeHandle nh;
56 6c9146d5 Tom Mullins
  nh.initNode();
57 49090532 Tom Mullins
58 2853d46b Tom Mullins
  /* Stepper */
59
  // TODO ROS messages to set bounds
60
  step_init();
61
  step_dir(1);
62
  step_set_size(STEP_WHOLE);
63
  step_sweep_bounds(-26, 26);
64
65 6c9146d5 Tom Mullins
  /* Range */
66 47e26dee Tom Mullins
  range_init();
67 6c9146d5 Tom Mullins
  sonar::sonar_distance range_msg;
68 86b48573 Tom Mullins
  ros::Publisher range_pub("sonar_distance", &range_msg);
69 6c9146d5 Tom Mullins
  ros::ServiceServer
70
    <sonar::sonar_toggleRequest, sonar::sonar_toggleResponse>
71
    range_toggle("sonar_toggle", range_toggle_cb);
72
  ros::ServiceServer
73
    <sonar::sonar_set_scanRequest, sonar::sonar_set_scanResponse>
74
    range_set_scan("sonar_set_scan", range_set_scan_cb);
75 86b48573 Tom Mullins
  nh.advertise(range_pub);
76 958699af Tom Mullins
  nh.advertiseService(range_toggle);
77
  nh.advertiseService(range_set_scan);
78 6c9146d5 Tom Mullins
79
  /* BOM */
80 47e26dee Tom Mullins
  bom_init();
81 6c9146d5 Tom Mullins
  bom::bom bom_msg;
82
  ros::Publisher bom_pub("bom", &bom_msg);
83 6e7f0a98 Tom Mullins
  nh.advertise(bom_pub);
84 88fb3a79 Tom Mullins
85 066b08bb Tom Mullins
  /* Headlights (aka Orbs) */
86 daf6a575 Tom Mullins
  //orb_init();
87 066b08bb Tom Mullins
  ros::Subscriber<headlights::set_headlights> orb_sub("set_headlights",
88
      orb_callback);
89
  nh.subscribe(orb_sub);
90
91 6e7f0a98 Tom Mullins
  id = 0;
92 6c9146d5 Tom Mullins
  next = 0;
93 88fb3a79 Tom Mullins
  while (1)
94
  {
95 6c9146d5 Tom Mullins
    nh.spinOnce();
96
97
    /* Skip loop if the loop period hasn't passed yet */
98
    /* TODO if we need more exact timing, we can enter a tight loop when now
99
     * gets close to next, and avoid the uncertainty of nh.spinOnce() */
100
    now = nh.getHardware()->time();
101
    if (now < next) {
102
      continue;
103
    }
104
    next = now + MAINLOOP_PERIOD;
105
106
    /* Temporary, for BOM testing */
107 6e7f0a98 Tom Mullins
    id++;
108
    if (id == 0x40) {
109
      id = 0;
110
    }
111
    set_robot_id(id);
112 066b08bb Tom Mullins
    bom_send(0);
113 6c9146d5 Tom Mullins
114
    /* BOM */
115 6e7f0a98 Tom Mullins
    for (i = 0; i < 4; i++) {
116
      int msg = bom_get(i);
117
      if (msg != BOM_NO_DATA) {
118
        bom_msg.sender = bom_msg_get_robot_id(msg);
119
        bom_msg.send_dir = bom_msg_get_dir(msg);
120
        bom_msg.recv_dir = i;
121
        bom_pub.publish(&bom_msg);
122
      }
123
    }
124 47e26dee Tom Mullins
125 6c9146d5 Tom Mullins
    /* Stepper / range sensor */
126 2853d46b Tom Mullins
    if (range_enabled) {
127
      step_sweep();
128
      range_measure(ranges);
129
      range_msg.header.stamp = nh.now();
130
      range_msg.header.seq++;
131 daf6a575 Tom Mullins
      range_msg.pos = step_get_pos();
132 2853d46b Tom Mullins
      range_msg.distance0 = ranges[0];
133
      range_msg.distance1 = ranges[1];
134
      range_pub.publish(&range_msg);
135
    }
136 6c9146d5 Tom Mullins
137 88fb3a79 Tom Mullins
  }
138
139
  return 0;
140 49090532 Tom Mullins
}
141 88fb3a79 Tom Mullins
142 807483bf Tom Mullins
#else ///////////////////////////////////////////////
143
144
extern "C"
145 88fb3a79 Tom Mullins
{
146
#include <stdlib.h>
147
#include <string.h>
148 fd73d758 Tom Mullins
#include <avr/io.h>
149
#include <util/delay.h>
150 88fb3a79 Tom Mullins
}
151
152 49090532 Tom Mullins
#include "Atmega128rfa1.h"
153 1c3c96ce Tom Mullins
#include "range.h"
154 f115416e Tom Mullins
#include "bom.h"
155 6c9146d5 Tom Mullins
#include "stepper.h"
156 49090532 Tom Mullins
157 31f4a032 Tom Mullins
Atmega128rfa1 avr;
158
159 812788aa Tom Mullins
void debug(const char *str)
160
{
161 31f4a032 Tom Mullins
  avr.puts(str);
162
}
163
164 88fb3a79 Tom Mullins
int main()
165
{
166 1c3c96ce Tom Mullins
  char buf[20];
167 86b48573 Tom Mullins
  //int i;
168
  //char id = 0;
169 2853d46b Tom Mullins
  /*unsigned long now, next = 0;
170
  unsigned int ranges[2];*/
171 88fb3a79 Tom Mullins
  avr.init();
172 1c3c96ce Tom Mullins
  range_init();
173 f115416e Tom Mullins
  bom_init();
174 2853d46b Tom Mullins
  func step_sweep_cb = step_init(10);
175 6c9146d5 Tom Mullins
  step_sweep_bounds(-26, 26);
176
  step_dir(1);
177 2853d46b Tom Mullins
  step_sweep_speed(50);
178
  PORTF |= _BV(PF4);
179
  _delay_ms(500);
180
  PORTF = (PORTF & ~_BV(PF4)) | _BV(PF5);
181
  _delay_ms(500);
182
  PORTF &= ~_BV(PF5);
183 88fb3a79 Tom Mullins
  while (1)
184
  {
185 2853d46b Tom Mullins
    step_sweep_cb();
186
    _delay_ms(10);
187
    /*now = avr.time();
188 49090532 Tom Mullins
    if (now > next) {
189 2853d46b Tom Mullins
      next = now + 500;*/
190 fd73d758 Tom Mullins
      /*ultoa(now, buf, 10);
191 812788aa Tom Mullins
      avr.puts(buf);
192 fd73d758 Tom Mullins
      avr.puts("\r\n");*/
193 6c9146d5 Tom Mullins
      /*id++;
194 31f4a032 Tom Mullins
      if (id == 0x40) {
195
        id = 0;
196
      }
197 6c9146d5 Tom Mullins
      set_robot_id(id);*/
198 2853d46b Tom Mullins
      /*range_measure(ranges);
199 86b48573 Tom Mullins
      utoa(ranges[0], buf, 10);
200 f115416e Tom Mullins
      avr.puts("Range: ");
201
      avr.puts(buf);
202
      avr.puts(", ");
203 86b48573 Tom Mullins
      utoa(ranges[1], buf, 10);
204
      avr.puts(buf);
205 2853d46b Tom Mullins
      avr.puts("\r\n");*/
206 6c9146d5 Tom Mullins
      /*for (i = 0; i < 4; i++) {
207 f115416e Tom Mullins
        bom_send(i);
208
        int msg = bom_get(i);
209
        if (msg != BOM_NO_DATA) {
210
          avr.puts("BOM ");
211
          itoa(i, buf, 10);
212
          avr.puts(buf);
213
          avr.puts(": ");
214 31f4a032 Tom Mullins
          itoa(bom_msg_get_robot_id(msg), buf, 10);
215
          avr.puts(buf);
216
          avr.puts(" (");
217
          itoa(bom_msg_get_dir(msg), buf, 10);
218 f115416e Tom Mullins
          avr.puts(buf);
219 31f4a032 Tom Mullins
          avr.puts(")\r\n");
220 f115416e Tom Mullins
        }
221 6c9146d5 Tom Mullins
      }*/
222 2853d46b Tom Mullins
    //}
223 88fb3a79 Tom Mullins
  }
224
  return 0;
225 cf115e3d Tom Mullins
}
226 807483bf Tom Mullins
227
#endif //////////////////////////////////////////////