microframework.dk

.NET softwired

Distance measurement with ultrasonic sensor

Introduction

It is very easy to use a .NET Micro Framework device to measure physical short range distance using a ultrasound range finder module. This article describes a hardware module and the software to make it work in a .NET microframework program. The module uses only 2 digital IO pins. You can use any MF device for this, but I have used the NetduinoMini in my project.

The module here can measure distance in the range of 3-450cm with a precision of around 4mm.

I have written a .NET class that wraps the module in easy to use C# code. The code is free to use for you.

Ultrasound module

It is of course possible to build your own module from scratch but it is easier and also cheaper to use a ready made module. I looked at Ebay, and found several candidates which are dirt cheap, and chose the "DYP-ME007" module, which I ordered:

All it takes to use this is +5V power and 2 digital IOs from a microcontroller, 1 output and 1 input.

This particular module has a trigger input and 2 types of output. It has a digital echo output for distance measurements and an additional digital output that indicates if an obstacle is within measurement range of the module (kind of an "on/off detection").

You can go find your own module on Ebay for as little as US$3 including world wide transport here Ebay: Ultrasonic module.

Note that not all modules have the additional output pin that indicates "object within range" but I'm not going to use that in this project anyway. Look for modules that have a "Trig" and "Echo" pins.


Theory

Ultrasonic range finder modules uses the principle of echolocation as used by bats. The module sends out soundwaves (a "ping") and waits for a sound reflection to come back. If the module detects a reflected signal, the distance to the object which caused the reflection can be calculated. This is also known as SONAR (SOund Navigation and RAnging).

Ultrasound soundwaves is defined to have a frequency above 20KHz. This is for most people the upper level of the human audible region. This and other modules similar to the one I use her, use a frequency at 40kHz.

Sound travels at a constant speed, depending on the media (air), pressure and temperature. In theory this means that in order to get very precise measurements, you will need to recalibrate the distance calculation if any of the parameters change after the initial calibration. You can also build in this calibration if you can measure these parameters.

The formula for calculating the distance, keeping airpressure and temperature constant, is:

Distance = 340m/s * Time / 2

Distance is in meters and Time is in seconds. The "divide by 2" part is because the soundwaves will travel from the transducer to the target and then back to the transducer again (2 x distance). 

I will keep it simple in this project and only perform a basic calibration and not care about the other parameters. I'm not going to use this for precise distance measurements anyway.

From the above equation it is easy to see that all we need to measure to be able to calculate the distance, is the "Time" parameter. This is the time it takes from we send out the "Ping" and until we receive the "Echo".


Module description

This module has 2 ultrasound transducers on the board. One for sending out the ping and one for receiving the echo. To get the module up and running, you only have to supply it with 5 volt power and then activate the "Trig" signal. If an obstacle is detected, then the echo pin will be activated and the time between the trigger and receiving the echo signals can be measured. Knowing the time for the echo signal it is a simple calculation to get the distance.

 

  

Making a ping and detecting the echo

To start the distance measurement you have to activate the "trig" pin and then measure the time it takes until the "echo" pin goes active.

Use a "OutputPort" for the trigger and an "InterruptPort" for the echo detection.

// HW IO definitions
private OutputPort trig;
private InterruptPort echo;

// Initialize IO ports and interrupthandler
trig = new OutputPort(TriggerPin, true);
echo = new InterruptPort(EchoPin, true, 
                         Port.ResistorMode.Disabled, 
                         Port.InterruptMode.InterruptEdgeLow);
Echo.OnInterrupt += new NativeEventHandler(echo_OnInterrupt);


Rangefinder code

The rangefinder is implemented as a class, using 2 digital IO pins.

using System;
using Microsoft.SPOT.Hardware;
using System.Threading;

namespace PFJ.NETMF.Hardware.UltrasoundRangefinder
{
    public delegate void RangefinderDistance(double Distance);
    public delegate void RangefinderTicks(long Ticks);

    public class UltrasoundRangefinder : IDisposable
    {
        // HW IO definitions
        private OutputPort trig;
        private InterruptPort echo;

        // Time measurement stuff
        private DateTime pingStart;
        private long timeTicks;

        // Calibration stuff
        private double slope;
        private double offset;

        // Thread stuff
        Thread ScanningThread;
        private object lockObject = new object();
        private bool running;

        // Properties
        private double distance;
        public double Distance
        {
            get 
            {
                lock (lockObject)
                {
                    return distance;
                }
            }
        }

        private bool calibrate;
        public bool Calibrate
        {
            get { return calibrate; }
            set { calibrate = value; }
        }

        #region Event definitions
        public event RangefinderDistance DistanceUpdated;
        private void onDistanceUpdated(double Distance)
        {
            if (DistanceUpdated != null)
                DistanceUpdated(Distance);
        }

        public event RangefinderTicks TicksUpdated;
        private void onTicksUpdated(long Ticks)
        {
            if (TicksUpdated != null)
                TicksUpdated(Ticks);
        }
        #endregion

        #region Construction and Destruction
        public UltrasoundRangefinder(Cpu.Pin TriggerPin, Cpu.Pin EchoPin)
        {
            try
            {
                // Initialize IO ports and interrupthandler
                trig = new OutputPort(TriggerPin, true);
                echo = new InterruptPort(EchoPin, true,
                                         Port.ResistorMode.Disabled,
                                         Port.InterruptMode.InterruptEdgeLow);

                echo.OnInterrupt += new NativeEventHandler(echo_OnInterrupt);

                // Initialize program variables
                timeTicks = 0;
                distance = 0;

                // Set default convertion parameters for ticks to centimeter convertion.
                slope = 0.00173;
                offset = -10.872;
            }
            catch
            {
                throw new Exception("Error initializing UltrasoundRangefinder.");
            }
        }

