Statistics
| Branch: | Revision:

root / scout_avr / src / main.cpp @ 958699af

History | View | Annotate | Download (4.7 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
  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
int main()
43
{
44
  unsigned long now, next;
45
  unsigned int ranges[2];
46
  char i, id;
47

    
48
  ros::NodeHandle nh;
49
  nh.initNode();
50

    
51
  /* 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

    
55
  /* Stepper */
56
  // TODO ROS messages to set bounds
57
  step_init();
58
  step_dir(1);
59
  step_set_size(STEP_WHOLE);
60
  step_sweep_bounds(-26, 26);
61

    
62
  /* Range */
63
  range_init();
64
  sonar::sonar_distance range_msg;
65
  ros::Publisher range_pub("sonar_distance", &range_msg);
66
  ros::ServiceServer
67
    <sonar::sonar_toggleRequest, sonar::sonar_toggleResponse>
68
    range_toggle("sonar_toggle", range_toggle_cb);
69
  ros::ServiceServer
70
    <sonar::sonar_set_scanRequest, sonar::sonar_set_scanResponse>
71
    range_set_scan("sonar_set_scan", range_set_scan_cb);
72
  nh.advertise(range_pub);
73
  nh.advertiseService(range_toggle);
74
  nh.advertiseService(range_set_scan);
75

    
76
  /* BOM */
77
  bom_init();
78
  bom::bom bom_msg;
79
  ros::Publisher bom_pub("bom", &bom_msg);
80
  nh.advertise(bom_pub);
81

    
82
  id = 0;
83
  next = 0;
84
  while (1)
85
  {
86
    nh.spinOnce();
87

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

    
97
    /* Temporary, for BOM testing */
98
    id++;
99
    if (id == 0x40) {
100
      id = 0;
101
    }
102
    set_robot_id(id);
103
    bom_send(id & 1);
104

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

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

    
128
  }
129

    
130
  return 0;
131
}
132

    
133
#else ///////////////////////////////////////////////
134

    
135
extern "C"
136
{
137
#include <stdlib.h>
138
#include <string.h>
139
#include <avr/io.h>
140
#include <util/delay.h>
141
}
142

    
143
#include "Atmega128rfa1.h"
144
#include "range.h"
145
#include "bom.h"
146
#include "stepper.h"
147

    
148
Atmega128rfa1 avr;
149

    
150
void debug(const char *str)
151
{
152
  avr.puts(str);
153
}
154

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

    
218
#endif //////////////////////////////////////////////