Our Artybot already controls motors via PWM signals and has rotational speed and motor position feedback via magnetic sensors, can "see" colors and has a user interface with switches, buttons, red LEDs, RGB LEDs and an OLED display. In this tutorial we are going to add a time of flight sensor, ToF, so that our robot can explore its environment avoiding obstacles autonomously.
This post is part 11 of my contribution to the "7 Ways to Leave Your Spartan-6" program, learning about the Xilinx Spartan-7 FPGA on the Digilent Arty S7 50 board.
In the video the robot exploring the world with obstacles with different IR reflective capacity.
The robot always moves forwards, If it detects an object within 15 cm, then it stops the robot, swings it on the right wheel ninety degrees and continues forward again.
In the video the sensor is confused by the small handcrafted fused glass plate on the side of the small wooden chest.
Bill of materials
If you want to replicate this project you will need:
| Product Name | Manufacturer | Datasheet |
| Arty S7 50 | Digilent | Buy Now |
| TI-RSLK | element14 | Buy Now |
| PmodOLED | Digilent | Buy Now |
| PmodCOLOR | Digilent | Buy Now |
| PmodToF | Digilent | Buy Now |
Pmod ToF sensor
The Digilent Pmod ToF (Time of Flight) is a sensor that enables optical distance sensing at low power. The on-chip digital signal processor of the ISL29501 calculates the time it takes for light emitted by the ToF to fly to the target and back, which is proportional to the distance to the target. The Pmod ToF can measure distances of up to five meters.
The Pmod ToF is designed to digitally report the distance measured by the Time of Flight based signal processing integrated circuit. It has up to 16-bits of resolution.
It operates using the principle of Square Wave Modulated Indirect Time of Flight (SWM-ITOF) in the frequency domain and obtains distance measurements from the phase shift. The sensing is done by an external emitter (LED) and detector (Photodiode). It has an onboard EEPROM to save the calibrations. Uses a 6-pin Pmod connector.
The Pmod ToF communicates with the host board via the I²C protocol.
| {gallery}Digilent Pmod ToF |
|---|
|
Digilent Pmod ToF unboxing 01 |
|
Digilent Pmod ToF unboxing 02 |
|
Digilent Pmod ToF unboxing 03 |
|
Digilent Pmod ToF unboxing 04 |
Adding the new sensor
We connect the sensor to the Pmod JB port of the Digilent Arty S7 50 board using 200mm cables with Dupont connectors.
We will place the sensor fixed in the front part of the robot with nothing that hinders its line of sight.
Top View
Front View
FPGA Hardware Design. Vivado Hierarchical Blocks
Digilent provides a Library to access Pmod ToF functionality with Digilent FPGA development boards. The Library can be found in the Vivado Hierarchies GitHub repository.
In Vivado, a Hierarchical Block is a block design within a block design. These blocks allow engineers to partition their designs into separate functional groups.
Digilent has several branches of his vivado-library. We'll use the vivado-library/hierarchies at hierarchies
References
- Adding a Hierarchical Block to a Vivado IPI Design - Digilent Reference
- Pmod ToF Reference Manual - Digilent Reference
Adding a Hierarchical Block to a Hardware Design
Launch Vivado, then open the Vivado Project the hierarchical block is to be used in, and open the project's Block Design.
Note: The design must contain a processor and a peripheral that can be used for stdout. In the case of Microblaze, a UART IP must be connected to the board's USBUART interface.
In Vivado's TCL Console, enter the following command:
source C:/Xilinx/vivado-library-zmod-v1-2019.1-2/hierarchies/PmodToF/create_hier.tcl
It will output:
# set script_dir [file normalize [file dirname [info script]]]
# set ip_repo_list [get_property IP_REPO_PATHS [current_project]]
# set ip_repo_path [file normalize $script_dir/../..]
# if {[lsearch $ip_repo_list $ip_repo_path] == -1} {
# lappend ip_repo_list $ip_repo_path
# set_property IP_REPO_PATHS $ip_repo_list [current_project]
# update_ip_catalog
# puts "Loaded IP Repo $ip_repo_path"
# }
# set list_source_files [glob -nocomplain [file join $script_dir sources *]]
# if { [llength $list_source_files] > 0 } {
# # adding the same file more than once is okay
# catch { add_files -fileset sources_1 $list_source_files }
# }
# set pmod [file tail $script_dir]
# for {set idx 0} {[llength [get_bd_cells /${pmod}_${idx}]] > 0} {incr idx} {}
WARNING: [BD 5-230] No cells matched 'get_bd_cells /PmodToF_0'
# set nameHier ${pmod}_${idx};
# set list_constr_files [glob -nocomplain [file join $script_dir constrs *]]
# set tempdir [file join $script_dir temp]
# file mkdir $tempdir
# set existing_constrs []
# foreach constr [get_files -of_objects [get_filesets constrs_1]] {
# lappend existing_constrs [file tail ${constr}]
# }
# foreach constr $list_constr_files {
# set constr_tail [file rootname [file tail ${constr}]]
# set constr_ext [file extension ${constr}]
# set filename ${nameHier}_${constr_tail}${constr_ext}
# if {[lsearch $existing_constrs ${filename}] == -1} {
# file copy ${constr} [file join ${tempdir} ${filename}]
# } else {
# for {set idx 0} {[lsearch $existing_constrs ${nameHier}_${constr_tail}_${idx}${constr_ext}] != -1} {incr idx} {}
# set filename ${nameHier}_${constr_tail}_${idx}${constr_ext}
# file copy ${constr} [file join ${tempdir} ${filename}]
# }
# }
# set list_temp_constr_files [glob -nocomplain [file join ${tempdir} *]]
# catch { import_files -fileset constrs_1 $list_temp_constr_files }
# file delete -force $tempdir
# source -notrace [file join $script_dir "bd.tcl"]
WARNING: [BD_TCL-1002] This script was generated using Vivado <2019.1> without IP versions in the create_bd_cell commands, but is now being run in <2021.1> of Vivado. There may have been major IP version changes between Vivado <2019.1> and <2021.1>, which could impact the parameter settings of the IPs.
INFO: [BD_TCL-6] Checking if the following IPs exist in the project's IP catalog:
xilinx.com:ip:axi_gpio:* xilinx.com:ip:axi_iic:* digilentinc.com:ip:pmod_bridge:* .
##################################################################
# Available Tcl procedures to recreate hierarchical blocks:
#
# create_hier_cell_PmodToF_0 parentCell nameHier
#
##################################################################
# create_hier_cell_${pmod}_* / $nameHier
When the script is finished running, the block design will contain a Hierarchical Block with several IP inside of it. The IP will be connected to one another and to the block's ports and pins. The contents of the hierarchy can be viewed and changed by expanding it with the “+” button.
It contains three blocks
We will need the interrupt signals so we expand the port number of the interrupt signal line concatenator (CONCAT IP) by two more and connect them to the new hierarchy.
Then connect Pmod_out to Board Pmod JB. Run connection automation.
This is the Readme File:
## Adding PmodToF Hierarchy IP to your Design
Please follow the guide :
https://reference.digilentinc.com/learn/programmable-logic/tutorials/vivado-hierarchical-blocks/start
Hierarchy Requirements
* Using the Board Flow it requires the board files (Installation steps https://reference.digilentinc.com/reference/software/vivado/board-files?redirect=1 )
Hierarchy Port Requirements
---------------------------
* IIC clock frequency 100KHz
Constraints
-----------
* When using the Board Flow, no additional constraints are required.
* When not using the Board Flow, template constraints for the Pmod_out port can be found in the imported "PmodToF_*.xdc" file.
Software Library. PmodToF Driver.
Digilent provides a driver that is not integrated into the IP block. You need to add the code files to the project. It is recommended not to change the directory tree structure.
The API is very simple. A method to Initialize. A method to get the distance in meters. Another method to perform three-step calibration. And methods to save the calibration made by the user, to read it and to reset it to the factory one. And finally another method to retrieve the serial number of the sensor.
- void PmodToF_Initialize(); // initializes the Pmod ToF device. This function initialize the EEPROM and ISL29501.
- double PmodToF_perform_distance_measurement(); // returns the distance in meters, measured by the PmodToF device
- uint8_t PmodToF_start_calibration(double actual_distance); // Function for performing a manual calibration of the device, for the actual distance provided as parameter. It calls the function implementing the required calibration sequence: CALIB_perform_magnitude_calibration, CALIB_perform_crosstalk_calibration, CALIB_perform_distance_calibration
- uint8_t PmodToF_WriteCalibsToEPROM_User(); // writes calibration data in the user calibration area of EEPROM.. This function should be called after changes were made in calibration data(after a manual calibration in order to save them in the non-volatile memory.
- uint8_t PmodToF_RestoreAllCalibsFromEPROM_Factory(); // This function restores the factory calibration data from EPROM.
- uint8_t PmodToF_ReadSerialNoFromEPROM(char *pSzSerialNo); // This function reads the user calibration data from EEPROM.
When reading we can apply a low pass filter by taking the average of several measurements.
Calibration
There are three types of calibrations required for Pmod ToF:
1. Magnitude calibration: compensates for the emitter current.
2. Crosstalk calibration: compensates for electrical crosstalk observed by the photodiode. At close range a large return signal values for crosstalk has a minor impact on distance measurements. At the far end of the distance range, the crosstalk might exceed the signal, adding significant error to measurements. In order to perform this calibration, the receiver or both optics need to be covered with the foam included in the package to make sure there is no return path for the IR signal emitted by the LED. If the optics are not correctly covered, it will result in large errors when measurements are taken.
NOTE: The foam included in the package might have some deformities due to the cutting process. In order to insure the best calibration please use the flat side of the foam.
3. Distance calibration: compensates for variation in delay of the emitter, photodiode, and the ISL29501 that will change the signal path delay. It will create a coefficient that will be subtracted in each measurement. For this calibration the user must set a predefined distance for which the calibration is performed. A good calibration at 1.5m can result in the ability of the Pmod ToF to measure up to 5 meters with an error of only a few centimeters. The calibrations should be performed at smaller distances (below 1.5m) to avoid distortion and noise that can affect the calibration. In order to perform the distance calibration, a white target (a target with high IR reflective capacity) should be placed at the desired distance from the Pmod ToF. The Pmod should be also placed at least 40cm above the ground or table and no objects should be within the +/-3° area of the optics. The farther the measurement is taken from the ToF, the bigger the area should be.
For more details about the calibration process, see the ISL29501's Calibration Guide.
The library function PmodToF_start_calibration performs the three steps in a row, giving a few seconds to prepare the devices before starting each of the three calibrations. It is a very simple process and gives very good results.
Simple Obstacle Avoidance App
The application to avoid obstacles autonomously that we have developed is very simple. The robot always moves forward in a straight line using a PID controller whose goal is to maintain the same distance traveled by both wheels. If it detects an object by polling the sensor within 15 cm, then it stops the robot, swings it on the right wheel ninety degrees and continues forward again.
Source Code
The code is the simplest of those we have developed so far thanks to the support of the application framework that we had already developed for the robot.
The application can be downloaded from the github repository under E14SpartanMigrationProgram/bot/src/emubot_obstacle_avoidance_app.c
GitHub - javagoza/E14SpartanMigrationProgram
// while switch4 on move the robot
while (SWITCHES_DRIVER_poll_switch4(&botDrivers.switchesDriver)) {
// drives the robot forward 1 cm if there is an obstacle less than 15 cm away cancel int canceled = DRIVING_DRIVER_drive_to_obstacle(&botDrivers.drivingDriver, 1.0, 15.0);
if (canceled > 0 ) { // driving canceled, swing turn right 90 degreees DRIVING_DRIVER_swing_turn_right_degrees(&botDrivers.drivingDriver, 90); }
// toggle LED for debugging LEDS_DRIVER_set_led4_toggle(&botDrivers.ledsDriver);
}
Main source code:
/************************************************************************/
/* */
/* obstacle_avoidance_app.c -- obstacle_avoidance application */
/* This file is part of the Arty S7 Bot Library */
/* */
/************************************************************************/
/* Author: Enrique Albertos */
/************************************************************************/
/*
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
*/
/************************************************************************/
/* Module Description: */
/* */
/* This file contains the obstacle_avoidance application */
/* */
/************************************************************************/
/* Revision History: */
/* */
/* 2022/07/24: (EAC) created */
/* */
/************************************************************************/
#include "obstacle_avoidance_app.h"
#include "sleep.h"
#define CHAR_FORWARD 0
#define CHAR_LEFT 3
#define CHAR_RIGHT 2
void obstacle_avoidance_application() {
BotDrivers botDrivers;
xil_printf("Obstacle Avoidance application Started\r\n");
BOT_init(&botDrivers);
sleep(1);
PmodToF_Initialize();
OLED_SetCharUpdate(&botDrivers.oled, 0);
OLED_ClearBuffer(&botDrivers.oled);
OLED_SetCursor(&botDrivers.oled, 0, 0);
while (1) {
if (SWITCHES_DRIVER_poll_switch4(&botDrivers.switchesDriver)) {
OLED_SetCharUpdate(&botDrivers.oled, 1);
OLED_Clear(&botDrivers.oled);
OLED_SetCursor(&botDrivers.oled, 0, 3);
OLED_PutString(&botDrivers.oled, "RUNNING");
OLED_SetCursor(&botDrivers.oled, 0, 0);
while (SWITCHES_DRIVER_poll_switch4(&botDrivers.switchesDriver)) {
int canceled = DRIVING_DRIVER_drive_to_obstacle(&botDrivers.drivingDriver, 1.0, 15.0);
if (canceled > 0 ) {
DRIVING_DRIVER_swing_turn_right_degrees(&botDrivers.drivingDriver, 90);
}
LEDS_DRIVER_set_led4_toggle(&botDrivers.ledsDriver);
}
}
OLED_SetCharUpdate(&botDrivers.oled, 0);
OLED_ClearBuffer(&botDrivers.oled);
OLED_SetCursor(&botDrivers.oled, 0, 0);
OLED_PutString(&botDrivers.oled, " OBSTACLE");
OLED_SetCursor(&botDrivers.oled, 0, 1);
OLED_PutString(&botDrivers.oled, " AVOIDANCE v1.0");
OLED_SetCursor(&botDrivers.oled, 0, 3);
OLED_PutString(&botDrivers.oled, "SW3 -> RUN");
OLED_Update(&botDrivers.oled);
BUTTONS_DRIVER_reset(&botDrivers.buttonsDriver);
}
}
