Statistics
| Branch: | Revision:

scoutos / scout_avr / src / main.cpp @ 64aea12e

History | View | Annotate | Download (4.76 KB)

1
#if 1 ///////////////////////////////////////////////
2

    
3
#include "ros.h"
4
#include "bom.h"
5
#include "range.h"
6
#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
#include <util/delay.h>
13

    
14
/* Period of main loop in ms */
15
#define MAINLOOP_PERIOD 100//50
16

    
17
char range_enabled = 0;
18

    
19
void debug(const char *str)
20
{
21
}
22

    
23
void callback(const std_msgs::Int16& msg)
24
{
25
}
26

    
27
/* 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
  if (range_enabled)
33
    step_enable();
34
  else
35
    step_disable();
36
  resp.ack = true;
37
}
38

    
39
void range_set_scan_cb(const sonar::sonar_set_scanRequest& req,
40
    sonar::sonar_set_scanResponse& resp)
41
{
42
  step_sweep_bounds(req.stop_l, req.stop_r);
43
  resp.ack = true;
44
}
45

    
46
int main()
47
{
48
  unsigned long now, next;
49
  unsigned int ranges[2];
50
  char i, id;
51

    
52
  ros::NodeHandle nh;
53
  nh.initNode();
54

    
55
  /* To be removed later; just an example of a subscirber */
56
  ros::Subscriber<std_msgs::Int16> test_in("test_in", callback);
57
  nh.subscribe(test_in);
58

    
59
  /* 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
  /* Range */
67
  range_init();
68
  sonar::sonar_distance range_msg;
69
  ros::Publisher range_pub("sonar_distance", &range_msg);
70
  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
  nh.advertise(range_pub);
77
  nh.advertiseService(range_toggle);
78
  nh.advertiseService(range_set_scan);
79

    
80
  /* BOM */
81
  bom_init();
82
  bom::bom bom_msg;
83
  ros::Publisher bom_pub("bom", &bom_msg);
84
  nh.advertise(bom_pub);
85

    
86
  id = 0;
87
  next = 0;
88
  while (1)
89
  {
90
    nh.spinOnce();
91

    
92
    /* Skip loop if the loop period hasn't passed yet */
93
    /* TODO if we need more exact timing, we can enter a tight loop when now
94
     * gets close to next, and avoid the uncertainty of nh.spinOnce() */
95
    now = nh.getHardware()->time();
96
    if (now < next) {
97
      continue;
98
    }
99
    next = now + MAINLOOP_PERIOD;
100

    
101
    /* Temporary, for BOM testing */
102
    id++;
103
    if (id == 0x40) {
104
      id = 0;
105
    }
106
    set_robot_id(id);
107
    bom_send(id & 1);
108

    
109
    /* BOM */
110
    for (i = 0; i < 4; i++) {
111
      int msg = bom_get(i);
112
      if (msg != BOM_NO_DATA) {
113
        bom_msg.sender = bom_msg_get_robot_id(msg);
114
        bom_msg.send_dir = bom_msg_get_dir(msg);
115
        bom_msg.recv_dir = i;
116
        bom_pub.publish(&bom_msg);
117
      }
118
    }
119

    
120
    /* Stepper / range sensor */
121
    if (range_enabled) {
122
      step_sweep();
123
      range_measure(ranges);
124
      range_msg.header.stamp = nh.now();
125
      range_msg.header.seq++;
126
      range_msg.pos = 0; // TODO fill this
127
      range_msg.distance0 = ranges[0];
128
      range_msg.distance1 = ranges[1];
129
      range_pub.publish(&range_msg);
130
    }
131

    
132
  }
133

    
134
  return 0;
135
}
136

    
137
#else ///////////////////////////////////////////////
138

    
139
extern "C"
140
{
141
#include <stdlib.h>
142
#include <string.h>
143
#include <avr/io.h>
144
#include <util/delay.h>
145
}
146

    
147
#include "Atmega128rfa1.h"
148
#include "range.h"
149
#include "bom.h"
150
#include "stepper.h"
151

    
152
Atmega128rfa1 avr;
153

    
154
void debug(const char *str)
155
{
156
  avr.puts(str);
157
}
158

    
159
int main()
160
{
161
  char buf[20];
162
  //int i;
163
  //char id = 0;
164
  /*unsigned long now, next = 0;
165
  unsigned int ranges[2];*/
166
  avr.init();
167
  range_init();
168
  bom_init();
169
  func step_sweep_cb = step_init(10);
170
  step_sweep_bounds(-26, 26);
171
  step_dir(1);
172
  step_sweep_speed(50);
173
  PORTF |= _BV(PF4);
174
  _delay_ms(500);
175
  PORTF = (PORTF & ~_BV(PF4)) | _BV(PF5);
176
  _delay_ms(500);
177
  PORTF &= ~_BV(PF5);
178
  while (1)
179
  {
180
    step_sweep_cb();
181
    _delay_ms(10);
182
    /*now = avr.time();
183
    if (now > next) {
184
      next = now + 500;*/
185
      /*ultoa(now, buf, 10);
186
      avr.puts(buf);
187
      avr.puts("\r\n");*/
188
      /*id++;
189
      if (id == 0x40) {
190
        id = 0;
191
      }
192
      set_robot_id(id);*/
193
      /*range_measure(ranges);
194
      utoa(ranges[0], buf, 10);
195
      avr.puts("Range: ");
196
      avr.puts(buf);
197
      avr.puts(", ");
198
      utoa(ranges[1], buf, 10);
199
      avr.puts(buf);
200
      avr.puts("\r\n");*/
201
      /*for (i = 0; i < 4; i++) {
202
        bom_send(i);
203
        int msg = bom_get(i);
204
        if (msg != BOM_NO_DATA) {
205
          avr.puts("BOM ");
206
          itoa(i, buf, 10);
207
          avr.puts(buf);
208
          avr.puts(": ");
209
          itoa(bom_msg_get_robot_id(msg), buf, 10);
210
          avr.puts(buf);
211
          avr.puts(" (");
212
          itoa(bom_msg_get_dir(msg), buf, 10);
213
          avr.puts(buf);
214
          avr.puts(")\r\n");
215
        }
216
      }*/
217
    //}
218
  }
219
  return 0;
220
}
221

    
222
#endif //////////////////////////////////////////////