Speed measurement with ultrasonic sensor Parallax PING))) (C++)

This tutorial guides you through calculating a relative velocity by using the distance measurement of the Parallax PING))) sensor over the time.

In order to be able to measure a speed at all, we must first be able to measure a distance to an object. Ultimately, the ultrasonic sensor can do no more. Therefore, first read Distance measurement with ultrasonic sensor Parallax PING))) (C++).

Calculating the speed

Let's take another closer look at the triangle from the previous chapter:

So we need

Speed = Distance / Time

However, we do not want to go to the speed of sound now and check whether there is a deviation here.

I will show you how to measure the speed of movement of an object using this sensor. For that purpose we need to take two distance measurements in a short time apart and we have:

distance2 - distance1 = distance speed at a given time

If we make the measurements in a time period of 1 second, then we get the speed of movement of the object in cm/s.

When the object is moving in the opposite direction, the speed represented on the display has a negative sign.

Programming the speed measurement

We change our code from the previous example as follows:

#include <iostream>
#include <wiringPi.h>
#include "libParallaxPING.h"

ParallaxPING::ParallaxPING(){}

void ParallaxPING::init(int signal)
{
    this->signal = signal;
    this->timeout = 0.05;
}

double ParallaxPING::distance()
{
    arrivalTime = 0;
    startTime = 0;

    // set to low
    pinMode(signal, OUTPUT);
    digitalWrite(signal, LOW);
    delay(500);

    delay(10);

    // set to high
    digitalWrite(signal, HIGH);
    delayMicroseconds(10);

    // set to low
    digitalWrite(signal, LOW);

    // set to input
    digitalWrite(signal, INPUT)

    timeout_start = micros();

    // Count microseconds that SIG was high
    while(digitalRead(signal) == 0) {
        startTime = micros();

        if(startTime - timeout_start > timeout) {
            return -1;
        }
    }

    while(digitalRead(signal) == 0) {
        arrivalTime = micros();

        if(startTime - timeout_start > timeout) {
            return -1;
        }
    }

    if(startTime != 0 && arrivalTime != 0) {
        pulse_duration = arrivalTime - startTime;
        distance = 100*((pulse_duration/1000000.0)*340.29)/2;

        if(distance >= 0) {
            return distance;
        } else {
            return -1;
        }
    } else {
        return -1;
    }
}

double ParallaxPING::speed()
{
    // calls the distance() function above
    start_distance = this->distance(timeout) * 0.01   // to m conversion

    // giving a time gap of 1 sec
    delay(1000);

    // calls the distance() function above a second time
    end_distance = this->distance(timeout) * 0.01     // to m conversion

    // formula change in distance divided by change in time
    // as the time gap is 1 sec we divide it by 1.
    speed = (end_distance - start_distance) / 1.0  // m/s

    return speed
}

The header looks like following

#ifndef DEF_SONAR
#define DEF_SONAR

class ParallaxPING
{
  public:
    ParallaxPING();
    void init(int signal);
    double distance();
    double speed();

  private:
    int signal;
    volatile long arrivalTime;
    volatile long startTime;
    long timeout_start;
    long timeout;
    long pulse_duration;
    double distance;
    double start_distance;
    double end_distance;
    double speed;
};

#endif

You can run it as example with:

#include <iostream>
#include <wiringPi.h>
#include "libParallaxPING.h"

using namespace std;

int signal = 0;

int main()
{
    if (wiringPiSetup() == -1)
        return -1;

    ParallaxPING ultrasonic;
    ultrasonic.init(signal);

    while(1){
        cout << "Speed is " << ultrasonic.speed() << " m/s." << endl;
    }
}

Now that we know what we can measure with the Parallax PING))) and how, we can write our driver and wrap it in ROS in the next chapter. Please read for this Writing your Parallax PING))) driver (C++).