34 
34 
**/

35 
35 

36 
36 
/*

37 

Authors: James Kong and Greg Tress


37 
Authors: James Kong, Greg Tress, Emily Hart

38 
38 

39 

Last Modified: 4/30/06 by James


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

40 
47 
Started log_distance conversion function !!!NOT COMPLETE!!!

41 
48 
Cleaning up comments

42 
49 

...  ...  
45 
52 
Using Sharp GP2D02 IR Rangefinder

46 
53 

47 
54 
Vin is the input to the rangefinder, designated RANGE_CTRL.

48 

Vout is the output from the rangefinder, designated RANGE_IN# where # is the rangefinder you are reading from


55 
Vout is the output from the rangefinder, designated RANGE_IN# where # is the


56 
rangefinder you are reading from

49 
57 

50 
58 
Expected Initial Conditions:

51 
59 
Vin is high and Vout should read high.

...  ...  
58 
66 

59 
67 
*/

60 
68 
#include <avr/pgmspace.h>


69 
#include <avr/interrupt.h>

61 
70 

62 
71 
#include "rangefinder.h"

63 
72 
#include "analog.h"

64 
73 
#include "dio.h"


74 
#include "time.h"

65 
75 

66 
76 
/*

67 
77 
read_distance returns the 8bit reading from the rangefinder

...  ...  
96 
106 
81,80,79,78

97 
107 
};

98 
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 

99 
119 
/**

100 
120 
* @defgroup rangefinder Rangefinder

101 
121 
* @brief Functions for using the IR rangefinders

...  ...  
107 
127 

108 
128 
/**

109 
129 
* Initializes the rangefinders. This must be called before

110 

* range_read_distance.


130 
* range_read_distance. This function does not initialize the filter.

111 
131 
*

112 
132 
* @see range_read_distance

113 
133 
**/

114 
134 
void range_init(void)

115 
135 
{

116 
136 
digital_output(_PIN_B4,0);


137 
use_filter = 0;

117 
138 
}

118 
139 

119 
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 
/**

120 
161 
* Reads the distance measured by one of the rangefinders.

121 
162 
* This distance is in arbitrary units.

122 
163 
*

...  ...  
151 
192 
}

152 
193 
}

153 
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 

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