Arduino Sonar Display using Processing - Radar & Waterfall

Introduction

The PING or its cheaper clone the HC-SR04 are often used in robotics as a means of obstacle detection. For some time now I have been meaning to put together a means of visualising what the sensor is detecting. This is useful in diagnosing the performance of your robot as it moves around its environment.

The Hardware

To read the HC-SR04 data and control the pan servo I used an Arduino Uno variant (the DFRobot Romeo BLE) that I already had. This is programable via Bluetooth but this isn't necessary, any vanilla uno will do. My setup also included a tilt servo, but this isn't used currently.

I 3D printed a mount for the Arduino which also provides a base for the servos and ultrasonic sensor. The Arduino sketch is very straight forward. It pans the servo from 10 to 170 degrees, with 90 degrees being straight ahead, and sends the current angle and distance to any obstacle (called the range) out on the serial port every 1 degree travelled. This code is reproduced below. The Servo and NewPing libraries do most of the heavy lifting.

/**********************
 @file    Sonar_Visualisation.ino
 @brief   Create visual representation of a sonar sweep using Processing.
 @author  David Such

Code:        David Such
Version:     1.0 
Last edited: 04/11/18
**********************/

#include < Servo.h > 

#include < NewPing.h >

  //  DEFINITIONS

#define MAX_DISTANCE 30

#define MAX_ANGLE 80

#define ANGLE_STEP 1

//  PIN CONNECTIONS

const byte TRIG_PIN = 2;
const byte ECHO_PIN = 3;
const byte H_SERVO = 9, V_SERVO = 10;
const byte LED_PIN = 13;

//  GLOBALS

int angle = 0;
int dir = 1;

//  CREATE CLASS INSTANCES

Servo hServo, vServo;
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);

//  METHODS

void centre(Servo servo, int offset) {
  digitalWrite(LED_PIN, !digitalRead(LED_PIN));
  servo.write(90 + offset);
  delay(15);
  digitalWrite(LED_PIN, !digitalRead(LED_PIN));
}

void sweep(Servo servo, int min, int max) {
  int pos = 0;

  min = constrain(min, 0, 180);
  max = constrain(max, min, 180);

  digitalWrite(LED_PIN, !digitalRead(LED_PIN));
  for (pos = min; pos <= max; pos += 1) {
    servo.write(pos);
    delay(15);
  }

  digitalWrite(LED_PIN, !digitalRead(LED_PIN));
  for (pos = max; pos >= min; pos -= 1) {
    servo.write(pos);
    delay(15);
  }
}

void sendSerialPacket(int angle, int distance) {
  Serial.print(angle);
  Serial.print(",");
  Serial.println(distance);
}

//  MAIN

void setup() {
  Serial.begin(115200);

  pinMode(H_SERVO, OUTPUT);
  pinMode(V_SERVO, OUTPUT);
  pinMode(LED_PIN, OUTPUT);

  digitalWrite(LED_PIN, HIGH);

  hServo.attach(H_SERVO);
  vServo.attach(V_SERVO);

  centre(hServo, 0);
  sweep(vServo, 45, 90);
  centre(vServo, 5);
}

void loop() {
  delay(40);
  unsigned int ping_distance_cm = sonar.ping_cm();

  ping_distance_cm = constrain(ping_distance_cm, 0, MAX_DISTANCE);
  sendSerialPacket(angle, ping_distance_cm);
  hServo.write(angle + MAX_ANGLE);

  if (angle >= MAX_ANGLE || angle <= -MAX_ANGLE) {
    dir = -dir;
  }

  angle += (dir * ANGLE_STEP);
}

Processing 3

Processing is a language and IDE designed for visual display. The language is VERY similar to that used for programming the Arduino and is a c variant. It is perfect for displaying data from the Arduino and this is what we used for our sonar display.

Processing is available for free and there are versions for Windows, MAC and Linux. It also comes as standard on Raspbian and so we used a Raspberry Pi to run our processing sketch and display the output. The same sketches will work on what ever OS you are using, you will just need to change the name of the USB port.

One thing you normally need to consider when connecting serial data is what voltage levels are being used. For example the Raspberry Pi uses 3.3V logic on its I/O and the UNO uses 5V. Connecting these directly could damage the Raspberry Pi. By using the USB ports, voltage conversion is handled by the boards and we don't have to worry about it.

