Statistics
| Revision:

root / demos / hunter_prey / lib / src / libdragonfly / rangefinder.c @ 1828

History | View | Annotate | Download (10.9 KB)

1
/**
2
 * Copyright (c) 2007 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 **/
25

    
26

    
27
/**
28
 * @file rangefinder.c
29
 * @brief Rangefinders
30
 *
31
 * Implementation of functions for rangefinder use.
32
 *
33
 * @author Colony Project, CMU Robotics Club
34
 **/
35

    
36
/*
37
  Authors: James Kong, Greg Tress, Emily Hart
38

39
  Last Modified: 5/12/10 by Emily
40
  - Added Butterworth filtering functions. As of right now, the filter should
41
    only be used if you really know what you are doing. These functions were
42
    designed to be used with a task scheduler, which is not currently complete.
43
    This implementation uses the RTC and runs massive computations in the RTC
44
    interrupt.
45

46
  Modified: 4/30/06 by James
47
  -Started log_distance conversion function !!!NOT COMPLETE!!!
48
  -Cleaning up comments
49

50
  -----------------
51
  rangefinder.c
52
  Using Sharp GP2D02 IR Rangefinder
53

54
  Vin is the input to the rangefinder, designated RANGE_CTRL.
55
  Vout is the output from the rangefinder, designated RANGE_IN# where # is the
56
  rangefinder you are reading from
57

58
  Expected Initial Conditions:
59
  Vin is high and Vout should read high.
60

61
  Usage:
62
  1.) Set Vin low. Vout should read low.
63
  2.) Wait for high on Vout.
64
  3.) Begin clocking Vin and reading 8 bits from Vout (MSB first).
65
  4.) Set Vin high for 2ms or more to turn off rangefinder
66

67
*/
68
#include <avr/pgmspace.h>
69
#include <avr/interrupt.h>
70

    
71
#include "rangefinder.h"
72
#include "analog.h"
73
#include "dio.h"
74
#include "time.h"
75

    
76
/*
77
  read_distance returns the 8-bit reading from the rangefinder
78
  parameters:
79
  range_id - dio pin set as the rangefinder Vout [i.e. RANGE_IN0]
80

81
  NOTE:
82
  The Sharp GD2D02 returns values on a decreasing logrithmic scale.
83
  So higher values correspond to closer distances.  Use linearize_distance to convert to normal centimeter scale.
84
  Also, when reading distances closer than 8cm, the Sharp GD2D02 will return lower values than the values at 8cm.
85
  At this point, we are only reading from one rangefinder [RANGE_IN0].
86
*/
87

    
88
// constants
89
/* Nasty IR approximation table
90
   I'm using this for the heck of it.  We can do whatever.
91

92
   Note the minimum value is .4V (20), and the maximum is 2.6V (133).
93
   Gives distance in mm.
94

95
   excel formula(valid for inputs 20-133):  ROUND(2353.6*(E2^(-1.1146))*10,0)
96

97
   This is only valid for the GP2D12, with objects directly ahead and more than
98
   10cm from the detector.  See the datasheet for more information.
99
*/
100

    
101
static int IR_dist_conversion[72] PROGMEM = {
102
        327,315,303,291,281,271,262,253,245,238,231,224,218,212,206,200,
103
        195,190,185,181,177,173,168,165,161,158,155,151,148,145,143,140,
104
        137,134,132,130,127,125,123,121,119,117,115,114,111,110,108,106,
105
        105,104,102,100,99,98,97,95,94,93,91,90,89,88,87,86,84,83,83,82,
106
        81,80,79,78
107
};
108

    
109
/* 1 if the filter is enabled, else 0 */
110
static int use_filter;
111
// VALUE - MIN_IR_LINEAR is stored so only 8 bits are needed
112
/* X values for the Butterworth filter, for each rangefinder */
113
static uint8_t butter_x[5][4];
114
/* Y values for the Butterworth filter, for each rangefinder */
115
static uint8_t butter_y[5][3];
116
/* How many consecutive -1s have been seen by each rangefinder */
117
static uint8_t neg_one_count[5];
118

    
119
/**
120
 * @defgroup rangefinder Rangefinder
121
 * @brief Functions for using the IR rangefinders
122
 * 
123
 * Functions for using the IR rangefinders.
124
 *
125
 * @{
126
 **/
