Building a Hexapod Robot: SONAR Measurement with FPGA

Introduction

One of the projects I am working on at the moment is an FPGA / SoC-controlled hexapod robot which is capable of walking and navigating around obstacles in its environment.

To navigate the robot will need to be able to detect the distance to objects. There are two ways this can be achieved: using stereoscopic cameras or an active sensor such as a ultrasonic range finder.

Ultrasonic range finders work by generating a high frequency signal, and then measuring the time of flight of the reflected signal to determine the distance to an object.

My plan for the hexpod robot is to use a Cora Z7 board to implement the motor control, navigation system, and user interface.

Key drivers for the overall project are size and power, as it will have to fit onto the chassis of the robot and be battery powered. This makes the Cora Z7 ideal.

The Cora Z7 board provides the ability to interface with Two Pmods and a Shield ChipKit connector. I will use the shield for the motor control interface and the control display / user interaction.

This leaves me with two Pmods, one of which will be used with a Pmod Sonar Sensor.

To detect distance, I will therefore be using the Pmod MAXSONAR and reporting the distance on the MTDS display.

This approach should enable the robot to measure the distance between 6 inches (2.54 cm) and 21 feet (6.45 meters) more than sufficient, I think.

What I need to do initially in this project is to create the basic measurement system and ensure it is accurate when measuring distance.

Vivado Build

The first step is to install the board definition files, which enables Vivado to understand the configuration of Cora Z7. You can download the board definition files from here.

However, we need to first make a little modification to the files before we use them, if we wish to use them with the MTDS.

This stems from the pin out of the SPI connection between the Cora Z7 and the MTDS.

The board definition files for the Cora Z7 route all of the SPI signals to the six pin SPI connector J7.

However, the MTDS uses the 6-pin SPI connector for the data signals and clock but the select signal is routed to shield pin 10.

Some development boards, e.g. the Arty S7, route the SPI pins to the SPI header and also to the shield connector IO10 through IO13. This is not the case with the Cora Z7.

As the Cora Z7 does not do this, we have two options:

• Use a separate constraints file for the SS signal

• Update the board definition file for the SS position suitable for use with the MTDS

For this application, I chose to update the board definition file, as it is the simplest approach and it means we can keep using the board interface in Vivado. This allows us to drag and drop board interfaces on to ports as we desire on to the block diagram.

To update the board definition file, under the Cora Z7 board files open part0_pins.xml in a text editor like notepad++ and edit line 72 to use pin U15 in place of pin F16.

To ensure there are no issues, set the pin currently defined to use U15 to F16 you can find this on line 47.

For Vivado to take notice of these changes, if it is currently open you must close and reopen the project.

With these completed the next step is to create a project and add in the elements we require.

From the Digilent IP library we need the following

• PmodMAXSONAR

• PmodMTDS

We also need the Zynq Processing system, as the application will run from the ARM A9 cores.

Creating the Vivado project is simple and involves the steps below:

When the final step has been completed the design can be implemented and the bit file exported to SDK.

Software Build

The software for this build needs to be able to read the PmodMAXSONAR and display the results on the MTDS.

We can of course output the distance over the UART however, that is only really good for testing on the bench.

To drive the PmodMAXSONAR we can use the drivers provided.

To start using the PmodMAXSONAR it is as simple as initializing the Pmod and telling it the clock frequency (100 MHz in this case).

We also have to define the sonar itself.

#include "PmodMAXSONAR.h "
#define PMOD_MAXSONAR_BASEADDR XPAR_PMODMAXSONAR_0_AXI_LITE_GPIO_BASEADDR
#define CLK_FREQ 100000000
PmodMAXSONAR sonar;

To initialize the function we call the MAXSONAR_begin function from within PmodMAXSONAR.h

MAXSONAR_begin(&sonar, PMOD_MAXSONAR_BASEADDR, CLK_FREQ);

The distance measured can then be provided by calling the MAXSONAR_getDistance function.

A gentle reminder for those in Europe this returns the distance in inches not SI units, to convert to SI we need to multiply by 2.54.

dist = MAXSONAR_getDistance(&sonar);

To use the MTDS and draw images on the display, we use the my display library. This provides a range of functions which ease use of the display.

If we want to use logos, we can load BMP images onto the MTDS SD card and access them via the display library when we wish to use them.

#include <MyDisp.h>

For this application to demonstrate the measurement and writing to the display, the main body of the code is pretty simple:

   char c[50];
   MAXSONAR_begin(&sonar, PMOD_MAXSONAR_BASEADDR, CLK_FREQ);
   printf("www.adiuvoengineering.com Presents\n\r");
   printf("Sonar Distance Measuring Example\n\r");
   mydisp.begin();
   mydisp.clearDisplay(clrBlack);
   mydisp.setForeground(clrWhite);
   mydisp.setPen(penSolid);
   mydisp.setForeground(clrBlue);
   mydisp.drawImage((char*) "Images/logo_large.BMP", 70, 20);
   mydisp.drawText((char*) "www.adiuvoengineering.com", 20, 160);
   mydisp.drawText((char*) "Hexapod Robot Controller", 40, 170);
   mydisp.drawText((char*) "Distance To Object", 60, 190);
   print("\n\r");
   u32 dist;
   while (1) {
	  dist = MAXSONAR_getDistance(&sonar);
	  xil_printf("dist (in) = %3d\r", dist);
	  sprintf(c, "%d", dist);
	  EraseImageBox(true, 120, 200, 240, 220);
	  mydisp.drawText(c, 120, 200);
	  usleep(200000);
   }

The position of text and images on the display is references using a X and Y location references. The MTDS display has offers 240 pixels by 320 lines, measured as standard from the top left of the display.

One point to remember is that each time the displayed distance is output, the text must first be cleared to ensure there are no redundant characters from the previous distance.

The function EraseImageBox is used to erase the line the distance is reported on before a new value is displayed.

The final stage is to then generate a first stage boot loader and generate the bit file such that we can run the Cora Z7 board independently.

Testing

The final stage is to test the accuracy by measuring the distance to an object and then measuring with the Sonar System.

The first step was to place an object away from the sensor and measure the distance between the two.

You can also see a short video of the measurement updating in real time below, note for distances below 6 inches the sensor outputs 6 inches.

This gives us the starting point for the hexpod robot and the ability to measure distance to objects and avoid them.

As a side note, it also implements a very handy standalone measurement system.

You can find the files associated with this project here:

https://github.com/ATaylorCEngFIET/Hackster

See previous projects here.

Additional Information on Xilinx FPGA / SoC Development can be found weekly on MicroZed Chronicles.