Statistics
| Branch: | Revision:

root / scout_avr / src / main.cpp @ 0970d303

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