Kria Robotic Starter Kit - Robotic Arm

Introduction

Robots and Robotic systems are one of the key technologies of modern industrial applications. Creating robots is a challenge as there are several competing challenges, performance, responsivity and power efficiency.

The AMD-Xilinx Kria Robotic Starter Kits is a great system to get started developing robot solutions.

In this project we are going to look at how we can get started using the Kria Robotics Starter kit to create a simple robotic arm application.

This project will focus on the hardware and software elements of the design. Once the application is completed and verified it can be expanded in the software domain for ROS2 support.

Robotic Arm

The robotic are selected for this application uses 6 servos to control its position. The end effector is a simple grip which can be opened and closed.

We have six servos to the control on the Arm, the software application will be developed such that each of the joints can be commanded and positioned to the desired point in -90 to +90 range of movement. This software algorithm can be later converted to a ROS2 Node is required.

Hardware Development

The hardware development is performed within Vivado, the signals to drive the Servo requires a PWN signal at 16 ms period with a 1.5 ms nominal high position for a 0 degree of rotation. If the pulse width increases to 2.5 ms the servo will move to the +90 position. If the pulse width becomes 0.5 ms the servo will move to the -90 position.

To this we can use IP integrator and six AXI Timers which are capable of giving out the PWM signal. One for each axis of movement.

To get started we need have Vitis / Vivado installed, the version required for Kria Robotic Starter kit is the 2022.1

Open Vivado and create a new project

select RTL Project

Select board and search for the KRS board, you will need to install this board to use it.

Once the board has been installed select it and assign the IO on the connectors.

Once the board is selected finish the project creation wizard

Once the project is open you should see the Kria Robotics Kit

Create a new block diagram project

Add in the Zynq MPSoC processing system

Run the block automation to configure the PS for the settings on the Kria Robotics Starter Kit

Add in a AXI Timer from the IP Library

Run the connection automation

Add in five more AXI Timers

Run the connection automation

The completed block design should look like below - make the PWM outputs external to the design.

Create a VHDL Wrapper and let Vivado Manage it

The next step is to break out the PWM signals to the Pmod connectors.

We can determine the FPGA pins from looking at the schematics of the Kria Robotics Starter Kit we can see the Pmod connectors are attached the High Density IO on the Kria SoM.

The connector shows the pin out, these can be cross reference with the SoM XDC file.

The XDC I used is below

set_property PACKAGE_PIN H12 [get_ports pwm0]
set_property PACKAGE_PIN E10 [get_ports pwm1]
set_property PACKAGE_PIN D10 [get_ports pwm2]
set_property PACKAGE_PIN C11 [get_ports pwm3]
set_property PACKAGE_PIN B10 [get_ports pwm4]
set_property PACKAGE_PIN E12 [get_ports pwm5]
set_property IOSTANDARD LVCMOS33 [get_ports pwm0]
set_property IOSTANDARD LVCMOS33 [get_ports pwm5]
set_property IOSTANDARD LVCMOS33 [get_ports pwm4]
set_property IOSTANDARD LVCMOS33 [get_ports pwm3]
set_property IOSTANDARD LVCMOS33 [get_ports pwm2]
set_property IOSTANDARD LVCMOS33 [get_ports pwm1]

The design is then ready to be built, once the bitstream is available export the XSA for use in Vitis.

Software Development

Create a new Vitis application targeting the XSA which was just exported from Vivado.

Select the first processor core

Leave the domain unchanged

Select the hello world application

We will use this as the base application upon which we build.

The software application will receive the desired axis indicated 1 to 6 and a series of commands

r - will move a joint to the far right position

l -will move the joint to its far left position

u - will move a joint to its maximum upper deflection

l - will move a joint to its maximum lower position

n - will mode a command to the neural position

p - will move to a -/+90 position

As such commands such as

1 r will move axis one to the right

1 p -45 will move axis one to its -45 position

when the arm is in the commanded position it will reply back with the ACK command enabling the next position to be sent.

As the software has, 6 axis which needs to be controlled, we can create a single function which is passed the parameters for the specific timer instance and the previous position.

