Monday, August 19, 2019

Tello Drone, Swift and State Machines (Part 2)

Stop Rolling Your Own State Machine Code

FIGURE 1. The Flight Plan iOS App

This sub heading is aimed at me to serve as a reminder to stop reinventing the wheel! A lot of the apps that I write benefit from having a Finite State Machine computational model. In an earlier article I wrote about controlling the Tello drone remotely using Swift. If you have a look at this code you will see that I have implemented a simple state machine to track the state of the drone. You need this because you can't send the drone a command unless WiFi is connected and the drone is in command mode (activated by sending it the "command" string via UDP). Thus our app responds differently depending on what state the drone is in.

I felt justified in writing my own state machine code because the initial application was relatively simple. If I was writing a game then I would always use GKStateMachine, the state machine class provided by Apple as part of game kit, but because this is a utility and I was in the UIKit headspace as opposed to the SceneKit/GameplayKit space I didn't think about it. But there is no reason you can't use GameplayKit classes in your UIKit app and in retrospect that is what I should have done (and just spent a day refactoring my code to do). Insert face palm emoji here!

I have continued to add functionality to my drone control app and as the complexity increased my home grown state machine started to become part of the problem and not the solution. Due to the organic development process (i.e. unstructured), I ended up with two state machines which were not scaling well. More importantly the app was acting weird and ending up in undefined states. Of course I could have fixed this in time, but I realised that Apple have already spent a lot of time putting together a robust state machine class and I should be using that!

FIGURE 2. Drone State Machine Diagram

The collateral benefit of having to refactor my code using GKStateMachine was that it made me sit down and plan out what states I needed and what would cause a transition. In other words I needed to develop a state transition table or diagram (Figure 2). After doing this exercise it became apparent that I didn't need two state machines, I just needed to add two states to the original machine. In addition, being forced to come up with the table made me think about some states and/or transitions that I wasn't handling.

TL;DR - Use GKStateMachine even for simple applications!

To demonstrate how easy it is, I will include the boiler plate code for my drone app.

STEP 1 - Create the state classes

For every state in your FSM you need a class to handle transitions, etc. Typically you will need to override the functions shown. I have included the outline for the disconnected state class below. The other state classes have exactly the same format but with different names.

//  DisconnectedState.swift
//  FlightPlan
//  Created by David Such on 18/8/19.
//  Copyright © 2019 Kintarla Pty Ltd. All rights reserved.

import Foundation
import GameplayKit