127

    
128
/**
129
 * Initializes the rangefinders. This must be called before
130
 * range_read_distance. This function does not initialize the filter.
131
 *
132
 * @see range_read_distance
133
 **/
134
void range_init(void)
135
{
136
  digital_output(_PIN_B4,0);
137
  use_filter = 0;
138
}
139

    
140
/**
141
 * Initializes the rangefinders with an option to enable the Butterworth
142
 * filtering.
143
 *
144
 * As of 5/12/2010, the filter should only be used if you really know what you
145
 * are doing. It was designed to be used with a task scheduler, which is not
146
 * currently complete. This implementation uses the RTC and runs massive
147
 * computations in the RTC interrupt.
148
 *
149
 * @param filter 1 to enable the filter, 0 to leave it turned off
150
 **/
151
void range_init_filter(int filter)
152
{
153
    range_init();
154
    if(filter){
155
        use_filter = 1;
156
        butter_init();
157
    }
158
}
159

    
160
/**
161
 * Reads the distance measured by one of the rangefinders.
162
 * This distance is in arbitrary units.
163
 *
164
 * @param range_id the rangefinder to use. This should be one
165
 * of the constants IR1 - IR5.
166
 *
167
 * @return the distance measured by the rangefinder
168
 *
169
 * @see range_init
170
 **/
171
int range_read_distance(int range_id) {
172
  return linearize_distance(analog8(range_id));
173
}
174

    
175
/**
176
 * Transforms distance readings from logarithmic to linear scale.
177
 * This probably isn't the function you are looking for.
178
 *
179
 * Note: pgm_read_word() needs additional testing
180
 *
181
 * @param value the 8-bit analog value from rangefinder
182
 *
183
 * @return linearized distance reading from rangefinder (integer in [101,800])
184
 **/
185
int linearize_distance(int value) {
186
  if(value < MIN_IR_ADC8) {
187
    return -1;
188
  } else if(value > MAX_IR_ADC8) {
189
    return -1;
190
  } else {
191
    return pgm_read_word(&(IR_dist_conversion[value - MIN_IR_ADC8]));
192
  }
193
}
194

    
195
/**
196
 * Initializes the butterworth filter.
197
 **/
198
void butter_init(void)
199
{
200
    int i;
201
    // init -1 count to 3 for each rangefinder
202
    // this will cause them to restart the filter
203
    for(i=0; i<5; i++)
204
    {
205
        neg_one_count[i] = 3;
206
    }
207

    
208
    // set up the rtc to run butter_task periodically
209
    rtc_init(SIXTEENTH_SECOND, butter_task);
210
}
211

    
212
/*
213
 * Reads each rangefinder and sends its value through the Butterworth filter.
214
 * This task should be run frequently so that range_read_filtered_distance
215
 * returns fresh values
216
 */
217
void butter_task(void)
218
{
219
    butter_filter(IR1, range_read_distance(IR1));
220
    butter_filter(IR2, range_read_distance(IR2));
221
    butter_filter(IR3, range_read_distance(IR3));
222
    butter_filter(IR4, range_read_distance(IR4));
223
    butter_filter(IR5, range_read_distance(IR5));
224
}
225

    
226
/*
227
 * Butterworth helper function that takes a rangefinder ID and turns it into
228
 * an array index between 0 and 4.
229
 */
230
int get_range_index(int range_id){
231
    switch(range_id){
232
    case IR1:
233
        return 0;
234
    case IR2:
235
        return 1;
236
    case IR3:
237
        return 2;
238
    case IR4:
239
        return 3;
240
    case IR5:
241
        return 4;
242
    default: // should never happen
243
        return 0xff;
244
    }
245
}
246

    
247
/**
248
 * Puts the given value from the given rangefinder through the Butterworth
249
 * filter. This function should be called every time a new value is read from a
250
 * rangefinder.
251
 *
252
 * The Butterworth filter has a cutoff frequency of 5Hz and an order of 3
253
 *
254
 * @param range_id the rangefinder to use. This should be one of the constants
255
 * IR1 - IR5.
256
 * @param the value read from that rangefinder
257
 **/
