Low-Cost Precision Robotics

Introduction

FPGAs excel in the areas of precision motor control and robotics thanks to their parallel nature. We recently examined how we could work with a high-end robot arm and develop a ROS2-based solution that autonomously wrote text on a whiteboard.

However, we can also create a cost-optimized robotics solution using a servo-based robot arm and devices within the AMD cost-optimized portfolio (such as the AMD Artix™ 7 FPGA or AMD Spartan™ 7 FPGA families).

Such a project is ideal for learning robotics development and FPGA and embedded system development, so that is exactly what we are going to do.

In this project, we are going to show how we can create a robotic arm application which is capable of the following:

• Controlling all 6 Axis joints on the arm via an FPGA
• Enabling control of the arm from a Jupyter Lab instance running on a remote machine
• Communication link shall be RS232 - Scalable to Ethernet using LwIP
• Tracking axis positioning information in the Jupyter Lab instance
• Ability to store positions of the arm in a file
• Ability to replay the stored file to drive the arm through a sequence of movements as required by the application
• Ability to command a selected joint from one position to another

Approach

The approach I am going to take for this is to create an AMD MicroBlaze™ V processor design in the FPGA logic.

This processor will execute a command line interpreter (CLI) which receives the angle for a joint and updates the drive logic for that particular joint.

Using this approach, we can easily update the CLI to support working with LwIP and ethernet commands, enabling a long-distance remote connection.

Each of the joints in the arm will be labelled A through to F, the protocol sent down over the UART link will be:

<joint> <angle> <cr><lf>

Where Joint is A-F and angle is 0 through 180 and CR is carriage return, while LF is line feed.

Inside the FPGA, a simple RTL IP block will be used, which generates the PWM signal necessary to control the motors. This will require the angle is converted to a drive signal on the processor.

Servos operate on a 50 Hz PWM cycle (20 ms). Of that 20 ms, the PWM period nominal on time is 1.5 ms which will place the servo position at the 90 degree point, often called the neutral position. Decreasing the on time to 1 ms will move the servo to the 0 degree point, while increasing it to 2 ms will move the servo to 180 degree point.

As such, we have 180 degrees of potential movement, with a granularity of 1ms / 180 = 5.555 microseconds per degree.

Connectivity

The selected robot arm selected interfaces with the Digilent Arty A7 / S7 board using the Arduino shield interface. It can be powered either externally or from the 5V supplied over the shield connector, via a switch on the shield.

As the 5V current is limited via the sShield connector and motors can be demanding, use the external wall ac-dc convertor to power the arm itself. This will prevent brown outs.

If you are using the later version of the robot arm shield, we need to set the soft start, pin-high to ensure the power to the servo connectors is enabled. We can do this by setting pin 12 high.

To determine if you need to do this or not with your version, examine the shield and if you see a dot on pin 12 in place of a number, you need to drive the pin high.

AMD Vivado™ Design Suite

To get started, we will create a new project in AMD Vivado™ Design Suite within which we will be instantiating the AMD MicroBlaze V processor design.

Once the project is created, the first step is to create an IP Integrator design into which we can add the AMD MicroBlaze V processor.

Once the AMD MicroBlaze V is instantiated, click on the run block automation.

Running the block automation enables us to define exactly how we want the AMD MicroBlaze V configured.

In this case, I am selecting to use the Microcontroller pre-set, 64 KB of local memory, enabling the debug module and peripheral AXI port, along with enabling a new interrupt controller and clocking wizard.

This will automatically configure the AMD MicroBlaze V system as we desire

The next step is to add in the PWMV2 IP from the Digilent Vivado Library available here.

To add in the library, download the zip file and extract it to a location on your development machine. Then use the settings IP repos to add in the new library.

This IP block is nice and simple for PWM generation and supports several PWM outputs.

Configure the block to have six outputs, make the output PWM external.

The penultimate IP block to add is the USB UART this can be added by dragging the USB UART from the boards tab onto the IP Integrator diagram. Run the connection automation to create the AXI network.

We also need to add in a constant block set to a logic high to drive the soft start pin on the robot arm shield.

The final block needed is the clock wizard. Add this to the design and makes its clock input external and configure as below.

Select the system clock as the clock input.

The completed design should look as below.

With this we can create the HDL wrapper (let AMD Vivado Design Suite manage it), before we can build the design we need the IO constraint. For the Arty A7, this can be seen below.

