Revision 86b48573

View differences:

scout_avr/src/main.cpp
12 12
#include <util/delay.h>
13 13

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

  
17 17
char range_enabled = 0;
18 18

  
......
55 55
  /* Range */
56 56
  range_init();
57 57
  sonar::sonar_distance range_msg;
58
  ros::Publisher range_distance("sonar_distance", &range_msg);
58
  ros::Publisher range_pub("sonar_distance", &range_msg);
59 59
  ros::ServiceServer
60 60
    <sonar::sonar_toggleRequest, sonar::sonar_toggleResponse>
61 61
    range_toggle("sonar_toggle", range_toggle_cb);
62 62
  ros::ServiceServer
63 63
    <sonar::sonar_set_scanRequest, sonar::sonar_set_scanResponse>
64 64
    range_set_scan("sonar_set_scan", range_set_scan_cb);
65
  nh.advertise(range_pub);
65 66

  
66 67
  /* BOM */
67 68
  bom_init();
......
111 112
    range_msg.pos = 0; // TODO fill this
112 113
    range_msg.distance0 = ranges[0];
113 114
    range_msg.distance1 = ranges[1];
115
    range_pub.publish(&range_msg);
114 116

  
115 117
  }
116 118

  
......
142 144
int main()
143 145
{
144 146
  char buf[20];
145
  int i;
146
  char id = 0;
147
  //int i;
148
  //char id = 0;
147 149
  unsigned long now, next = 0;
150
  unsigned int ranges[2];
148 151
  avr.init();
149 152
  range_init();
150 153
  bom_init();
151
  func step_sweep_cb = step_init(10);
154
  /*func step_sweep_cb = step_init(10);
152 155
  step_sweep_bounds(-26, 26);
153 156
  step_dir(1);
154
  step_sweep_speed(50);
157
  step_sweep_speed(50);*/
155 158
  avr.puts("Hello!\r\n");
156 159
  while (1)
157 160
  {
158
    step_sweep_cb();
159
    _delay_ms(10);
160
    /*now = avr.time();
161
    /*step_sweep_cb();
162
    _delay_ms(10);*/
163
    now = avr.time();
161 164
    if (now > next) {
162
      next = now + 50;*/
165
      next = now + 500;
163 166
      //step_sweep_cb();
164 167
      /*ultoa(now, buf, 10);
165 168
      avr.puts(buf);
......
169 172
        id = 0;
170 173
      }
171 174
      set_robot_id(id);*/
172
      /*utoa(range_get(0), buf, 10);
175
      range_measure(ranges);
176
      utoa(ranges[0], buf, 10);
173 177
      avr.puts("Range: ");
174 178
      avr.puts(buf);
175 179
      avr.puts(", ");
176
      utoa(range_get(1), buf, 10);
177
      avr.puts(buf);*/
180
      utoa(ranges[1], buf, 10);
181
      avr.puts(buf);
182
      avr.puts("\r\n");
178 183
      /*for (i = 0; i < 4; i++) {
179 184
        bom_send(i);
180 185
        int msg = bom_get(i);
......
191 196
          avr.puts(")\r\n");
192 197
        }
193 198
      }*/
194
    //}
199
    }
195 200
  }
196 201
  return 0;
197 202
}
scout_avr/src/range.cpp
14 14
 * 37.5ms * 8 MHz / 8 prescaler = 37500 max wait
15 15
 * 37.5ms * 16 MHz / 8 prescaler = problem
16 16
 * 37.5ms * 16 MHz / 64 prescaler = 9375 max wait
17
 * 147us/in * 16 MHz / 64 prescaler = 1.44685039 ticks / mm
17 18
 */
18 19

  
19 20
struct range_t {
20 21
  unsigned int start; // timer value on rising edge
21 22
  unsigned int value; // last measured range
22
  char started;
23
  char done;
23
  char busy;
24 24
} volatile range[2];
25 25

  
26 26
static void on_edge(int which)
......
40 40
  if (int_high)
41 41
  {
42 42
    range[which].start = time;
43
    range[which].started = 1;
43
    range[which].busy = 1;
44 44
  }
45 45
  else
46 46
  {
47 47
    // if timer overflowed since start, this arithmetic should still work out
48 48
    range[which].value = time - range[which].start;
49
    range[which].done = 1;
49
    range[which].busy = 0;
50 50
  }
51 51
}
52 52

  
......
83 83
  for (i = 0; i < 2; i++)
84 84
  {
85 85
    range[i].value = RANGE_ERR;
86
    range[i].started = 0;
87
    range[i].done = 0;
86
    range[i].busy = 0;
88 87
  }
89 88

  
90 89
  // TODO ensure that one interrupt won't be delayed because of the other
91 90
  PORT_SONAR_TX |= _BV(P_SONAR_TX);
92
  _delay_ms(1);
91
  _delay_ms(40);
93 92
  PORT_SONAR_TX &= ~ _BV(P_SONAR_TX);
94 93

  
95 94
  for (i = 0; i < 2; i++)
96 95
  {
97
    if (range[i].started) {
98
      while (!range[i].done) {}
99
      values[i] = range[i].value;
100
    }
96
    while (range[i].busy) {}
97
    values[i] = range[i].value;
101 98
  }
102 99
}

Also available in: Unified diff