Self-Balancing Robot Control

1 Introduction – system setup

For this project it has been used following software:

• Ubuntu 20.04 OS - both on workstation and KRIA
• PetaLinux 2020.2 Kria K26 Special Release and Kria KV260 Starter Kit 2020.2.2 BSP
• Vitis 2020.2, Vivado 2020.2.2 with Y2K22 patch, and Board files, which should be installed in <installed_directory>/Vivado/2020.2/data/boards/board_files
• ROS 2 Foxy distribution - workstation and KRIA
• The Kria Robotics Stack (KRS) alpha version

Ubuntu on KRIA

To be able use KRS accelerators without snappy tricks it may be good to use for the Dynamic Function eXchange (DFX) appropriate software and PetaLinux kernel:

• download libdfx, modify libdfx/src/CMakeLists.txt: add_library(dfx SHARED ${libdfx_sources}), compile and install,
• and after download, compile and install dfx-mgr. It may be also good to configure it latter.

Besides, to be able to use Open-AMP to work with Cortex R5F processor additional software should be installed:

• libmetal
  ubuntu@kria:~$ sudo apt install libhugetlbfs-dev libsysfs-dev libudev-dev
  ubuntu@kria:~$ git clone -b xilinx-v2020.2 https://github.com/Xilinx/libmetal.git
  ubuntu@kria:~$ cd libmetal; mkdir build; cd build;
  ubuntu@kria:~$ cmake .. -DWITH_SHARED_LIB=ON; make VERBOSE=1;
  ubuntu@kria:~$ sudo make VERBOSE=1 install
• open-amp
  ubuntu@kria:~$ git clone -b xilinx-v2020.2 https://github.com/Xilinx/open-amp.git
  ubuntu@kria:~$ mkdir open-amp/build
  ubuntu@kria:~$ cd open-amp/build/
  ubuntu@kria:~$ cmake .. -DCMAKE_INCLUDE_PATH=/usr/local/include/ -DCMAKE_LIBRARY_PATH=/usr/local/lib/ -DWITH_APPS=ON
  ubuntu@kria:~$ make VERBOSE=1
  

Unfortunately, Linux Kernel provided by Ubuntu don't support many things and thus it may be good to switch to the PetaLinux custom kernel.

PetaLinux Kernel

Create project on workstation (e.g. petalinux-create -t project -s ../kv260_working.BSP.bsp), enter project directory and configure soft Real Time capabilities:

$ petalinux-build -c linux-xlnx -x distclean
$ petalinux-config -c kernel
$ cd components/yocto/workspace/sources/linux-xlnx/
wget http://cdn.kernel.org/pub/linux/kernel/projects/rt/5.4/older/patch-5.4.3-rt1.patch.gz

$ zcat patch-5.4.3-rt1.patch.gz | patch –p1

$ petalinux-config -c kernel

Set FULL PREEMPT in Kernel configuration utility and build project with following command:

$ petalinux-build

Modify system.dtb as described in to add support for the zocl.

To use custom hardware platform e.g.:

$ petalinux-config --get-hw-description <Vivado_Export_to_SDK_Directory>/system.xsa

Add modules that you need – this process could be reapeat in the future. For example, plug USB WiFi dongle to workstation and check (dmesg, lsmod) which modules it need and then add it to the PetaLinux configuration and build the project.

Copy from PetaLinux project images/linux dirrectory following files to KRIA boot partition: boot.scr, Image, ramdisk.cpio.gz.u-boot, system.dtb - there should be only these files. Modify system.dtb as described in to add support for zocl.

Copy kernel modules directory 5.4.0-rt1-xilinx-v2020.2/ to the KRIA root portion into /lib/modules/ dirrectory. It may be required to run once on KRIA:

ubuntu@kria:~$ sudo depmod -a

to actualize modules database.

Vivado Hardware Platform

To modify default Hardware platform from the KRS for the KV260 (KRIA) please unzip kv260_base.xsa, this file may be in krs_ws/src/acceleration/acceleration_firmware_kv260/acceleration_firmware_kv260/firmware/platform/hw/kv260_base.xsa and enter prj directory and rebuild it:

$ cd prj

