Got sensors on mail yesterday, play with it several hours and I got pisses due to inconsistent reading. After I quit and go to bed (1am) I thought of an alrorithm that may work so tonight I implemented it the first thing when I got home from work, surprisingly it works beautifully. So here is the driver you could use to play and see if you see what I see.
Not sure who the author of the original code is because there is no header on it. I downloaded it from RobotFreak’s project. So I went ahead and attached GHI’s standard header on it with my modification comments.
Here is the work flow:
-Take 10 readings at a time into an arraylist
-Sort the arraylist asc order
-Then eliminate low/high extreme values.
-Then I avg the middle 6 values. If the avg is out of manufacture’s specs return min or max respectively.
Same concept can be utilized on sharp sensors, so I will post sharp sensors drivers soon as well.
Hope this helps.
//-------------------------------------------------------------------------------------
// GHI Electronics, LLC
// Copyright (c) 2009
// All rights reserved
//-------------------------------------------------------------------------------------
// Modifications:
//-------------------------------------------------------------------------------------
// Hai Nguyen
// 09/2010 Added GetDistanceCmWithFilter() and quickSort() in attempt to
// eliminate possible invalid readings
//
//-------------------------------------------------------------------------------------
/*
* You can use this file if you agree to the following:
*
* 1. This header can't be changed under any condition.
*
* 2. This is a free software and therefore is provided with NO warranty.
*
* 3. Feel free to modify the code but we ask you to provide us with
* any bugs reports so we can keep the code up to date.
*
* 4. This code may ONLY be used with GHI Electronics, LLC products.
*
* THIS SOFTWARE IS PROVIDED BY GHI ELECTRONICS, LLC ``AS IS'' AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
* TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
* GHI ELECTRONICS, LLC BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
* ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR ORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
* Specs are subject to change without any notice
*/
using System;
using System.Threading;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using GHIElectronics.NETMF.Hardware;
using GHIElectronics.NETMF.FEZ;
namespace GHIElectronics.NETMF.FEZ
{
public static partial class FEZ_Components
{
public class Maxbotix : IDisposable
{
AnalogIn adc;
public float div = 1;
public void Dispose()
{
adc.Dispose();
}
public enum MaxbotixSensorType : byte
{
LV_EZx_3V3,
LV_EZx_5V,
XL_EZx_3V3,
XL_EZx_5V,
}
public Maxbotix(FEZ_Pin.AnalogIn pin, MaxbotixSensorType type)
{
adc = new AnalogIn((AnalogIn.Pin)pin);
adc.SetLinearScale(0, 3300);
switch (type)
{
case MaxbotixSensorType.LV_EZx_3V3:
div = (float)(6.4 / 2.54);
break;
case MaxbotixSensorType.LV_EZx_5V:
div = (float)(9.8 / 2.54);
break;
case MaxbotixSensorType.XL_EZx_3V3:
div = (float)(3.2);
break;
case MaxbotixSensorType.XL_EZx_5V:
div = (float)(4.9);
break;
}
}
public float GetDistance_cm()
{
return (float) ((float)adc.Read() / div);
}
public float GetDistanceCmWithFilter()
{
const int ARRAY_SIZE = 10;
float[] array2Sort = new float[ARRAY_SIZE];
//make 10 readings
for (int i = 0; i < ARRAY_SIZE; i++)
array2Sort[i] = GetDistance_cm();
//sort the values in ascending order
quickSort(0, ARRAY_SIZE - 1, ref array2Sort);
//elimate lowest extremes and highest extremes. calculate avg for the middle 6 values
float avg = 0;
for (int i = 2; i < 8; i++)
avg += array2Sort[i];
avg = avg / 6;
//check for valid ranges, make sure reading is within manufacture's specs.
if (avg <= 0 )
return 0;
else if (div == (6.4 / 2.54) || div == (9.8 / 2.54) )
if(avg > 6.45 * 100)
return (float)(6.45 * 100);
else if (div == 3.2 || div == 4.9)
if(avg > (25.1 * 12 * 2.54) )
return (float)(25.1 * 12 * 2.54);
return avg;
}
public void quickSort(int left, int right, ref float[] array2Sort)
{
int leftHolder = left;
int rightHolder = right;
float pivot = array2Sort[left];
while( left < right )
{
while( (array2Sort[right] >= pivot) && (left < right) )
right--;
if( left != right ) {
array2Sort[left] = array2Sort[right];
left++;
}
while( (array2Sort[left] <= pivot) && (left < right) )
left++;
if( left != right ) {
array2Sort[right] = array2Sort[left];
right--;
}
}
array2Sort[left] = pivot;
pivot = left;
left = leftHolder;
right = rightHolder;
if (left < pivot) quickSort(left, (int)pivot - 1, ref array2Sort);
if (right > pivot) quickSort((int)pivot + 1, right, ref array2Sort);
}
}//quickSort
}
}