u32 pos(XTmrCtr *inst_tmr, u32 HighTime){
u8  Value;
//u32 Step;
u32 Curr_Pos;
s32 Diff;
u32 target;
u8  bytes[4]={0,0,0,0};
float target_f ;
u8  DutyCycle;
print("Select Left (l) / Right (r) / Nominal (n) / or exact Position (p)? \n\r");
Value = inbyte();
Curr_Pos = HighTime;
if (Value == 'l') {
target = min;
} else if (Value =='r'){
target = max;
} else if (Value =='n'){
target = nom;
} else if (Value =='p'){
print("Enter Value Between -90 and +90? \n\r");
bytes[0] = inbyte();
bytes[1] = inbyte();
bytes[2] = inbyte();
int result = atoi(bytes);
target_f = ((float) result ) * 0.11;
s32 target_i = (int) target_f * Step;
if (target_f < 0) { //between -90 and 0
target = nom - abs(target_i);
} else { //between 0 and +90
target = nom + abs(target_i);
}
}
Diff = Curr_Pos - target;
while(Diff != 0)
{
if(Diff < 0 ){
HighTime = HighTime + Step;
Curr_Pos = HighTime;
} else {
HighTime = HighTime - Step;
Curr_Pos = HighTime;
}
Diff = Curr_Pos - target;
XTmrCtr_PwmDisable(inst_tmr);
DutyCycle = XTmrCtr_PwmConfigure(inst_tmr, Period, HighTime);
XTmrCtr_PwmEnable(inst_tmr);
usleep(100000);
}
print("ack\n\r");
return HighTime;
}

The main application performs the following

• Configure and initialise the AXI Timers
• Wait for the motor selection before calling the motor control functions

#include <stdio.h>
#include "platform.h"
#include "xil_printf.h"
#include "xtmrctr.h"
#include "xparameters.h"

#define PWM_PERIOD              16000000    /* PWM period in (500 ms) */
#define TMRCTR_0                0            /* Timer 0 ID */
#define TMRCTR_1                1            /* Timer 1 ID */
#define CYCLE_PER_DUTYCYCLE     10           /* Clock cycles per duty cycle */
#define MAX_DUTYCYCLE           100          /* Max duty cycle */
#define DUTYCYCLE_DIVISOR       4            /* Duty cycle Divisor */
#define WAIT_COUNT              PWM_PERIOD   /* Interrupt wait counter */
#define PWM1			        XPAR_TMRCTR_0_DEVICE_ID
#define PWM2			        XPAR_TMRCTR_1_DEVICE_ID
#define PWM3			        XPAR_TMRCTR_2_DEVICE_ID
#define PWM4			        XPAR_TMRCTR_3_DEVICE_ID
#define PWM5			        XPAR_TMRCTR_4_DEVICE_ID
#define PWM6			        XPAR_TMRCTR_5_DEVICE_ID

XTmrCtr tmr_pwm1;
XTmrCtr tmr_pwm2;
XTmrCtr tmr_pwm3;
XTmrCtr tmr_pwm4;
XTmrCtr tmr_pwm5;
XTmrCtr tmr_pwm6;

u32 Step;
u32 max;
u32 min;
u32 nom;
u32 Period;
u32 HighTime1;
u32 HighTime2;
u32 HighTime3;
u32 HighTime4;
u32 HighTime5;
u32 HighTime6;