$ /opt/xilinx/Vivado/2020.2/bin/vivado -source rebuild.tcl

To use PMOD NAV with Linux add axi_quad_spi with 3 slaves to the Block Desing. Run Connection Automation for this block.

Next add the PIN constraint file (pin.xdc):

# SPI PMOD NAV
set_property PACKAGE_PIN H12 [get_ports Pmod_out_pin1_io]
set_property PACKAGE_PIN E10 [get_ports Pmod_out_pin2_io]
set_property PACKAGE_PIN D10 [get_ports Pmod_out_pin3_io]
set_property PACKAGE_PIN C11 [get_ports Pmod_out_pin4_io]
set_property PACKAGE_PIN B10 [get_ports Pmod_out_pin7_io]
set_property PACKAGE_PIN E12 [get_ports Pmod_out_pin8_io]
set_property PACKAGE_PIN D11 [get_ports Pmod_out_pin9_io]
set_property PACKAGE_PIN B11 [get_ports Pmod_out_pin10_io]
set_property IOSTANDARD LVCMOS33 [get_ports Pmod_out_*]
set_property SLEW SLOW [get_ports Pmod_out_*]
set_property DRIVE 4 [get_ports Pmod_out_*]
#FAN
set_property PACKAGE_PIN A12 [get_ports fan_pin]
set_property IOSTANDARD LVCMOS33 [get_ports fan_pin]

Create output port and run Connection Automation for this block. I have been using 100 MHz clock.

I have been also using fan control from the project, to reduce noise.

Connect interrupt signals. BD should look like this:

Next generate Bitstream and export Hardware Platform with Bitstream.

Now Hardware Platform may be replaced in the KRS (need recompilation) and PetaLinux project may be rebuild, but to make full use of it, we need to add Kernel Modules (st_pressure, st_pressure_spi, st_lsm6dsx) for IMU and generate appropriate Device tree overlays File (*.dtbo). To do it combine original kv260-dp.dtsi from kv260_base.xsa with components/plnx_workspace/device-tree/device-tree/pl.dtsi from the PetaLinux project directory into one dtsi file. Add appropriate lines for sensors e.g.:

axi_quad_spi_0: axi_quad_spi@80000000 {
                                bits-per-word = <8>;
                                clock-names = "ext_spi_clk", "s_axi_aclk";
                                clocks = <&zynqmp_clk 71>, <&misc_clk_0>;
                                compatible = "xlnx,axi-quad-spi-3.2", "xlnx,xps-spi-2.00.a";
				 status = "okay";
                                fifo-size = <16>;
                                interrupt-names = "ip2intc_irpt";
                                interrupt-parent = <&gic>;
                                interrupts = <0 89 1>;
                                num-cs = <0x3>;
                                reg = <0x0 0x80000000 0x0 0x10000>;
                                xlnx,num-ss-bits = <0x3>;
                                xlnx,spi-mode = <0>;
	                        #address-cells = <1>;
        	                #size-cells = <0>;

				imu@0 {
					compatible = "st,lsm9ds1-imu";
					reg = <0x00>;
					spi-max-frequency = <10000000>;	
				};

                                magn@1 {
                                        compatible = "st,lsm9ds1-magn";
                                        reg = <0x01>;
                                        spi-max-frequency = <10000000>;
                                };

                                press@2 {
                                        compatible = "st,lps25h-press";
                                        reg = <0x02>;
                                        spi-max-frequency = <10000000>;
                                };

                        };

and compile it with dtc tool and compile it with dtc tool (e.g. dtc -O dtb -o pl.dtbo -b 0 -@ pl.dtsi). The result dtbo file should be copied krs_ws/src/acceleration/acceleration_firmware_kv260/acceleration_firmware_kv260/firmware/device_tree/kernel_default.dtbo

Besides, we could use it to make some default configuration for the dfx-mgrd. To do it copy files from /lib/firmware/xilinx/base/ rootfs (e.g rootfs.cpio) (PetaLinux project, images directory) to KRIA root partion:

