Highly Accurate driver for the MaxBotix Sensors

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 
    }
}
 

I have most of that in my Ascended.Sensors library which also implements the same IDistanceSensor interface as my sharp sensor classes. It supports the same filtering with standard deviation, averaging, moving averaging. I just havent gotten around to creating the wiki page yet.

(link removed)

@ Hai: Thanks for sharing your code. Why do you perform a quick sort? Would it be less strain on the CPU if you just had a getMax and getMin method? As the purpose for your sort is to remove the first and last readings which are sorted.

Big O notation:
The worst case senario for a quick sort is O(n²)
Getting the min and max is O(1)


private byte getMaxPos (float [] array) {

 byte max = 0

 for (byte b = 1; b < array.Length; b++) {
   if (array[b] > array[max]) {
    max = b;
   }
 }

 return max;
}

btw RJ, if you’re not worried about ram usage… converting that byte b to int b will save you a 10-15 nanoseconds each iteration :slight_smile:

RJ,
I agree with you about performance of quicksort, I know soon or later someone will bring it up. I will refine it as soon as i got a chance.

Thanks for the input.

I have something that uses quicksort as well. I might make a optimized version of it. Too bad the codeshare isn’t around yet.

Dear All,

I updated the code to eleminate possible performance hicup with quick sort. Note: the code was written on a non testable laptop so, it is possible that it may contain systax errors. I will test it once i get home and update it as needed

-Hai

Nice work Hai. I have a MaxBotix sensor, so I will be sure to use your code.

RJ,
Not really though, I coded that at work on a regular .net framework environment , i didn’t realize that on micro framework 4.1, System.Collections.ArrayList library does not have Sort and RemoveRange methods, so it those need to substitute with something else. You can use driver by MarkH instead.

-Thanks!

Thank you Hai for your contribution.
Make sure to have it on CodeExchange when we get it out… soon. 8)