        public void Dispose()
        {
            // Dispose the IO ports
            trig.Dispose();
            echo.DisableInterrupt();
            echo.Dispose();
        }
        #endregion

        #region Public methods
        // Use a linear function to convert timeticks into centimeters
        public void SetCalibrationData(double SlopeFactor, double Offset)
        {
            this.slope = SlopeFactor;
            this.offset = Offset;
        }

        // Send one ping 
        public void Ping()
        {
            // Send ping and await echo. Echo activates interrupt port.
            lock (lockObject)
            {
                pingStart = DateTime.Now; // Log the starttime of the ping
            }
            // Trig the ultrasound module
            trig.Write(true);
            trig.Write(false);
        }

        // Initialize rangefinder thread and start it.
        public void StartScanning()
        {
            if (ScanningThread == null)
            {
                ScanningThread = new Thread(new ThreadStart(RangeScanning));
                ScanningThread.Start();
            }
        }

        // Set the variable used in the thread method to false to stop the thread.
        public void StopScanning()
        {
            running = false;
        }
        #endregion

        #region Private methods. Thread and interrupthandler.
        // Thread method which activates the Ultrasound module and calculates the distance
        // in centimeters
        // Scanning every 100 milli second
        private void RangeScanning()
        {
            running = true;
            while (running)
            {
                Ping();
                Thread.Sleep(100);
            }
            ScanningThread = null;
        }

        // Echo interrupt handler method
        private void echo_OnInterrupt(uint data1, uint data2, DateTime time)
        {
            // Calculate time from pingstart to echo. 
            // Time calculated in ticks (1 tick = 100 ns)
            lock (lockObject)
            {
                timeTicks = time.Subtract(pingStart).Ticks;

                // Convert timeticks to centimeters
                distance = (double)timeTicks * slope + offset; 
                if (distance < 3 || distance > 500)
                    distance = -1;
            }

            // If calibration is active then raise event with updated ticks value
            if (this.calibrate)
                onTicksUpdated(timeTicks);
            else
                // Raise event with updated distance value, but only if distance > 0
                if (distance > 0)
                    onDistanceUpdated(distance);
        }
        #endregion
    }
}


Links

Controlling Servo Motors with .NET MicroFramework

This post illustrates how you can control Servo Motors from .NET MicroFramework using only simple output pins and C# code.

I have this idea of making a walking robot, so in order to test the basic servo control from .NET MicroFramework, I have used 2 servo motors and built a leg. Each Servo motor controls a joint in a leg.


You can start out with a look at the short video on YouTube that I made to show the leg moving.


In my first test I have used the GHI USBizi development board, but no special hardware has been used, only simple GPIO pins. I wanted to use the built-in PWM feature of the GHI board, but the program locked up when I initialized the PWM feature! I don't know if this is an error in the GHI firmware or what, but instead of investigating this further I made my own Servo control class in C#, which implements the PWM control.


Update 2015.

The code running in the video, is as follows:

using System;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using System.Threading;

namespace PFJ.NETMF.Hardware.Motor
{
    public class Servo
    {
        OutputPort servoPort;
        int pulseWidth;

        int max;
        int min;
        int mid;

        object lockitem;

        public Servo(Cpu.Pin Pin)
        {
            servoPort = new OutputPort(Pin, false);

            max = 1450;
            min = 300;
            mid = 575; // (max - min) / 2;
            pulseWidth = mid;

            lockitem = new object();

            Thread ServoThread = new Thread(ServoProcessor);
            ServoThread.Start();
        }

        public void Left()
        {
            lock (lockitem)
            {
                pulseWidth = min;
            }
        }

        public void Middle()
        {
            lock (lockitem)
            {
                pulseWidth = mid;
            }
        }

        public void Right()
        {
            lock (lockitem)
            {
                pulseWidth = max;
            }
        }

        public void Position(double Percent)
        {
            double d = System.Math.Round((Percent / 100D) * (max - min) + min);
            lock (lockitem)
            {
                pulseWidth = Convert.ToInt32(d.ToString());
            }
        }

        public void Wait(int Delay)
        {
            Thread.Sleep(Delay);
        }

        protected virtual void ServoProcessor()
        {
            while (true)
            {
                lock (lockitem)
                {
                    ServoHighPulse();
                    ServoLowPulse();
                }
                Thread.Sleep(2);
            }
        }


        private void ServoHighPulse()
        {
            servoPort.Write(true);
            DelayMicroSec(pulseWidth);
        }

        private void ServoLowPulse()
        {
            servoPort.Write(false);
            DelayMicroSec(max - pulseWidth);
        }

        /// <summary>
        /// Blocks thread for given number of microseconds
        /// </summary>
        /// <param name="microSeconds">Delay in microseconds</param>
        private void DelayMicroSec(int microSeconds)
        {
            DateTime startTime = DateTime.Now;

            int stopTicks = microSeconds * 10;

            TimeSpan divTime = DateTime.Now - startTime;
            while (divTime.Ticks < stopTicks)
            {
                divTime = DateTime.Now - startTime;
            }
        }
    }
}

Peter