class DisconnectedState: GKState {
    unowned let viewController: ViewController
    init(viewController: ViewController) {
        self.viewController = viewController
    override func didEnter(from previousState: GKState?) {
        viewController.statusLabel.text = "DISC"
        viewController.WiFiImageView.image = UIImage(named: "WiFiDisconnected")
        if !UserDefaults.standard.warningShown {
            viewController.showAlert(title: "Not Connected to Tello WiFi", msg: "In order to control the Tello you must be connected to its WiFi network. Turn on the Tello and then go to Settings -> WiFi to connect.")
            UserDefaults.standard.warningShown = true
    override func willExit(to nextState: GKState) {
    override func isValidNextState(_ stateClass: AnyClass) -> Bool {
        return (stateClass == WiFiUpState.self) || (stateClass == PlanningState.self)
    override func update(deltaTime seconds: TimeInterval) {


A couple of points. Firstly, make sure that you import GameplayKit. Second, note the constant definition:

unowned let viewController: ViewController

In my app this is the main view controller which contains the UI and will never be NIL. To prevent a retain cycle we use unowned (and not weak since that view controller can never be NIL).

This constant is used to update the UI based on state changes (alternatively you could use a delegate).

STEP 2 - Define the State Machine

Next, within the view controller referred to in step 1, you need to define your state machine.

//  ViewController.swift
//  FlightPlan
//  Created by David Such on 3/6/19.
//  Copyright © 2019 Kintarla Pty Ltd. All rights reserved.

import UIKit
import GameplayKit

class ViewController: UIViewController {
    lazy var stateMachine: GKStateMachine = GKStateMachine(states: [
        DisconnectedState(viewController: self),
        WiFiUpState(viewController: self),
        CommandState(viewController: self),
        PlanningState(viewController: self),
        ManualState(viewController: self),
        AutoPilotState(viewController: self)

As shown above, this is very straight forward. A lazy stored property is a property whose initial value is not calculated until the first time it is used. You indicate a lazy stored property by writing the lazy modifier before its declaration. We need this so that we can assign a pointer to the class containing our state machine (i.e. viewController which is an instance of ViewController) after it has been initialised.

STEP 3 - Use the State Machine

Now we can use our new state machine to keep track of the drone state and handle transitions between states. The first thing you will want to do is to set the initial state. For our drone this is the disconnected state.


You will probably do this in the viewDidLoad method of viewController. Then you can change states when the appropriate event is triggered. For example, the following method is called when the take off button is tapped.

@IBAction func takeOffTapped(_ sender: UIButton) {
        switch stateMachine.currentState {
        case is DisconnectedState:
            showAlert(title: "Not Connected to Tello WiFi", msg: "In order to control the Tello you must be connected to its WiFi network. Turn on the Tello and then go to Settings -> WiFi to connect.")
        case is WiFiUpState:
            showAlert(title: "Awaiting CMD Response", msg: "We haven't received a valid response to our initialisation command. Try sending again from Setup.")
        case is CommandState:
        case is PlanningState:
            if tello.flightPlan.count == 0 {
                let zoom = scrollView.zoomScale - 0.25
                let pitch = dronePointer.frame.size.height
                dronePointerCenter.y -= pitch
                UIView.animate(withDuration: 0.5, delay: 0, options: .curveEaseInOut, animations: { = self.dronePointerCenter}, completion: nil)
                scrollView.setZoomScale(zoom, animated: true)


Depending on the current drone state (stateMachine.currentState) we want to perform different actions. To take off manually, we need to be in the command state. In the planning state, we add the take off command to our flight plan, and animate the action on our viewController.

One last tip. In the example above we are using switch for program control to handle the various states. If you want to check the current state against only one state don't use "==". It wont compile. You need to use "is" instead. For example, to check if the current state is Auto Pilot, you would use:

if stateMachine.currentState is AutoPilotState {

That's it. Next time you need a state machine, don't write your own! Hop on over to GameplayKit and grab GKStateMachine.

Tuesday, June 25, 2019

Mesh Security System (Argon Hub, OLED and MP3 Shields) - Part 2

OLED Display

Figure 5. Argon mounted on Tripler with OLED.

Having demonstrated that we can blink a LED on the Argon, we now want to move onto something a bit more useful. The Argon will form the hub of the Mesh Security System and will connect to an OLED and MP3 shield to indicate system status. In Part 2 we will get the OLED and MP3 shields working.

As shown in Figure 5, connection is simple using the Featherwing Tripler. By mounting the shields horizontally rather than stacking them you can still easily see all the indication LEDs. You will have to solder the headers on the tripler and shields. Do the tripler first. I solder one pin and then check that the header is correctly positioned before soldering the rest. It is a lot easier to rectify an issue with only one pin soldered in place. Once you have completed soldering the headers on the tripler you can use this as a jig to hold the pins in place when soldering them to the shields. This will ensure that the shield pins line up with the headers on the tripler.

Figure 6. OLED Operational

The display board is 128x32 monochrome OLED which has 3 user buttons plus reset. This screen is made of 128x32 individual white OLED pixels and because the display makes its own light, no backlight is required. This reduces the power required to run the OLED and is why the display has such high contrast. The board uses a SSD1306 and connects via I2C (pins D0 and D1), so it is very pin frugal. As I2C is a shared bus you can have other shields which utilise I2C connected at the same time (as long as they have different I2C addresses). The three buttons use:
AD4No pull-up. Can't be used with Ethernet.
BD3100K pull-up. Can't be used with Ethernet.
CD2No pull-up.
So all up this shield uses 5 pins (D0 - D4).

The library is available in the Web IDE as oled-wing-adafruit and using the display from the Argon is easy. The library takes care of setting the appropriate input modes and debouncing the buttons for you.

I've reproduced my test code stub below. I always like to get each element of a project working before adding the next. This makes debugging much easier.

MP3 Shield

The MP3 Shield is shown in Figure 5 above. This is before the through hole headers have been soldered onto the shield. The shield version that we are using is the Adafruit Music Maker FeatherWing. This shield uses the the VS1053, an encoding/decoding (codec) chip that can decode a wide variety of audio formats such as MP3, AAC, Ogg Vorbis, WMA, MIDI, FLAC, WAV (PCM and ADPCM). This chip also allows you to adjust bass, treble, and volume digitally.

Figure 7. Argon Block Diagram (showing I/O).

Communication is via a SPI interface which allows audio to be played from an SD card. There's also a special MIDI mode that you can boot the chip into that will read 'classic' 31250 Kbaud MIDI data from the UART TX pin. The hardware SPI pins are needed whenever you are transmitting data from the SD card to the decoder chip. If you are using the wing in the special MIDI mode, they're not used.

D11: SPI MISO - connected to MISO - used by both the SD card and VS1053
D12: SPI MOSI - connected to MOSI - used by both the SD card and VS1053
D13: SPI SCK - connected to SCK - used by both the SD card and VS1053

The Adafruit VS1053 Library does include a constructor to define the SPI pins you want to use, but this doesn't help us because:

  1. The hardware SPI pins are already connected by the tripler; and
  2. The alternative SPI pins on the Argon are D2, D3 and D4 - which seem to be very popular with shield designers!

Figure 8. Adafruit Music Maker FeatherWing Shield.

Next are the control pins required to play music. From left to right, in Figure 9 below, they are:

MP3_DCS - this is the VS1053 data select pin
DREQ        - this is the VS1053 data request interrupt pin
MP3_CS    - this is the VS1053 chip select pin
SD_CS       - this is the SD Card chip select pin

Figure 9. MP3 Control Pins.

Unfortunately the MP3 control pins connected (via the tripler) to the Argon conflict with the A, B and C buttons connected to D2, D3 and D4 from the OLED shield. Thankfully there is no conflict on pins D0 or D1, so we can still control the OLED with the MP3 shield in place. Obviously the designers of the two shields at Adafruit didn't talk to each other!

Figure 10. MP3 Shield Installed.

To summarise, the Argon pins used to control the MP3 shield are:

SD_CS                = D2;                 // SD Card chip select pin
MP3_CS             = D3;                 // VS1053 chip select pin (output)
DREQ                 = D4;                 // VS1053 Data request, ideally an Interrupt pin
MP3_DCS          = D5;                 // VS1053 Data/command select pin (output)
SPI MISO           = D11;               // used by both the SD card and VS1053
SPI MOSI           = D12;              // used by both the SD card and VS1053
SPI SCK             = D13;               // used by both the SD card and VS1053

Figure 10 shows the MP3 shield in place on the tripler adjacent to the OLED shield. To give myself a bit more room, I removed the OLED shield while soldering the header pins to the MP3 shield. I again inserted the header pins into the tripler before soldering to make sure that everything lined up.

There are two versions of the Adafruit Music Maker, one includes an amplifier and the other just has a 3.5mm connection for headphones or powered speakers. In retrospect I should have got the one with the amplifier built in. Nevertheless I happen to have a Duinotech 2 x 3W amplifier, so I might as well use that. This is the red PCB shown in Figure 10. Before dealing with this, you will want to make sure that the MP3 shield is working.

Thankfully ScruffR has done the hard work of porting the Adafruit VS1053 Arduino library to work with Particle mesh boards. You will need to import this library and the SDFat library in order to get the shield working. This is easy, just search for the libraries in the Web IDE and then add them. Plug in some headphones (assuming you have the same version shield as I do) and you can use the code below to test the operation of your shield. You will obviously need to copy some mp3 files to SD card before you can play them. Make sure that the names of the files are in the 8.3 format or they wont be able to be played.

Duinotech 2 x 3W Amplifier

Rather than use the 3.5mm jack on the MP3 shield, we will connect directly to the Ground, Right and Left pins next to the headphone jack (Figure 11). They are line level, AC coupled outputs which are suitable for connection to an amplifier.

Figure 11. MP3 Shield Audio Out Pins.

The Duinotech 2 x 3W Class D Amplifier (Figure 12) has greater than 90% efficiency and typically delivers 3W into 4 ohm speakers (or 1.5W into 8 ohms). Its operating voltage range is 2.5 to 5.5 VDC.

The amplifier board uses the PAM8403 chip and power output will be determined by a combination of the input voltage supplied and output impedance. As we are using the regulated 3.3V from the Argon and 8 ohm speakers our expected power output from the amplifier is around 0.5W.

Figure 12. Duinotech 2 x 3W Amplifier.

The amplifier pin out description is provided in the table below.

Amplifier Pinout
Right Speaker
Left Speaker
Ground Connection
Power Supply
Shutdown Control
Ground Connection
Left Audio In
Ground for Audio
Right Audio In

Connection between the MP3 shield and amplifier is straight forward.
  1. MP3 Shield L and G connect to LIN and Audio GND on the amplifier.
  2. MP3 Shield R and G connect to RIN and Audio GND on the amplifier.
  3. R+/R- on the amplifier connect to the right speaker.
  4. L+/L- on the amplifier connect to the left speaker.
  5. +5V and GND on the amplifier connect to the 3.3V and GND pins on the Argon.
In Part 3 we will complete construction of the Argon Hub and 3D print an enclosure for it. We will then move onto configuring the Xenon's.

Tuesday, June 18, 2019

Mesh Security System using the Particle Argon and Xenon - Part 1


Figure 1. Argon Board plus some other bits and pieces.

I wanted to learn about the (relatively) new mesh capable boards from Particle, and decided a good project for this would be a mesh security system for our two garages and carport. These are some distance from the house and so should provide a good test of the mesh network range.

The system design will look something like Figure 2. The three Xenon's will communicate via the RF mesh to each other and to the Argon Hub. The Argon will monitor the state of the Xenon's and indicate the system status using an OLED and MP3 shield. The Argon will also connect to our LAN using WiFi and provide more detailed security status via  a web page. If you were building a for real security system it probably wouldn't be a good idea to publish the details on the internet.

Figure 2. Mesh Security Block Diagram.

For this first article we will focus on getting the Argon up and configured. Subsequent articles will focus on the Xenon's.

Particle Xenon and Argon Boards

The Xenon is a low cost mesh-enabled development kit that can act as either an endpoint or repeater within a Particle Mesh network.

The boards are based on the Nordic nRF52840 SoC (System on a Chip), and communicate using the IEEE 802.15.4-2006 standard to create a PAN (Personal Area Network). Bluetooth and active Near Field Communication (NFC) is also available. They have built-in battery charging circuitry which makes it possible to connect and recharge an appropriately sized Li-Po battery. The Xenon has 20 mixed signal (6 x Analog, 8 x PWM) GPIOs to interface with sensors, actuators, and other electronics. Programming it is very similar to an Arduino. The board is compatible with the Adafruit FeatherWing layout and shields can be connected to the base board using a FeatherWing doubler or tripler.

The Particle Argon is similar but includes Wi-Fi. It can be used as a standalone Wi-Fi device or as a Wi-Fi enabled gateway, repeater, or endpoint for Particle Mesh networks. We will be using it in the second configuration for our network.

The Argon has both the Nordic nRF52840 and the Espressif ESP32 processors on board. As with the Xenon it has battery charging circuitry and 20 mixed signal (6 x Analog, 8 x PWM) GPIOs. Other interfaces include UART, I2C, and SPI.

Programming the boards can be done via an extension to Visual Studio Code or using their online IDE. We will try out both methods.

Connecting the Antenna

The Argon uses two different MCU's for WiFi and BLE/Mesh. The WiFi is done using the ESP32 capability and the BLE/Mesh via the nRF82540. Each communication method uses the following frequencies:

  1. WiFi - 2.412 GHz to 2.484 GHz (14 channels)
  2. Bluetooth - 2.400 to 2.485 GHz
  3. Mesh - 2.4 GHz (uses 6LoWPAN over 802.15.4)
  4. NFC - 13.56 MHz

So there is a lot going on around 2.4 GHz if you are using WiFi, BLE and mesh at the same time. This is probably why an external antenna is provided. I assume there is also some smart deconfliction occurring at the hardware or Device OS level.

When talking about the Particle Mesh you may see Thread referenced. Thread is an open mesh networking protocol released by the Thread Group. Particle Mesh uses OpenThread, an open source implementation of Thread released by Nest.

6LoWPAN is an unfortunate acronym that combines the latest version of the Internet Protocol (IPv6) and Low-power Wireless Personal Area Networks (LoWPAN). 6LoWPAN, therefore, allows for the smallest devices with limited processing ability to transmit information wirelessly using an internet protocol. It is a competitor to ZigBee.

Figure 3. Particle 2.4 GHz Antenna

The Argon has 3 antenna connectors (u.FL connector); two on top “BT” (for mesh - nRF52840) and WiFi (for the ESP32), and one on the underside (under the micro-USB connector) for NFC. The Xenon's have 2 antenna connectors; one for “BT” (mesh) and one on the underside for NFC.

The Antenna provided with the Argon is tuned for 2.4GHz so use it for Mesh or WiFi. If you are using NFC, you will need to purchase an antenna tuned for 13.56 MHz. I am going to start out with the external Antenna on WiFi. This is required if you wish to use the WiFi connectivity.

There are two options for the Mesh antenna on the Argon. It comes with an on-board PCB antenna which is selected by default in the device OS and a u.FL connector if you wish to connect an external antenna. If you wish to use the external antenna, you'll need to buy one and issue an appropriate command in the firmware.

Connecting the antenna plug to the u.FL socket on the Argon is easiest done using a pair of long nosed pliers.

First Time Setup

Particle have put together a good video showing how to setup your Argon, so there is no need to reproduce all the steps here. TL;DR - download the iOS or Android app and follow the instructions.

As part of registering your device you will probably have to update the device OS (which abstracts away some of the complexity of programming the Argon). It is all very straight forward and worked well for me. I like the use of the RGB LED to indicate the various states of the device.

Once you've completed the setup you will be able to program your device and send over-the-air (OTA) updates to it.

Flashing the standard blink "hello world" example is a trivial exercise using the Web IDE. I was impressed by how simple this all was. The devs at Microsoft Azure IoT could learn something from this! Next up we will try something a bit more challenging - connecting the OLED and MP3 shields using the FeatherWing Tripler.

Figure 4. FeatherWing Tripler

Saturday, June 8, 2019

Programming the Tello Drone using Swift (Part 1)

The Tello Drone

In this article we will explore how to write a simple iOS app in Swift to allow control of the Tello.

Tello is a mini drone equipped with a HD camera that is manufactured by Ryze Robotics and includes a flight controller with DJI smarts. It is a great drone to learn to fly on as you can use it indoors and because it is so light (80 grams), crashing is fairly painless if you have the prop guards on. I have crashed mine (a lot) and the worst that has happened is that a propeller came off, which is easy to replace. It is also relatively inexpensive. You can manually control it using either an app (iOS or Android) on your phone, or a combination of the app and a dedicated Bluetooth remote. Either works fine. If you do get the Bluetooth remote be careful of not moving out of Bluetooth range of your phone while you are flying the drone.

Tello Specifications

Tello is Powered by a DJIGlobal flight control system and an Intel processor (Movidius MA2x chipset). The MA2x is based on a SARC LEON processor which has two RISC CPUs to run the RTOS, firmware, and runtime scheduler (Ref: RyzeTelloFirmware). The other specifications are:

  • Weight: Approximately 80 g (Propellers and Battery Included)
  • Dimensions: 98×92.5×41 mm
  • Propeller: 3 inches
  • Built-in Functions: Range Finder, Barometer, LED, Vision System, 2.4 GHz 802.11n Wi-Fi, 720p Live View
  • Port: Micro USB Charging Port
  • Max Flight Distance: 100m
  • Max Speed: 8m/s
  • Max Flight Time: 13min
  • Max Flight Height: 30m

Programming - Firmware Versions

Apart from being a good platform to earn your flying chops, the best thing about the Tello from my perspective is that you can write a script or a program to control the drone remotely. This opens up a lot of possibilities.

Note that there are three different Tello's that you can buy (the Tello, the newer Tello EDU and the Ironman Edition), and they use slightly different API's. So make sure that you use the appropriate version for your drone.

You can work out which firmware you have by connecting your mobile to the Tello WiFi, opening the Tello app, tapping on settings (the gear icon), then tap on the More button, and finally tap on the "..." button to the left of the screen. This should bring up the screen shown below which includes the firmware and app version numbers. My Tello is running firmware version You can download the relevant SDK document for this version.

The Tello EDU uses version 2.0 of the SDK. You can download a PDF of the V2 SDK from here.

Commands that are available in SDK v1.3 but not v2.0 are:

  • height?
  • temp?
  • attitude?
  • baro?
  • acceleration?
  • tof?

Conversely, commands that are available in SDK v2.0 but not v1.3 are:

  • stop (hover)
  • go x y z speed mid (same as go x y z speed but uses the mission pad)
  • curve x1 y1 z1 x2 y2 z2 speed mid (same as curve x1 y1 z1 x2 y2 z2 speed but uses the mission pad)
  • jump x y z speed yaw mid1 mid2 (Fly to coordinates x, y and z of mission pad 1 and recognize coordinates 0, 0 and z of mission pad 2 and rotate to the yaw value)
  • mon
  • moff
  • mdirection
  • ap ssid pass
  • sdk?
  • sn?

The Tello EDU also has a swarm mode if you want to control a bunch of drones.

Programming - Python

There are plenty of examples on how to use Python to control your Tello. For drones running v1.3 have a look at the DroneBlocks code. For the Tello EDU (i.e. v2.0 SDK), Ryze Robotics provide some sample code for you to download and try out.

I uploaded the DroneBlocks code using my Raspberry Pi connected to the Tello WiFi and it worked a treat. Given that there are lots of Python examples, I thought I would put together something in Swift and work up to an app which provides additional functionality not found in the official Tello app.

Programming - Swift (iOS)

We access the Tello API by connecting to the airframe via a WiFi UDP port. Once a connection is in place, the drone is controlled using simple text commands.

The first thing we want to determine is whether our device is connected to the Tello WiFi. There are a couple of Swift functions which can assist with establishing this. The Tello SSID name contains the string "TELLO" (see image above), so this is what we will use to determine wether we are connected to the correct WiFi network.

We can use the code above in our ViewController to ensure that we are hooked up to the Tello, and if not provide an alert. The screenshot below shows this implemented in my proof of concept app.

The code for the ViewController is shown next. It should be fairly self explanatory.


UDP (User Datagram Protocol) is a communications protocol, similar to Transmission Control Protocol (TCP), but used primarily for establishing low-latency, low-bandwidth and loss-tolerating connections. UDP sends messages, called datagrams, and is considered a best-effort mode of communications. With UDP there is no checking and resending of lost messages (unlike TCP).

Both UDP and TCP run on top of the Internet Protocol (IP) and are sometimes referred to as UDP/IP or TCP/IP.

UDP provides two services not provided by the IP layer. It provides port numbers to help distinguish different user requests and, optionally, a checksum capability to verify that the data arrived intact.

The Tello IP address is The UDP Services available are:

UDP PORT: 8889 - Send command and receive a response.
UDP SERVER: UDP PORT: 8890 - Receive Tello state.
UDP SERVER: UDP PORT: 11111 - Receive Tello video stream.

If you want to send and receive via UDP on iOS then the two main libraries in use appear to be SwiftSocket and GCDAsyncUDPSocket.

Swift Socket looks to be the simpler of the two libraries, so I used that for my initial attempt. I put together a Tello Swift class to do the heavy lifting. It is reproduced below and works as advertised. You will need to put together your own UI but if you hook up the relevant buttons in the View Controller then you shouldn't have any problem reproducing what I have done.

I will add a bit more functionality to the app (e.g. video) and then stick it up on the app store for download.

Friday, December 21, 2018

Arduino Sonar Display using Processing - Radar & Waterfall


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 >


#define MAX_DISTANCE 30
#define MAX_ANGLE 80
#define ANGLE_STEP 1


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


int angle = 0;
int dir = 1;


Servo hServo, vServo;


void centre(Servo servo, int offset) {
  digitalWrite(LED_PIN, !digitalRead(LED_PIN));
  servo.write(90 + offset);
  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) {

  digitalWrite(LED_PIN, !digitalRead(LED_PIN));
  for (pos = max; pos >= min; pos -= 1) {

void sendSerialPacket(int angle, int distance) {

//  MAIN

void setup() {

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

  digitalWrite(LED_PIN, HIGH);


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

void loop() {
  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, 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.