258
void butter_filter(int range_id, int val)
259
{
260
    int range_index = get_range_index(range_id);
261
    // we have a non-error value
262
    if(val > -1 && val <= MAX_IR_LINEAR)
263
    {
264
      // we just passed two or fewer -1's: act as though none seen
265
      if(neg_one_count[range_index] < 3)
266
      {
267
        // shift the values of the arrays to the left
268
        int i;
269
        for(i=0; i<2; i++)
270
        {
271
          butter_x[range_index][i] = butter_x[range_index][i+1];
272
          butter_y[range_index][i] = butter_y[range_index][i+1];
273
        }
274
        butter_x[range_index][2] = butter_x[range_index][3];
275
        // add the new value to the X array
276
        butter_x[range_index][3] = val - MIN_IR_LINEAR;
277
      }
278
      // we just passed three or more -1 values: reset filter with new value
279
      else
280
      {
281
        int i;
282
        // fill x and y values with the new value
283
        for(i=0; i<3; i++)
284
        {
285
          butter_x[range_index][i] = val - MIN_IR_LINEAR;
286
          butter_y[range_index][i] = val - MIN_IR_LINEAR;
287
        }
288
        butter_x[range_index][3] = val - MIN_IR_LINEAR;
289
      }
290

    
291
      // reset the -1 count value
292
      neg_one_count[range_index] = 0;
293

    
294
      /*
295
       * butterworth filter the last values
296
       *
297
       * butterworth filter equation is
298
       * y(t) = x(t-3)/6 + x(t-2)/2 + x(t-1)/2 + x(t)/6 - y(t-2)/3
299
       *
300
       * values are multiplied by 16 before divisions, and divided by 16 at the
301
       * very end to mitigate rounding errors
302
       *
303
       * 8 is added before dividing by 16 so that numbers are rounded instead of
304
       * truncated
305
       */
306
      int16_t temp1 = (int16_t)butter_x[range_index][3] +
307
          (int16_t)butter_x[range_index][0] + (2*MIN_IR_LINEAR);
308
      int16_t temp2 = (int16_t)butter_x[range_index][2] +
309
          (int16_t)butter_x[range_index][1] + (2*MIN_IR_LINEAR);
310
      int16_t temp3 = (int16_t)butter_y[range_index][0] + MIN_IR_LINEAR;
311
      int16_t filtered_big = ((((temp1*8)-(temp3*16))/3)+(temp2*8)+8)/16;
312
      filtered_big -= MIN_IR_LINEAR;
313
      uint8_t filtered = filtered_big > 0xff ? 0xff : (uint8_t)filtered_big;
314

    
315
      butter_y[range_index][2] = filtered;
316
    }
317
    // -1 seen - don't want to store it
318
    else
319
    {
320
        // increment -1 count, preventing overflow
321
        neg_one_count[range_index] = neg_one_count[range_index] == 0xff ? 0xff :
322
            neg_one_count[range_index]+1;
323
    }
324
}
325

    
326
/**
327
 * Returns the most recent filtered reading of the rangefinder. The raw
328
 * rangefinder values have been run through a Butterworth filter.
329
 *
330
 * If the filter was not initialized in rangefinder_init, will return the
331
 * unfiltered value from the rangefinder.
332
 *
333
 * @param range_id the rangefinder to use. This should be one of the constants
334
 * IR1 - IR5.
335
 **/
336
int range_read_filtered_distance(int range_id){
337
    if(!use_filter){
338
        return range_read_distance(range_id);
339
    }
340

    
341
    int range_index = get_range_index(range_id);
342

    
343
    // haven't seen too many -1s recently - return filtered value
344
    if(neg_one_count[range_index] < 3)
345
    {
346
        return butter_y[range_index][2] + MIN_IR_LINEAR;
347
    }
348
    // have seen several -1s - return -1
349
    return -1;
350
}
351

    
352
/** @} **/ //end defgroup