set_property IOSTANDARD LVCMOS33 [get_ports {pwm_0[5]}]
set_property IOSTANDARD LVCMOS33 [get_ports {pwm_0[4]}]
set_property IOSTANDARD LVCMOS33 [get_ports {pwm_0[3]}]
set_property IOSTANDARD LVCMOS33 [get_ports {pwm_0[2]}]
set_property IOSTANDARD LVCMOS33 [get_ports {pwm_0[1]}]
set_property IOSTANDARD LVCMOS33 [get_ports {pwm_0[0]}]
set_property PACKAGE_PIN T11 [get_ports {pwm_0[0]}]
set_property PACKAGE_PIN T14 [get_ports {pwm_0[1]}]
set_property PACKAGE_PIN T15 [get_ports {pwm_0[2]}]
set_property PACKAGE_PIN M16 [get_ports {pwm_0[3]}]
set_property PACKAGE_PIN V17 [get_ports {pwm_0[4]}]
set_property PACKAGE_PIN U18 [get_ports {pwm_0[5]}]

set_property IOSTANDARD LVCMOS33 [get_ports {soft_start[0]}]
set_property PACKAGE_PIN R17 [get_ports {soft_start[0]}]

The XDC comes from the robot arm shield schematics, available here.

With this completed, we can build the bitstream and export the XSA for software development to begin using AMD Vitis™ Unified Software Suite.

AMD Vitis Unified Software Suite

The next stage of the development is to create the the application for the AMD MicroBlaze V processor.

To get started, we need to first create a new platform which includes the XSA configuration.

Select Hardware design and point the Hardware Design (XSA) to the location of the exported XSA from the AMD Vivado Design Suite project.

Select the operating system as standalone, and the processor as the MicroBlaze_RISCV_0.

Click Finish.

Create a new application.

Target the platform we just created.

Select the domain created in the platform creation process.

The software will be design such that it uses the CLI we have used on previous projects with the command expanded to cover the 6 joints of the servos.

To do this there will be several files created, the files are attached to the project and available in the project git repo.

A description of the files is provided below:

• main.c: This file serves as the entry point of the application. It includes master_include.h and defines two main functions: main() and setup_pwm(). The main() function initializes the platform, sets up PWM, and continuously parses user commands by calling cli_parse_command(). The setup_pwm() function is responsible for configuring the PWM hardware by writing appropriate values to the control and duty cycle registers. This file manages the main application flow and hardware interactions.
• cli.h: This is the header file for the command-line interface (CLI) functionalities. It defines several functions and constants that support UART operations and command parsing, such as read_serial(), init_uart0(), and cli_parse_command(). It also declares some global variables (test_id, test_stop, test_oneshot) that are used throughout the CLI system. The header acts as an interface to expose the functions needed for handling serial communication and command processing.
• cli.c: This is the implementation file for the CLI functionalities declared in cli.h. It includes master_include.h and provides the actual implementation for initializing the UART (init_uart0()), reading serial commands (read_serial()), and parsing user commands (cli_parse_command()). It also contains helper functions for converting data types, such as string_to_u8() and char_to_int(). The file manages the interaction between the user and the system, interpreting commands and translating them into corresponding actions.
• master_include.h: This header file acts as the central inclusion point for your project, bringing together various standard and external library headers. It includes libraries such as stdint.h, stdio.h, and Xilinx-specific headers like xil_types.h and xil_io.h. It also includes cli.h for CLI functionalities and defines constants for PWM register offsets (PWM_AXI_CTRL_REG_OFFSET, PWM_AXI_PERIOD_REG_OFFSET, PWM_AXI_DUTY_REG_OFFSET). This file simplifies the inclusion of required libraries across your project, ensuring all necessary dependencies are available.

Key functions used in the cli.c file are:

Convert the angle to servo drive duration.

// Function to convert angle to PWM value
unsigned int angle_to_pwm(int angle) {
    // Clamp angle within valid range
    if (angle < ANGLE_MIN) angle = ANGLE_MIN;
    if (angle > ANGLE_MAX) angle = ANGLE_MAX;
    // Map angle to pulse width in ms
    double pulse_width_ms = MIN_PULSE_WIDTH_MS + ((double)(angle - ANGLE_MIN) / (ANGLE    _MAX - ANGLE_MIN)) * (MAX_PULSE_WIDTH_MS - MIN_PULSE_WIDTH_MS);
    // Convert pulse width in ms to counter value
    unsigned int pwm_period = CLOCK_FREQUENCY / PWM_FREQUENCY;
    unsigned int pulse_width_counts = (unsigned int)((pulse_width_ms / 1000.0) * CLOCK    _FREQUENCY);
    return pulse_width_counts;
}

Xil Print Float - this enables XIL_PRINTF to print out floats.

void xil_printf_float(float x){
    int integer, fraction, abs_frac;
    integer = x;
    fraction = (x - integer) * 100;
    abs_frac = abs(fraction);
    xil_printf("%d.%3d\n\r", integer, abs_frac);
}

Joint processing in the main CLI loop.

