Check out the new projects site for A-i-S www.adventuresinsilicon.com

Tuesday, December 15, 2009

Code below will let your Arduino poll a Maxsonic EZ0 distance sensor and read the PWM to intelligently report back the distance (uses multiple samples and calculates standard deviation as well to report of how confident you can be it got it right).

//Note - time to sample is not deterministic with this code, it will depend on the distance measured


int pin = 2; //pin revceiving PWM data from EZ0
const int NUMOFSAMPLES = 15; //number of samples to take
const int SAMPLEDELAY = 0; //delay in millis between samples
unsigned int count = 0; //variable to count samples

float duration = 0.0f; //Variable to hold the delay in travel time of ultrasonic sound per sample (microseconds)

float distancesample1 = 0.0f;
float distancesample2 = 0.0f;
float distancesample3 = 0.0f;
float distancefinal = 0.0f;


float distancesamples[NUMOFSAMPLES];

void setup()
{
pinMode(pin, INPUT);
Serial.begin(9600);

// initialise array
for(count = 0; count < NUMOFSAMPLES; count++)
{
distancesamples[count] = 0;
}
count = 0;
}

void loop()
{
//populate array with samples from the EZ0 hardware
count = 0;
for(count = 0; count < NUMOFSAMPLES; count++)
{
duration = 0;
duration = pulseIn(pin, HIGH);
distancesamples[count] = (duration/58.0f); //147us/inch or 58us/cm
duration = 0;
delay(SAMPLEDELAY);
}
//now average the samples to produce the final result
//Note no attempt to remove outliers yet
count = 0;
for(count = 0; count < NUMOFSAMPLES; count++)
{
distancefinal+=distancesamples[count];
}
distancefinal = distancefinal/NUMOFSAMPLES;

//Compute the Standard Deviation of the dataset
float mean = distancefinal;
float SamplesStdDevScratch[NUMOFSAMPLES]; //array to hold intermediate values during calculations
float StdDevScratch = 0.0f;

for(count = 0; count < NUMOFSAMPLES; count++)
{
SamplesStdDevScratch[count] = (pow((distancesamples[count]-mean),2.0f));
}
//now add these up
StdDevScratch = 0.0f;
for(count = 0; count < NUMOFSAMPLES; count++)
{
StdDevScratch += SamplesStdDevScratch[count];
}
//divide by the number of samples
StdDevScratch = StdDevScratch / NUMOFSAMPLES;
//square root this number
StdDevScratch = sqrt(StdDevScratch);

Serial.write("distance : ");
Serial.print(distancefinal);
Serial.write(" cm to object with a Standard Deviation of ");
Serial.print(StdDevScratch);
Serial.write(" \r\n");



delay(100);
}

No comments: