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 8bit 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 20133): 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 8bit 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 nonerror 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(t3)/6 + x(t2)/2 + x(t1)/2 + x(t)/6  y(t2)/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 