if (strcmp(ptr, "a") == 0)
{
    ptr = strtok(NULL, command_delim);
    val = char_to_int(strlen(ptr), ptr);
    unsigned int pulse_width = angle_to_pwm(val);
    Xil_Out32(XPAR_PWM_0_BASEADDR + PWM_AXI_DUTY_REG_OFFSET,pulse_width);
    val = Xil_In32( XPAR_PWM_0_BASEADDR + PWM_AXI_DUTY_REG_OFFSET);
    xil_printf(" Val: 0x%x (%d)\r\n", val, val);
}

Jupyter Lab Application

The control of the robot arm uses a Jupyter lab note book, this communicates over the serial port and implements most of the functionality for commanding the arm.

W developed the application in Jupyter labs notebook, but the entire code can be seen below.

import serial # pyserial library
import ipywidgets as widgets
from IPython.display import display, clear_output
import json
import time

# Define the serial port and the baud rate
port = 'COM4'  # Replace with your serial port name, e.g., '/dev/ttyUSB0' on Linux
baud_rate = 9600  # Common baud rate
try:
    # Open the serial port
    #ser = serial.Serial(port, baud_rate, timeout=1)
    
    # Function to send command to the serial port
    def send_command(change):
        joint = change['owner'].description.split(' ')[1].lower()  # Get joint identifier
        angle = change['new']
        command = f"{joint} {angle}\n\r"
        print(f"Message to be sent: {command.strip()}")
        ser.write(command.encode('ascii'))
        clear_output(wait=True)  # Clear previous output to keep it clean
        print(f"Sent command: {command.strip()}")
    
    # Function to save the current joint settings to a file
    def save_settings():
        with open('joint_settings.json', 'a') as file:
            for joint, value in sliders.items():
                command = f"{joint} {value.value}\n"
                file.write(command)
        print("Joint settings saved to joint_settings.json")
    
    # Function to execute the settings from the file
    def execute_saved_settings():
        try:
            with open('joint_settings.json', 'r') as file:
                for line in file:
                    joint, angle = line.strip().split()
                    command = f"{joint} {angle}\n\r"
                    ser.write(command.encode('ascii'))
                    sliders[joint].value = int(angle)  # Update slider to reflect current position
                    print(f"Executing command: {command.strip()}")
        except FileNotFoundError:
            print("No saved settings file found.")

    # Function to reset all joints to 90 degrees
    def home_position():
        for joint, slider in sliders.items():
            slider.value = 90
            command = f"{joint} 90\n\r"
            ser.write(command.encode('ascii'))
            print(f"Resetting {joint} to 90 degrees")
        print("All joints reset to home position (90 degrees).")

    # Function to transition a joint from a start point to an end point
    def transition_joint(joint, start, end, step=1, delay=0.05):
        if start < end:
            for angle in range(start, end + 1, step):
                command = f"{joint} {angle}\n\r"
                ser.write(command.encode('ascii'))
                sliders[joint].value = angle  # Update slider to reflect current position
                print(f"Transitioning {joint} to {angle} degrees")
                time.sleep(delay)
        else:
            for angle in range(start, end - 1, -step):
                command = f"{joint} {angle}\n\r"
                ser.write(command.encode('ascii'))
                sliders[joint].value = angle  # Update slider to reflect current position
                print(f"Transitioning {joint} to {angle} degrees")
                time.sleep(delay)
                
    # Function to get the current position of all sliders
    def get_current_positions():
        positions = {joint: slider.value for joint, slider in sliders.items()}
        print("Current joint positions:", positions)
        return positions            
    
    # Function to execute saved settings by transitioning joints
    def execute_saved_settings_with_transition():
        try:
            with open('joint_settings.json', 'r') as file:
                for line in file:
                    joint, target_angle = line.strip().split()
                    target_angle = int(target_angle)
                    current_positions = get_current_positions()
                    start_angle = current_positions[joint]
                    transition_joint(joint, start_angle, target_angle)
        except FileNotFoundError:
            print("No saved settings file found.")
    
    # Create sliders for each joint (a to f)
    sliders = {}
    slider_widgets = []
    for joint in ['a', 'b', 'c', 'd', 'e', 'f']:
        slider = widgets.IntSlider(value=90, min=0, max=180, step=1, description=f'Joint {joint.upper()}')
        slider.observe(send_command, names='value')
        sliders[joint] = slider
        slider_widgets.append(slider)
    sliders_box = widgets.VBox(slider_widgets)
    
    # Button to save the current joint settings
    save_button = widgets.Button(description="Save Current Settings")
    save_button.on_click(lambda x: save_settings())
    display(save_button)
    
    # Button to execute the saved settings
    execute_saved_button = widgets.Button(description="Execute Saved Settings")
    execute_saved_button.on_click(lambda x: execute_saved_settings())
    display(execute_saved_button)

    # Button to reset all joints to home position
    home_button = widgets.Button(description="Home Position")
    home_button.on_click(lambda x: home_position())
    display(home_button)

    joint_selector = widgets.Dropdown(options=['a', 'b', 'c', 'd', 'e', 'f'], description='Joint:')
    start_box = widgets.BoundedIntText(value=0, min=0, max=180, step=1, description='Start:')
    end_box = widgets.BoundedIntText(value=180, min=0, max=180, step=1, description='End:')
    move_button = widgets.Button(description="Move Joint")
    
    transition_box = widgets.HBox([joint_selector, start_box, end_box, move_button])
    
    # Button to execute saved settings with transition
    execute_transition_button = widgets.Button(description="Execute Saved Settings with Transition")
    execute_transition_button.on_click(lambda x: execute_saved_settings_with_transition())
    display(execute_transition_button)
    
    def on_move_button_click(_):
        joint = joint_selector.value
        start = start_box.value
        end = end_box.value
        transition_joint(joint, start, end)
    
    move_button.on_click(on_move_button_click)

    # Button to get current positions of sliders
    get_positions_button = widgets.Button(description="Get Current Positions")
    get_positions_button.on_click(lambda x: get_current_positions())
    
    close_button = widgets.Button(description="Close Serial Port")
    close_button.on_click(lambda x: close_serial_port())
    display(close_button)   

    # Arrange buttons in a structured layout
    buttons_box = widgets.VBox([
        widgets.HBox([save_button, execute_saved_button, execute_transition_button]),
        widgets.HBox([home_button, get_positions_button, close_button])
    ])
    
    # Display all widgets in a structured layout
    display(sliders_box, transition_box, buttons_box)
    
    # Close the serial port when done
    def close_serial_port():
        if ser.is_open:
            ser.close()
            print("Serial port closed.")
    
    # Create a button to close the serial port


except serial.SerialException as e:
    print(f"Error: {e}")
except Exception as e:
    print(f"An unexpected error occurred: {e}")

The code is designed to provide an interactive interface for communicating with the AMD MicroBlaze V in the Arty and hence the robot arm.

To make the jupyter lab notebook interactive, I use the ipywidgets library to create a series of sliders and buttons that enable users to adjust the position of individual robotic joints, save, execute specific joint configurations, and smoothly transition between positions.

The core of the functionality involves establishing communication with the AMD MicroBlaze V via a serial port, using the PySerial library (serial). This allows commands to be directly transmitted to the arm based on interactions in the notebook.

The use of interactive widgets makes it easy to visualize and adjust the robot's state in real-time, simplifying the process of controlling complex, multi-joint movements. These position indicators are updated as the application runs to show the current position of the arms joints.

Each joint is represented by a slider widget, which can be set to values between 0 and 180 degrees. Whenever a user changes the value of a slider, the corresponding joint is updated immediately by sending a command to the AMD MicroBlaze V through the serial interface.

To enable the arm to replace sequences, buttons are provided to save the current joint configuration to a file.

This enables the user to move the arm to a position and store that location, it can then be moved to the next position and the next position stored again. Like stop go animation, this allows us to build up a sequence for the robot arm to move through. Commands are stored into a simple json file.

We can then execute this saved sequence using the execute saved sequence button.

To ensure the movement is smooth and not jerky, there is a python function provided, the smooth transition feature.

The smooth transition feature further allows for gradual joint movement, which is crucial for precise and controlled operation, particularly in scenarios where abrupt changes could lead to instability or mechanical stress.

This feature is implemented through a function that iteratively changes the joint value in small steps, sending incremental commands with a slight delay in between.

To ensure a smooth and user-friendly experience, the code also includes functions for closing the serial port safely and for displaying the current position of all joints.

Testing

When we put the hardware, AMD MicroBlaze V processor application and Jupyter notebook together we can see the finished result is a very good system for working with a robot arm.

In the video we can see the arm being moved through a sequence which has been saved using the save location feature and then being replayed.

As the file executes the arm moves smoothly and the position indicators on the sliders move to reflect the position, uou can also see the serial commands being sent over the link to the AMD MicroBlaze V

Wrap Up

This project has been a lot of fun, and it has shown how we can extend the CLI created previously to control a PWM drives for a robotic arm. We were also able to create a detailed python application which works in cooperation with the AMD MicroBlaze™ V to create a fun robotics application.

The completed project can be found here on my GitHub.

AMD sponsored this project. AMD, and the AMD Arrow logo, Artix, MicroBlaze, Spartan, Vitis, Vivado, and combinations thereof are trademarks of Advanced Micro Devices, Inc. Other product names used in this publication are for identification purposes only and may be trademarks of their respective companies.