So to get the serial data from the UNO to the Raspberry Pi we just connect the appropriate USB cable between the two boards.

The Raspberry Pi also comes with the Arduino IDE so you can even program the UNO using this if you want, using the same USB cable. Upload the Arduino code first. You can then use this data to debug your processing sketches.

Sonar Displays

I wrote 3 Processing sketches to display the data in different ways. The first is based on the design done by Tony Zhang at hackster.io, I liked his pseudo radar display and wanted to emulate it. Note that I have significantly modified his sketch as it seems to be unnecessarily complicated and includes a bunch of unused code for some reason. You can download the Sonar Display Processing Sketch. Note that all 3 of the sketches use the integer point class which you can also download from the Reefwing Gist.

The second display is my attempt at a waterfall display, similar to that used on submarines to display sonar data. It turned out more like a depth sounder display, but I like the use of perlin noise to represent the outer limit of the sonar range. Download the Depth Display Processing Sketch.

The third display is a combination of the first two displays, which I called the Range Display Processing Sketch.

HC-SR04 Ultrasonic Sensor Python Class for Raspberry Pi

The HC-SR04

The HC - SR04 ultrasonic ranging module provides 2cm - 400cm non-contact
measurement, with ranging accuracy up to 3mm. The module includes ultrasonic transmitters, receiver and control circuitry. The time difference between transmission and reception of ultrasonic signals is calculated. Using the speed of sound and ‘Speed = Distance/Time‘ equation, the distance between the source and target can be easily calculated.

Credit to Vivek and his article on the same subject for the diagrams.

Wiring the HC-SR04 to a Raspberry Pi

The module has 4 pins:

• VCC - 5V Supply
• TRIG - Trigger Pulse Input
• ECHO - Echo Pulse Output
• GND - 0V Ground 

Wiring is straight forward with one exception, note that the sensor operates at 5V not the 3.3V of the Raspberry Pi. Connecting the ECHO pulse pin directly to the Raspberry Pi would be a BAD idea and could damage the Pi. We need to use a voltage divider or a logic level converter module to drop the logic level from the HC-SR04 to a maximum of 3.3V. Current draw for the sensor is 15 mA.

As we have a spare logic level converter, we will use that. Connections for the logic converter are shown below.

For the voltage divider option: Vout = Vin x R2/(R1+R2) = 5 x 10000/(4700 + 10000) = 3.4V

Python Class for the HC-SR04 Ultrasonic Sensor

To utilise the HC-SR04:

1. Provide a trigger signal to TRIG input, it requires a HIGH signal of at least 10μS duration.
2. This enables the module to transmit eight 40KHz ultrasonic bursts.
3. If there is an obstacle in-front of the module, it will reflect those ultrasonic waves
4. If the signal comes back, the ECHO output of the module will be HIGH for a duration of time taken for sending and receiving ultrasonic signals. The pulse width ranges from 150μS to 25mS depending upon the distance of the obstacle from the sensor and it will be about 38ms if there is no obstacle.
5. Obstacle distance = (high level time × velocity of sound (343.21 m/s at sea level and 20°C) / 2
6. Allow at least 60 ms between measurements.

Time taken by the pulse is actually for return travel of the ultrasonic signals. Therefore Time is taken as Time/2.

Distance = Speed * Time/2

Speed of sound at sea level = 343.21 m/s or 34321 cm/s

Thus, Distance = 17160.5 * Time (unit cm).

As we are using the ultrasonic sensor with our Raspberry Pi robot, we have created a python class that can be easily imported and used. Note the calibration function which can be used to help correct for things like altitude and temperature.

We have included a simple low pass filter function which is equivalent to an exponentially weighted moving average. This is useful for smoothing the distance values returned from the sensor. The higher the value of beta, the greater the smoothing.

#!/usr/bin/python
# RS_UltraSonic.py - Ultrasonic Distance Sensor Class for the Raspberry Pi 
#
# 15 March 2017 - 1.0 Original Issue
#
# Reefwing Software
# Simplified BSD Licence - see bottom of file.

import RPi.GPIO as GPIO
import os, signal

from time import sleep, time

# Private Attributes
__CALIBRATE      = "1"
__TEST           = "2"
__FILTER         = "3"
__QUIT           = "q"

class UltraSonic():
    # Ultrasonic sensor class 
    
    def __init__(self, TRIG, ECHO, offset = 0.5):
        # Create a new sensor instance
        self.TRIG = TRIG
        self.ECHO = ECHO
        self.offset = offset                             # Sensor calibration factor
        GPIO.setmode(GPIO.BCM)
        GPIO.setup(self.TRIG, GPIO.OUT)                  # Set pin as GPIO output
        GPIO.setup(self.ECHO, GPIO.IN)                   # Set pin as GPIO input

    def __str__(self):
        # Return string representation of sensor
        return "Ultrasonic Sensor: TRIG - {0}, ECHO - {1}, Offset: {2} cm".format(self.TRIG, self.ECHO, self.offset)

    def ping(self):
        # Get distance measurement
        GPIO.output(self.TRIG, GPIO.LOW)                 # Set TRIG LOW
        sleep(0.1)                                       # Min gap between measurements        
        # Create 10 us pulse on TRIG
        GPIO.output(self.TRIG, GPIO.HIGH)                # Set TRIG HIGH
        sleep(0.00001)                                   # Delay 10 us
        GPIO.output(self.TRIG, GPIO.LOW)                 # Set TRIG LOW
        # Measure return echo pulse duration
        while GPIO.input(self.ECHO) == GPIO.LOW:         # Wait until ECHO is LOW
            pulse_start = time()                         # Save pulse start time

        while GPIO.input(self.ECHO) == GPIO.HIGH:        # Wait until ECHO is HIGH
            pulse_end = time()                           # Save pulse end time

        pulse_duration = pulse_end - pulse_start 
        # Distance = 17160.5 * Time (unit cm) at sea level and 20C
        distance = pulse_duration * 17160.5              # Calculate distance
        distance = round(distance, 2)                    # Round to two decimal points

        if distance > 2 and distance < 400:              # Check distance is in sensor range
            distance = distance + self.offset
            print("Distance: ", distance," cm")
        else:
            distance = 0
            print("No obstacle")                         # Nothing detected by sensor
        return distance

    def calibrate(self):
        # Calibrate sensor distance measurement
        while True:
            self.ping()
            response = input("Enter Offset (q = quit): ")
            if response == __QUIT:
                break;
            sensor.offset = float(response)
            print(sensor)
            
    @staticmethod
    def low_pass_filter(value, previous_value, beta):
        # Simple infinite-impulse-response (IIR) single-pole low-pass filter.
        # ß = discrete-time smoothing parameter (determines smoothness). 0 < ß < 1
        # LPF: Y(n) = (1-ß)*Y(n-1) + (ß*X(n))) = Y(n-1) - (ß*(Y(n-1)-X(n)))
        smooth_value = previous_value - (beta * (previous_value - value))
        return smooth_value
        

def main():
    sensor = UltraSonic(8, 7)       # create a new sensor instance on GPIO pins 7 & 8
    print(sensor)

    def endProcess(signum = None, frame = None):
        # Called on process termination. 
        if signum is not None:
            SIGNAL_NAMES_DICT = dict((getattr(signal, n), n) for n in dir(signal) if n.startswith('SIG') and '_' not in n )
            print("signal {} received by process with PID {}".format(SIGNAL_NAMES_DICT[signum], os.getpid()))
        print("\n-- Terminating program --")
        print("Cleaning up GPIO...")
        GPIO.cleanup()
        print("Done.")
        exit(0)

    # Assign handler for process exit
    signal.signal(signal.SIGTERM, endProcess)
    signal.signal(signal.SIGINT, endProcess)
    signal.signal(signal.SIGHUP, endProcess)
    signal.signal(signal.SIGQUIT, endProcess)

    while True:
        action = input("\nSelect Action - (1) Calibrate, (2) Test, or (3) Filter: ")

        if action == __CALIBRATE:
            sensor.calibrate()
        elif action == __FILTER:
            beta = input("Enter Beta 0 < ß < 1 (q = quit): ")
            filtered_value = 0
            if beta == __QUIT:
                break;
            while True:
                filtered_value = sensor.low_pass_filter(sensor.ping(), filtered_value, float(beta))
                filtered_value = round(filtered_value, 2)
                print("Filtered: ", filtered_value, " cm")
        else:
            sensor.ping()

if __name__ == "__main__":
    # execute only if run as a script
    main()

## Copyright (c) 2017, Reefwing Software
## All rights reserved.
##
## Redistribution and use in source and binary forms, with or without
## modification, are permitted provided that the following conditions are met:
##
## 1. Redistributions of source code must retain the above copyright notice, this
##   list of conditions and the following disclaimer.
## 2. Redistributions in binary form must reproduce the above copyright notice,
##   this list of conditions and the following disclaimer in the documentation
##   and/or other materials provided with the distribution.
##
## THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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 TORT
## (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
## SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

Sharp GP2Y0A02YK0F IR Distance Sensor (20-150 cm) Arduino Library

Distance Measuring Options

If you are building an autonomous robot then you need to have some sort of obstacle avoiding sensors. I have attached a Parallax PING ultrasonic sensor to the front of AVA using a servo (so that I can scan 180 degrees). Ultrasonic sensors are generally pretty accurate but since they use reflected sound to calculate distance, they don't perform well if the obstacle is sound absorbing. Ultrasonic sensors can also miss thin objects or objects that reflect the sound away from the sensor. However, the range of ultrasonic sensors is much better than IR. For the PING, the available sensing range is 2 cm to 3 m.

To address the ultrasonic issues, I also mounted a Sharp IR Distance Sensor (GP2Y0A02YK0F) above the PING. The IR sensors don't perform well outside but indoors there accuracy is good enough as long as you stay within the quoted detection limits. IR sensors are generally cheaper than ultrasonic, their beams are more directional (narrower) and reflectivity of the surface is more important than the sound absorbing properties of potential obstacles.

Putting the two sensors together is complementary and allows the short comings of both sensors to be addressed (to an extent).

Sharp GP2Y0A02YK0F IR Distance Sensor (20-150 cm) 

Sharp manufactures a range of IR Distance Sensors. For the front sensor I selected the GP2Y0A02YK0F, which has a usable detection range of 20 to 150 cm's.

The Sharp GP2Y0A02YK0F measures distances in the 20–150 cm range using a reflected beam of infrared light.  By using triangulation to calculate the distance measured, this sensor can provide consistent readings that are less influenced by surface reflectivity, operating time, or environmental temperature.  The Sharp GP2Y0A02YK0F outputs an analog voltage corresponding to the distance to the reflecting object.

If you have a look at the GP2Y0A02YK0F datasheet, you will see that the analog voltage output does not have a linear relationship to distance. You can also see that the values go crazy below about 20 cm.

Noah over at the Arduino Mega Blog has reversed engineered this plot to work out the relationship between distance and the output voltage.

distance = 10650.08 * sensorValue ^ (-0.935) - 10 cm

Sharp GP2Y0A02YK0F IR Distance Sensor Arduino Library

To connect to an Arduino and get a distance you could just use Noah's formula above, but sometimes it is easier to wrap the complexity up in a library. I did a search and didn't find an existing library, so I decided to do one myself. Partly because I haven't done one before.

I did find a library for the GP2Y0A21YK IR Distance sensor (10 - 80 cm), but the characteristics must be different to the GP2Y0A02YK0F as the distances provided by the library are way off. Noah's formula on the other hand, provides very good correlation with the distances measured by the PING. For consistency, I based my library on what jeroendoggen did for his.

You can download the Sharp GP2Y0A02YK0F IR Distance Sensor (20-150 cm) Arduino Library files, and then follow these instructions to use it:

Instructions:

• Create a directory called GP2Y0A02YK0F within the libraries sub directory where your Arduino sketches are saved.
• Copy GP2Y0A02YK0F.h, GP2Y0A02YK0F.cpp and keywords.txt into the GP2Y0A02YK0F directory.
• Within the GP2Y0A02YK0F directory, create a sub directory called examples.
• Copy DisplayCM.ino into the examples sub directory.
• Restart the Arduino IDE to see the new library.

Sharp GP2Y0A02YK0F Mounted on AVA

The following photo shows the Sharp IR sensor mounted above the front PING on AVA. I have fitted a sensor shield to the Arduino Mega which makes it very easy to connect the various sensors to the micro controller.