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 |