Statistics
| Branch: | Revision:

root / scout_avr / src / main.cpp @ 6c9146d5

History | View | Annotate | Download (4.24 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
#define MAINLOOP_PERIOD 50
16
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
  ros::Publisher range_distance("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
66
  /* BOM */
67 47e26dee Tom Mullins
  bom_init();
68 6c9146d5 Tom Mullins
  bom::bom bom_msg;
69
  ros::Publisher bom_pub("bom", &bom_msg);
70 6e7f0a98 Tom Mullins
  nh.advertise(bom_pub);
71 88fb3a79 Tom Mullins
72 6e7f0a98 Tom Mullins
  id = 0;
73 6c9146d5 Tom Mullins
  next = 0;
74 88fb3a79 Tom Mullins
  while (1)
75
  {
76 6c9146d5 Tom Mullins
    nh.spinOnce();
77
78
    /* Skip loop if the loop period hasn't passed yet */
79
    /* TODO if we need more exact timing, we can enter a tight loop when now
80
     * gets close to next, and avoid the uncertainty of nh.spinOnce() */
81
    now = nh.getHardware()->time();
82
    if (now < next) {
83
      continue;
84
    }
85
    next = now + MAINLOOP_PERIOD;
86
87
    /* Temporary, for BOM testing */
88 6e7f0a98 Tom Mullins
    id++;
89
    if (id == 0x40) {
90
      id = 0;
91
    }
92
    set_robot_id(id);
93 47e26dee Tom Mullins
    bom_send(id & 1);
94 6c9146d5 Tom Mullins
95
    /* BOM */
96 6e7f0a98 Tom Mullins
    for (i = 0; i < 4; i++) {
97
      int msg = bom_get(i);
98
      if (msg != BOM_NO_DATA) {
99
        bom_msg.sender = bom_msg_get_robot_id(msg);
100
        bom_msg.send_dir = bom_msg_get_dir(msg);
101
        bom_msg.recv_dir = i;
102
        bom_pub.publish(&bom_msg);
103
      }
104
    }
105 47e26dee Tom Mullins
106 6c9146d5 Tom Mullins
    /* Stepper / range sensor */
107
    // TODO sweep stepper
108
    range_measure(ranges);
109
    range_msg.header.stamp = nh.now();
110
    range_msg.header.seq++;
111
    range_msg.pos = 0; // TODO fill this
112
    range_msg.distance0 = ranges[0];
113
    range_msg.distance1 = ranges[1];
114
115 88fb3a79 Tom Mullins
  }
116
117
  return 0;
118 49090532 Tom Mullins
}
119 88fb3a79 Tom Mullins
120 807483bf Tom Mullins
#else ///////////////////////////////////////////////
121
122
extern "C"
123 88fb3a79 Tom Mullins
{
124
#include <stdlib.h>
125
#include <string.h>
126 fd73d758 Tom Mullins
#include <avr/io.h>
127
#include <util/delay.h>
128 88fb3a79 Tom Mullins
}
129
130 49090532 Tom Mullins
#include "Atmega128rfa1.h"
131 1c3c96ce Tom Mullins
#include "range.h"
132 f115416e Tom Mullins
#include "bom.h"
133 6c9146d5 Tom Mullins
#include "stepper.h"
134 49090532 Tom Mullins
135 31f4a032 Tom Mullins
Atmega128rfa1 avr;
136
137 812788aa Tom Mullins
void debug(const char *str)
138
{
139 31f4a032 Tom Mullins
  avr.puts(str);
140
}
141
142 88fb3a79 Tom Mullins
int main()
143
{
144 1c3c96ce Tom Mullins
  char buf[20];
145 f115416e Tom Mullins
  int i;
146 31f4a032 Tom Mullins
  char id = 0;
147 cf115e3d Tom Mullins
  unsigned long now, next = 0;
148 88fb3a79 Tom Mullins
  avr.init();
149 1c3c96ce Tom Mullins
  range_init();
150 f115416e Tom Mullins
  bom_init();
151 6c9146d5 Tom Mullins
  func step_sweep_cb = step_init(10);
152
  step_sweep_bounds(-26, 26);
153
  step_dir(1);
154
  step_sweep_speed(50);
155 31f4a032 Tom Mullins
  avr.puts("Hello!\r\n");
156 88fb3a79 Tom Mullins
  while (1)
157
  {
158 6c9146d5 Tom Mullins
    step_sweep_cb();
159
    _delay_ms(10);
160
    /*now = avr.time();
161 49090532 Tom Mullins
    if (now > next) {
162 6c9146d5 Tom Mullins
      next = now + 50;*/
163
      //step_sweep_cb();
164 fd73d758 Tom Mullins
      /*ultoa(now, buf, 10);
165 812788aa Tom Mullins
      avr.puts(buf);
166 fd73d758 Tom Mullins
      avr.puts("\r\n");*/
167 6c9146d5 Tom Mullins
      /*id++;
168 31f4a032 Tom Mullins
      if (id == 0x40) {
169
        id = 0;
170
      }
171 6c9146d5 Tom Mullins
      set_robot_id(id);*/
172 f115416e Tom Mullins
      /*utoa(range_get(0), buf, 10);
173
      avr.puts("Range: ");
174
      avr.puts(buf);
175
      avr.puts(", ");
176 807483bf Tom Mullins
      utoa(range_get(1), buf, 10);
177 f115416e Tom Mullins
      avr.puts(buf);*/
178 6c9146d5 Tom Mullins
      /*for (i = 0; i < 4; i++) {
179 f115416e Tom Mullins
        bom_send(i);
180
        int msg = bom_get(i);
181
        if (msg != BOM_NO_DATA) {
182
          avr.puts("BOM ");
183
          itoa(i, buf, 10);
184
          avr.puts(buf);
185
          avr.puts(": ");
186 31f4a032 Tom Mullins
          itoa(bom_msg_get_robot_id(msg), buf, 10);
187
          avr.puts(buf);
188
          avr.puts(" (");
189
          itoa(bom_msg_get_dir(msg), buf, 10);
190 f115416e Tom Mullins
          avr.puts(buf);
191 31f4a032 Tom Mullins
          avr.puts(")\r\n");
192 f115416e Tom Mullins
        }
193 6c9146d5 Tom Mullins
      }*/
194
    //}
195 88fb3a79 Tom Mullins
  }
196
  return 0;
197 cf115e3d Tom Mullins
}
198 807483bf Tom Mullins
199
#endif //////////////////////////////////////////////