Statistics
| Branch: | Revision:

root / scout_avr / src / main.cpp @ 066b08bb

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