ubuntu@kria:~$ ls -all /lib/firmware/xilinx/base/*

/lib/firmware/xilinx/base/base.dtbo
/lib/firmware/xilinx/base/kv260_base.bit.bin
/lib/firmware/xilinx/base/shell.json

ubuntu@kria:~$ cat /lib/firmware/xilinx/base/shell.json

{
"shell_type" : "XRT_FLAT",
"num_slots": "1"
}

ubuntu@kria:~$ sudo modprobe st_pressure_spi st_lsm6dsx

Replace base.dtbo file from the /lib/firmware/xilinx/base dirrectory with the previously complied kernel_default.dtbo file. Now we could load the Bitware to the FPGA:

ubuntu@kria:~$ sudo dfx-mgr-client -load base

ubuntu@kria:~$ sudo modprobe st_pressure_spi # don't load automatically

And finally we should be able to access the IMU sensors data by the use iio interface (the order may be difrent):

ubuntu@kria:~$ cat /sys/bus/iio/devices/iio\:device2/name

lsm9ds1-imu_gyro

ubuntu@kria:~$ cat /sys/bus/iio/devices/iio\:device3/name

lsm9ds1-imu_accel

ubuntu@kria:~$ cat /sys/bus/iio/devices/iio\:device4/name

lsm9ds1_magn

ubuntu@kria:~$ cat /sys/bus/iio/devices/iio\:device5/name

lps25h

Unfortunately, the access without triggers is rather slowly and I was not able to get proper gyro data for frequency grater then 10 Hz and effectively achieving only few Hz. It is impossible to control balancing robot with such huge time delay.

I must look after different solutions. Fortunately, there is Vivado Library, and recent version seems to work well with 2020.2 Vitis/Vivado. What is also very nice is that with DTBO and DFX we could rearrange system completely quite quickly.

So, let's modify our Hardware Platform a little bit with PmodNAV instead of axi_quad_spi:

It may be worth to notice, that PmodNAV requires 50 MHz clock source. So, I modified appropriately last clock in Clocking Wizard:

And after rebuilding Bitstream and exporting Hardware Platform (with Bitstream) we compile again entirely KRS workspace and PetaLinux project and update dtbo files.

Next, I have created new Platform using Vitis which is basing on Hardware Platform created previously with Vivado. The Vitis Platform has been used for creating standalone application for the Cortex R5F processor. BSP settings have been checked for libmetal and openamp libraries.

Basing on the OpenAMP examples from Vitis, example from Vivado Library for PmodNAV module and some very nice IMU data fusion algorithm, I have created two version of program to menage the IMU sensors – the first (epi_nav_ctrl_open_amp_w_mag.elf) to be used with magnetometers and the second without magnetometer (epi_nav_ctrl_open_amp.elf). The first one runs with frequency about 40 Hz and may be useful for classical mobile robots especially during the outdoor usage and the second one is dedicated to run with 238 Hz, which seems to be more than enough for this project. However, I was able to achieve even about 1000 Hz. The source code for both applications is attached to the project.

Finally, I have written ROS2 application (krm_imu_kria) that receives data using RPSMsgs from the Cortex R5 processor. Here I would like to mention that I would like to implement everything that is considered of low-level robot control for Cortex R5F processors and eventually accelerators created with the use of Vitis HLS. However, KRIA is rather limited when it comes to the GPIO resources. Besides, moving control to the Real Time capable Linux has some advantages as well. For example, HiLS may be implemented relatively easy.

It is worthy to mention that standard memory configuration of the RPMsgs is in conflict with DFX. So, using /dev/nmap (KRIA) I figured out that addresses may be changed from 0x3ED00000 to 0x2ED00000 etc. For PetaLinux project example definitions are in ./project-spec/meta-user/recipes-bsp/device-tree/files/openamp.dtsi and they should be appropriately copied to ./project-spec/meta-user/recipes-bsp/device-tree/files/system-user.dtsi.

reserved-memory {
		#address-cells = <2>;
		#size-cells = <2>;
		ranges;
		rpu0vdev0vring0: rpu0vdev0vring0@2ed40000 {
			no-map;
			reg = <0x0 0x2edf0000 0x0 0x4000>;
		};
		rpu0vdev0vring1: rpu0vdev0vring1@2ed44000 {
			no-map;
			reg = <0x0 0x2ed44000 0x0 0x4000>;
		};
		rpu0vdev0buffer: rpu0vdev0buffer@2ed48000 {
			no-map;
			reg = <0x0 0x2ed48000 0x0 0x100000>;
		};
		rproc_0_reserved: rproc@2ed00000 {
			no-map;
			reg = <0x0 0x2ed00000 0x0 0x40000>;
		};
	};

	zynqmp-rpu {
		compatible = "xlnx,zynqmp-r5-remoteproc-1.0";
		#address-cells = <2>;
		#size-cells = <2>;
		ranges;
		core_conf = "split";
		reg = <0x0 0xFF9A0000 0x0 0x10000>;
		r5_0: r5@0 {
			#address-cells = <2>;
			#size-cells = <2>;
			ranges;
			memory-region = <&rproc_0_reserved>, <&rpu0vdev0buffer>, <&rpu0vdev0vring0>, <&rpu0vdev0vring1>;
			pnode-id = <0x7>;
			mboxes = <&ipi_mailbox_rpu0 0>, <&ipi_mailbox_rpu0 1>;
			mbox-names = "tx", "rx";
			tcm_0_a: tcm_0@0 {
				reg = <0x0 0xFFE00000 0x0 0x10000>;
				pnode-id = <0xf>;
			};
			tcm_0_b: tcm_0@1 {
				reg = <0x0 0xFFE20000 0x0 0x10000>;
				pnode-id = <0x10>;
			};
		};
	};


	zynqmp_ipi1 {
		compatible = "xlnx,zynqmp-ipi-mailbox";
		interrupt-parent = <&gic>;
		interrupts = <0 29 4>;
		xlnx,ipi-id = <7>;
		#address-cells = <1>;
		#size-cells = <1>;
		ranges;

		/* APU<->RPU0 IPI mailbox controller */
		ipi_mailbox_rpu0: mailbox@ff990600 {
			reg = <0xff990600 0x20>,
			      <0xff990620 0x20>,
			      <0xff9900c0 0x20>,
			      <0xff9900e0 0x20>;
			reg-names = "local_request_region",
				    "local_response_region",
				    "remote_request_region",
				    "remote_response_region";
			#mbox-cells = <1>;
			xlnx,ipi-id = <1>;
		};
	};

