SHARP 2Y3A001 F Arduino filtering

I'm using the Sharp 5 beam sensor in a project of mine and I was having trouble with the measurements varying a lot. Stupid me, after checking it out with my Parallax USB Oscilloscope I realized that the input voltage to the sensor wasn't right, I simply didn't have it plugged in right. That stabilized the output and my mean analog readings were close, but I felt that taking the mean wasn't necessarily the best way to go about it, because if you look at the output of the sensor under an oscilloscope it spikes then oscillates, so if you simply do an average of a lot of readings that initial spike will affect your output. I figured that the best value would come from the mode and not from the mean. A mean of the top two frequencies would also work, but for simplicity, I"m just using one. Here is the code I used on the arduino platform. (Essentially just C)

I took 50 samples, put them in an array, applied an insertion sort, and then searched through the list once finding the highest mode. Then I printed out the average mode and min and max to the screen. I also made use of a printFloat function I found floating on the net. Notice I'm also only reading one out of the 5 beams at the moment, I wanted to get the algorithm to work first before changing, but its just a matter of uncommenting a few lines and making some changes. The output from the sensor is not rock solid when you place it down and aim it at an object.


// printFloat prints out the float 'value' rounded to 'places' places after the decimal point
void printFloat(float value, int places) {
// this is used to cast digits
int digit;
float tens = 0.1;
int tenscount = 0;
int i;
float tempfloat = value;

// make sure we round properly. this could use pow from , but doesn't seem worth the import
// if this rounding step isn't here, the value 54.321 prints as 54.3209

// calculate rounding term d: 0.5/pow(10,places)
float d = 0.5;
if (value < 0)
d *= -1.0;
// divide by ten for each decimal place
for (i = 0; i < places; i++)
d/= 10.0;
// this small addition, combined with truncation will round our values properly
tempfloat += d;

// first get value tens to be the large power of ten less than value
// tenscount isn't necessary but it would be useful if you wanted to know after this how many chars the number will take

if (value < 0)
tempfloat *= -1.0;
while ((tens * 10.0) <= tempfloat) {
tens *= 10.0;
tenscount += 1;
}


// write out the negative if needed
if (value < 0)
Serial.print('-');

if (tenscount == 0)
Serial.print(0, DEC);

for (i=0; i< tenscount; i++) {
digit = (int) (tempfloat/tens);
Serial.print(digit, DEC);
tempfloat = tempfloat - ((float)digit * tens);
tens /= 10.0;
}

// if no places after decimal, stop now and return
if (places <= 0)
return;

// otherwise, write the point and continue on
Serial.print('.');

// now write out each decimal place by shifting digits one by one into the ones place and writing the truncated value
for (i = 0; i < places; i++) {
tempfloat *= 10.0;
digit = (int) tempfloat;
Serial.print(digit,DEC);
// once written, subtract off that digit
tempfloat = tempfloat - (float) digit;
}
}





float rawData = 0;
float distance =0;
float alpha[5] = {7545.2,7522,7860.2,8373.9,8093.6};
float beta[5] = {-1.3297,-1.3272,-1.3341,-1.3445,-1.339};
//will be used in equations for different LEDs example : LED1 = 9009x^-1.3591
int iterations = 50;
byte measurements[50];
int mode, TempModeValue;
byte HighestFrequency, TempFrequency;
int i,j,z;
byte key;

void setup(){
Serial.begin(9600); // set up Serial library at 9600 bps
pinMode(13, OUTPUT);
pinMode(11, OUTPUT);
digitalWrite(18,LOW);
for(i=5;i<=9;i++)
digitalWrite(i,HIGH);



}
void loop(){

//distance = rawData;
//Serial.println((int)rawData);
//delay(500);
//Serial.print("?f");//clear screen
rawData =0;
//for(i=4;i<=8;i++)
i=4;
{

///TIME CRITICAL READINGS
digitalWrite(i,LOW);//10cm sensor is inverted
digitalWrite(9,HIGH);
delay(20);
for(z =0;z {
measurements[z] = analogRead(0);
rawData += measurements[z];
}
digitalWrite(i,HIGH);
digitalWrite(9,LOW);
///TIME CRITICAL READINGS OVER


//sort list
for(j=1;j {
key=measurements[j];
i=j-1;
while(measurements[i]>key && i>=0)
{
measurements[i+1]=measurements[i];
i--;
}
measurements[i+1]=key;
}
//take first value as highest
mode = TempModeValue = measurements[0];
HighestFrequency = TempFrequency = 0;

for(i=1;i {
if (measurements[i] == TempModeValue)
TempFrequency++;//represents highest frequency
else
{
TempModeValue = measurements[i];
TempFrequency = 1;
}

if(TempFrequency > HighestFrequency)
{
mode = TempModeValue;
HighestFrequency = TempFrequency;
}
}




rawData /= iterations;
//distance = 1148*pow(rawData,-1.0

//measurements[i-4]= pow(rawData,-1.3591);
//rawData = alpha[i-4]*pow(rawData,beta[i-4]);
for(z=0;z {
printFloat(rawData,2);
Serial.print(',');
Serial.print(measurements[0],DEC);
Serial.print(',');
Serial.print(measurements[49],DEC);
Serial.println();
}

//printFloat(rawData,6);
if(i<8)
//Serial.print(',');
//measurements[i-4] = rawData;
delay(5);
}
// Serial.println();
delay(250);




/*if(Serial.available() > 0)
{
temp = Serial.read();
if( temp == 'r')
{
//printFloat(distance,2);
for(i=0;i<4;i++)
{
printFloat(measurements[i],6);
Serial.print(',');
}
printFloat(measurements[4],6);//so we don't have an extra comma
Serial.println();
/* }
}*/
}