Statistics
| Branch: | Revision:

root / scout_avr / src / main.cpp @ 8741d18c

History | View | Annotate | Download (5.44 KB)

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

    
3
#include "ros.h"
4
#include "bom.h"
5
#include "range.h"
6
#include "stepper.h"
7
#include "orb.h"
8
#include "cliffSensor.h"
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
#include <headlights/set_headlights.h>
14
#include <cliffsensor/cliff_status_changed.h>
15
#include <util/delay.h>
16

    
17
/* Period of main loop in ms */
18
#define MAINLOOP_PERIOD 100//50
19

    
20
char range_enabled = 0;
21

    
22
void debug(const char *str)
23
{
24
}
25

    
26
void orb_callback(const headlights::set_headlights& msg)
27
{
28
  orb_set0(msg.left_red, msg.left_green, msg.left_blue);
29
  orb_set1(msg.right_red, msg.right_green, msg.right_blue);
30
}
31

    
32
/* dammit, Priya, this capitalization just looks ridiculous */
33
void range_toggle_cb(const sonar::sonar_toggleRequest& req,
34
    sonar::sonar_toggleResponse& resp)
35
{
36
  range_enabled = req.set_on;
37
  if (range_enabled)
38
    step_enable();
39
  else
40
    step_disable();
41
  resp.ack = true;
42
}
43

    
44
void range_set_scan_cb(const sonar::sonar_set_scanRequest& req,
45
    sonar::sonar_set_scanResponse& resp)
46
{
47
  step_sweep_bounds(req.stop_l, req.stop_r);
48
  resp.ack = true;
49
}
50

    
51
int main()
52
{
53
  unsigned long now, next;
54
  unsigned int ranges[2];
55
  char i, id;
56

    
57
  ros::NodeHandle nh;
58
  nh.initNode();
59

    
60
  /* Stepper */
61
  // TODO ROS messages to set bounds
62
  step_init();
63
  step_dir(1);
64
  step_set_size(STEP_WHOLE);
65
  step_sweep_bounds(-26, 26);
66

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

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

    
87
  /* Headlights (aka Orbs) */
88
  //orb_init();
89
  ros::Subscriber<headlights::set_headlights> orb_sub("set_headlights",
90
      orb_callback);
91
  nh.subscribe(orb_sub);
92

    
93
  /* Cliff sensors */
94
  cliffSensor_init();
95
  cliffsensor::cliff_status_changed cliff_msg;
96
  ros::Publisher cliff_pub("cliff", &cliff_msg);
97
  nh.advertise(cliff_pub);
98

    
99
  id = 0;
100
  next = 0;
101
  while (1)
102
  {
103
    nh.spinOnce();
104

    
105
    /* Skip loop if the loop period hasn't passed yet */
106
    /* TODO if we need more exact timing, we can enter a tight loop when now
107
     * gets close to next, and avoid the uncertainty of nh.spinOnce() */
108
    now = nh.getHardware()->time();
109
    if (now < next) {
110
      continue;
111
    }
112
    next = now + MAINLOOP_PERIOD;
113

    
114
    /* Temporary, for BOM testing */
115
    id++;
116
    if (id == 0x40) {
117
      id = 0;
118
    }
119
    set_robot_id(id);
120
    bom_send(0);
121

    
122
    /* BOM */
123
    for (i = 0; i < 4; i++) {
124
      int msg = bom_get(i);
125
      if (msg != BOM_NO_DATA) {
126
        bom_msg.sender = bom_msg_get_robot_id(msg);
127
        bom_msg.send_dir = bom_msg_get_dir(msg);
128
        bom_msg.recv_dir = i;
129
        bom_pub.publish(&bom_msg);
130
      }
131
    }
132

    
133
    /* Stepper / range sensor */
134
    if (range_enabled) {
135
      step_sweep();
136
      range_measure(ranges);
137
      range_msg.stamp = nh.now();
138
      range_msg.pos = step_get_pos();
139
      range_msg.distance0 = ranges[0];
140
      range_msg.distance1 = ranges[1];
141
      range_msg.stamp = nh.now();
142
      range_pub.publish(&range_msg);
143
    }
144

    
145
    /* TODO remove raw values and have single bitmask */
146
    cliff_msg.front_raw = read_cliffSensor_front();
147
    cliff_msg.left_raw = read_cliffSensor_left();
148
    cliff_msg.right_raw = read_cliffSensor_right();
149
    cliff_msg.cliff_status = cliff_msg.front_raw | cliff_msg.left_raw
150
      | cliff_msg.right_raw;
151

    
152
  }
153

    
154
  return 0;
155
}
156

    
157
#else ///////////////////////////////////////////////
158

    
159
extern "C"
160
{
161
#include <stdlib.h>
162
#include <string.h>
163
#include <avr/io.h>
164
#include <util/delay.h>
165
}
166

    
167
#include "Atmega128rfa1.h"
168
#include "range.h"
169
#include "bom.h"
170
#include "stepper.h"
171

    
172
Atmega128rfa1 avr;
173

    
174
void debug(const char *str)
175
{
176
  avr.puts(str);
177
}
178

    
179
int main()
180
{
181
  char buf[20];
182
  //int i;
183
  //char id = 0;
184
  /*unsigned long now, next = 0;
185
  unsigned int ranges[2];*/
186
  avr.init();
187
  range_init();
188
  bom_init();
189
  func step_sweep_cb = step_init(10);
190
  step_sweep_bounds(-26, 26);
191
  step_dir(1);
192
  step_sweep_speed(50);
193
  PORTF |= _BV(PF4);
194
  _delay_ms(500);
195
  PORTF = (PORTF & ~_BV(PF4)) | _BV(PF5);
196
  _delay_ms(500);
197
  PORTF &= ~_BV(PF5);
198
  while (1)
199
  {
200
    step_sweep_cb();
201
    _delay_ms(10);
202
    /*now = avr.time();
203
    if (now > next) {
204
      next = now + 500;*/
205
      /*ultoa(now, buf, 10);
206
      avr.puts(buf);
207
      avr.puts("\r\n");*/
208
      /*id++;
209
      if (id == 0x40) {
210
        id = 0;
211
      }
212
      set_robot_id(id);*/
213
      /*range_measure(ranges);
214
      utoa(ranges[0], buf, 10);
215
      avr.puts("Range: ");
216
      avr.puts(buf);
217
      avr.puts(", ");
218
      utoa(ranges[1], buf, 10);
219
      avr.puts(buf);
220
      avr.puts("\r\n");*/
221
      /*for (i = 0; i < 4; i++) {
222
        bom_send(i);
223
        int msg = bom_get(i);
224
        if (msg != BOM_NO_DATA) {
225
          avr.puts("BOM ");
226
          itoa(i, buf, 10);
227
          avr.puts(buf);
228
          avr.puts(": ");
229
          itoa(bom_msg_get_robot_id(msg), buf, 10);
230
          avr.puts(buf);
231
          avr.puts(" (");
232
          itoa(bom_msg_get_dir(msg), buf, 10);
233
          avr.puts(buf);
234
          avr.puts(")\r\n");
235
        }
236
      }*/
237
    //}
238
  }
239
  return 0;
240
}
241

    
242
#endif //////////////////////////////////////////////