In Vitis applications changes should be made in lscript.ld file. This file should be opened outside Vitis and every occurrence of 3ed and 3ED should be changed to 2ed or 2ED.

Of course, everything needs to be rebuilt again, but the effects are satisfying.

To run demo:

$ros2 launch krm_imu_kria rviz.py # workstation

ubuntu@kria:~$ros2 launch krm_imu_kria imu.py

Finally we have configured tools and we can start do some serious work.

2 Mobile Robot Control with ROS2

Presented methods of control are extension of some of our previously work. Using ROS it is possible to create complete system for control of mobile and stationary robots. For example, we could create a map

and localize robot using this map.

So, I have moved control laws from the microcontroller to the ROS2 software running on KRIA. Now microcontroller realize some tasks like PWM control of the H-Bridges and Encoders monitoring, but feedback loop is realized efficiently at the KRIA’a Cortex A53 processor. I would like to have everything implemented using FPGA, but unfortunately due to low number of PINs I have to use USB port to communicate with microcontroller.

At this point I could start work at the Control of the Self Balancing Robot.

3 Simulations

Basing on the mathematical model published in, ROS2 simulation and control software was developed.

The whole simulations was computed using KRIA Cortex A53 proccessor. To run it in software mode please run followning commands:

ubuntu@kria:~$ros2 launch sbr_control_simulator simulation.py

$ros2 launch sbr_control_simulator rviz.py # workstation

#ros2 run teleop_twist_keyboard teleop_twist_keyboard # workstation or KRIA

This is proof-of-concept approach. Some issues of control still must be addressed. The accelerated version (accelerated_sbr_control_simulator) using OpenCL kernel running on the FPGA was also developed with the use of the KRS (Vitis).

Hardware version seems to work better in mater of lower jitter but being honest this software needs to be rewritten to isolate non-RT communications from RT control procedures.

3 Real robot

Finally the Self Balancing Robot was assembled.

A few issues have to be corrected, but nevertheless initial tests seems to be very promisisng.