int main()
{
	u8  DutyCycle;
	u32 Curr_Pos2;
	s32 Diff2;
	u32 target2;


    print("Avnet Robotics\n\r");
    XTmrCtr_Initialize(&tmr_pwm1, PWM1);
    XTmrCtr_PwmDisable(&tmr_pwm1);
    XTmrCtr_Initialize(&tmr_pwm2, PWM2);
    XTmrCtr_PwmDisable(&tmr_pwm2);
    XTmrCtr_Initialize(&tmr_pwm3, PWM3);
    XTmrCtr_PwmDisable(&tmr_pwm3);
    XTmrCtr_Initialize(&tmr_pwm4, PWM4);
    XTmrCtr_PwmDisable(&tmr_pwm4);
    XTmrCtr_Initialize(&tmr_pwm5, PWM5);
    XTmrCtr_PwmDisable(&tmr_pwm5);
    XTmrCtr_Initialize(&tmr_pwm6, PWM6);
    XTmrCtr_PwmDisable(&tmr_pwm6);
    Period = PWM_PERIOD;
    min =  500000;  //PWM_PERIOD * 0.03125; //0.5ms
    max = 2500000; //PWM_PERIOD * 0.1525;  //2.5ms
    nom = 1500000; //PWM_PERIOD * 0.09375; //1.5ms
    Step = 100000;//PWM_PERIOD * 0.00625; //step size of 0.01 ms
    HighTime1 = nom;

    DutyCycle = XTmrCtr_PwmConfigure(&tmr_pwm1, Period, HighTime1);
    DutyCycle = XTmrCtr_PwmConfigure(&tmr_pwm2, Period, HighTime2);
    DutyCycle = XTmrCtr_PwmConfigure(&tmr_pwm3, Period, HighTime3);
    DutyCycle = XTmrCtr_PwmConfigure(&tmr_pwm4, Period, HighTime4);
    DutyCycle = XTmrCtr_PwmConfigure(&tmr_pwm5, Period, HighTime5);
    DutyCycle = XTmrCtr_PwmConfigure(&tmr_pwm6, Period, HighTime6);

	/* Enable PWM */
	XTmrCtr_PwmEnable(&tmr_pwm1);
	XTmrCtr_PwmEnable(&tmr_pwm2);
	XTmrCtr_PwmEnable(&tmr_pwm3);
	XTmrCtr_PwmEnable(&tmr_pwm4);
	XTmrCtr_PwmEnable(&tmr_pwm5);
	XTmrCtr_PwmEnable(&tmr_pwm6);

	/*
	 * TMR 1 base Rotates, Left, Right, Stop, Neutral.
	 * TMR 2 Forward, BackWard, Stop, Neutral.
	 * TMR 3 Up, Down, Stop, Neutral
	 * TMR 4 Up, Down, Stop, Neutral
	 * TMR 5 Left, Right Stop, Neutral
	 * TMR 6 Open, Close
	 */

	while(1){
		 print("Select a Channel between 1 and 6 \n\r");
		 u8 channel = inbyte();
		 /* Read an input value from the console. */

		 channel = channel - 0x30;
	 switch(channel){
		 case 1:
			 HighTime1 = pos(&tmr_pwm1, HighTime1);
			 break;
		 case 2:
			 HighTime2 = pos(&tmr_pwm2, HighTime2);
			 break;
		 case 3:
			 HighTime3 = pos(&tmr_pwm3, HighTime3);
			 break;
		 case 4:
			 HighTime4 = pos(&tmr_pwm4, HighTime4);
			 break;
		 case 5:
			 HighTime5 = pos(&tmr_pwm5, HighTime5);
			 break;
		 case 6:
			 HighTime6 = pos(&tmr_pwm6, HighTime6);
			 break;
		 }
	}

    cleanup_platform();
    return 0;
}

Testing

The Kria Robotics Kit does not provide developers with the ability to change the boot more to JTAG as such we need to use the script below and XSDB to download the FSBL, FPGA, PMU Firmware and Application software.

proc boot_jtag { } {
############################
# Switch to JTAG boot mode #
############################
targets -set -filter {name =~ "PSU"}
# update multiboot to ZERO
mwr 0xffca0010 0x0
# change boot mode to JTAG
mwr 0xff5e0200 0x0100
# reset
rst -system
}

connect
boot_jtag

after 2000
targets -set -filter {name =~ "PSU"}
fpga "C:/hdl_projects/avnet_robotics/wksp/design_1_wrapper/hw/design_1_wrapper.bit"
mwr 0xffca0038 0x1FF

# Download pmufw.elf
targets -set -filter {name =~ "MicroBlaze PMU"}
after 500
dow C:/hdl_projects/avnet_robotics/wksp/design_1_wrapper/zynqmp_pmufw/pmufw.elf
con
after 500

# Select A53 Core 0
targets -set -filter {name =~ "Cortex-A53 #0"}
rst -processor -clear-registers
dow C:/hdl_projects/avnet_robotics/wksp/design_1_wrapper/zynqmp_fsbl/fsbl_a53.elf
con
after 10000

dow C:/hdl_projects/avnet_robotics/wksp/Avnet_robotics/Debug/Avnet_robotics.elf
after 500
con

Testing the robot over the serial link shows we have full range of control using a serial port

Conclusion

The Kria Robotic Start kit provides a great way to start developing robotic applications. This project while pretty straight forward shows how easily we can get started integrating the hardware project and working with the Robot hardware.