Intelligent Motor Control

Introduction

Several projects I have published on here have focused on robotics, most of these have used simple motors or servos. A couple of these have used more advanced motor solutions such as the hand writing robot arm or Turtle Bot Waffle. However when we have used more advanced solutions we have leveraged custom / provided hardware to interface with the motors.

This does not really demonstrate one of the main benefits of using an FPGA solution, system integration.

When it comes to working with advanced motors such as the Dynamixel range, the custom / provided hardware which implements the motor interfaces communicates using either a TTL or RS485 communications.

These TTL / RS485 communications are received by processors within the motor itself. It is this processor motor which controls the actual configuration and movement of the motor. More crucially and necessary for real precision robotics is the reporting of the accurate motor position and status back to the controller.

When it comes to Dynamixel motors, the protocol sent over the physical layer, are half duplex, UART based all that changes is physical layer being either TTL or RS485

As this is basically a UART based solution, we are able to integrate this within a FPGA simply and create a driver which is cable of controlling these intelligent motors.

To help create more complex robotic solutions it is possible to daisy chain motors to implement robotic solutions which need multiple axis or motor drives.

Intelligent Motor Protocol

Dynamixel motors use a digital packet-based control protocol carried over a half-duplex serial interface. Depending on the motor family, the physical layer is either TTL (5 V) or RS-485, with TTL commonly used on smaller actuators and RS-485 on larger or longer-cable installations. Multiple motors share the same bus and are daisy-chained together, each identified by a unique ID. Typical baud rates range from 9.6 kbps up to 1 Mbps or higher, with an initial default of 57.6 Kbps.

Communication is performed using structured frames sent from the controller to the motor. A frame contains a header, the target motor ID, a length field, an instruction (such as read or write), associated parameters, and a 16 bit CRC for error detection. Motors can respond with a status frame that confirms the command and optionally returns requested data. Broadcast frames can also be used to command all motors simultaneously without generating replies.

Each Dynamixel motor exposes a control table, which is a memory map of internal registers. This table is split into EEPROM and RAM regions. EEPROM stores non-volatile settings such as motor ID, baud rate, operating mode, and limit values, and these persist across power cycles. RAM holds run-time parameters such as torque enable, goal position or velocity, and real-time status values like present position, current, and temperature. By reading and writing these registers via the protocol, full configuration and control of the motor is achieved entirely in software.

This sounds like a perfect job for a MicroBlaze V with a little custom UART Interface to implement either the RS485 or TTL interface.

Design

The nice thing about using a MicroBlaze is we are able to integrate the smart motor control within the FPGA itself using a small logic footprint. If we desire we can implement in the software standalone applications, or something which integrates with other higher level processors using MicroRoS etc.

What we need to be able to do is transmit and receive bytes at half duplex using a simple UART. We want to do this as simple as possible so a simple UART will be created which uses AXI Stream interfaces. This will make it much simpler than even the AXI UART Lite.

The use of AXI Stream works well for the proposed MicroBlaze V solution as we are able to use the MicroBlaze Streaming links which implement AXI Stream interfaces. The software stack to work with these links is also very simple which helps keep the required code foot print reduced.

UART development.

The first stage then was the development of a VHDL UART interface. This module would be designed to implement a 8 bit UART with 1 bit start and stop and no parity. Designed to have both Tx and Rx data interfaces implemented via AXI Stream, configurable baud rate via a generic and designed to operate from a 100 MHz clock.

For this design as we are working with a TTL based motor the XL430-W250 (link) a single bidirectional IO signal.

--------------------------------------------------------------------------------
-- Dynamixel XL430-W250 UART PHY Layer Interface
--
-- Description:
--   Half-duplex UART interface for Dynamixel servo communication
--   - Baud rate: 57600
--   - Clock: 100 MHz
--   - 8 data bits, no parity, 1 stop bit
--   - Single bidirectional TTL line
--   - TX enable asserted 1 bit period before transmission
--
-- AXI Stream Interface:
--   TX: Data to be transmitted to Dynamixel
--   RX: Data received from Dynamixel
--------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
entity dynamixel_uart is
generic (
CLK_FREQ    : integer := 100_000_000;  -- 100 MHz
BAUD_RATE   : integer := 57_600 -- 57600 baud
);
port (
-- Clock and Reset
clk         : in std_logic;
rst         : in std_logic;
-- Bidirectional data line
data_io     : inout std_logic;
-- AXI Stream TX (data to transmit)
tx_tdata    : in std_logic_vector(7 downto 0);
tx_tvalid   : in std_logic;
tx_tready   : out std_logic;
-- AXI Stream RX (data received)
rx_tdata    : out std_logic_vector(7 downto 0);
rx_tvalid   : out std_logic;
rx_tready   : in std_logic
);
end entity dynamixel_uart;
architecture rtl of dynamixel_uart is
-- Baud rate generator
constant CLKS_PER_BIT : integer := CLK_FREQ / BAUD_RATE;  -- 1736 for 100MHz/57600
-- TX State Machine
type tx_state_t is (TX_IDLE, TX_ENABLE_PRE, TX_START_BIT, TX_DATA_BITS, TX_STOP_BIT);
signal tx_state      : tx_state_t := TX_IDLE;
signal tx_clk_count  : integer range 0 to CLKS_PER_BIT-1 := 0;
signal tx_bit_index  : integer range 0 to 7 := 0;
signal tx_data_reg   : std_logic_vector(7 downto 0) := (others => '0');
signal tx_active     : std_logic := '0';
signal tx_out        : std_logic := '1';
signal tx_enable     : std_logic := '0';
-- RX State Machine
type rx_state_t is (RX_IDLE, RX_START_BIT, RX_DATA_BITS, RX_STOP_BIT);
signal rx_state      : rx_state_t := RX_IDLE;
signal rx_clk_count  : integer range 0 to CLKS_PER_BIT-1 := 0;
signal rx_bit_count  : integer range 0 to 8 := 0;
signal rx_data_reg   : std_logic_vector(7 downto 0) := (others => '0');
signal rx_data_out   : std_logic_vector(7 downto 0) := (others => '0');
signal rx_valid_reg  : std_logic := '0';
-- Data line control
signal data_in       : std_logic := '1';
signal data_in_sync  : std_logic := '1';
signal data_in_prev  : std_logic := '1';
begin
-- Bidirectional data line control
-- When transmitting, drive the line; otherwise, tristate for receiving
data_io <= tx_out when tx_enable = '1' else 'Z';
data_in <= data_io;
-- Synchronize input to avoid metastability
process(clk)
begin
if rising_edge(clk) then
data_in_sync <= data_in;
data_in_prev <= data_in_sync;
end if;
end process;
-------------------------------------------------------------------------
-- TX Process
-------------------------------------------------------------------------
process(clk)
begin
if rising_edge(clk) then
if rst = '1' then
tx_state     <= TX_IDLE;
tx_clk_count <= 0;
tx_bit_index <= 0;
tx_data_reg  <= (others => '0');
tx_active    <= '0';
tx_out       <= '1';
tx_enable    <= '0';
tx_tready    <= '1';
else
case tx_state is
when TX_IDLE =>
tx_out       <= '1';
tx_clk_count <= 0;
tx_bit_index <= 0;
tx_tready    <= '1';
tx_enable    <= '0';
if tx_tvalid = '1' and tx_active = '0' then
tx_data_reg  <= tx_tdata;
tx_active    <= '1';
tx_tready    <= '0';
-- Assert tx_enable 1 bit period before transmission
tx_enable    <= '1';
tx_state     <= TX_ENABLE_PRE;
end if;
when TX_ENABLE_PRE =>
-- Wait 1 bit period with tx_enable asserted before starting
tx_out <= '1';  -- Keep line high during pre-enable
if tx_clk_count < CLKS_PER_BIT - 1 then
tx_clk_count <= tx_clk_count + 1;
else
tx_clk_count <= 0;
tx_state     <= TX_START_BIT;
end if;
when TX_START_BIT =>
tx_out <= '0';  -- Start bit
if tx_clk_count < CLKS_PER_BIT - 1 then
tx_clk_count <= tx_clk_count + 1;
else
tx_clk_count <= 0;
tx_state     <= TX_DATA_BITS;
end if;
when TX_DATA_BITS =>
tx_out <= tx_data_reg(tx_bit_index);
if tx_clk_count < CLKS_PER_BIT - 1 then
tx_clk_count <= tx_clk_count + 1;
else
tx_clk_count <= 0;
if tx_bit_index < 7 then
tx_bit_index <= tx_bit_index + 1;
else
tx_bit_index <= 0;
tx_state     <= TX_STOP_BIT;
end if;
end if;
when TX_STOP_BIT =>
tx_out <= '1';  -- Stop bit
if tx_clk_count < CLKS_PER_BIT - 1 then
tx_clk_count <= tx_clk_count + 1;
else
tx_clk_count <= 0;
tx_enable    <= '0';  -- Release the line
tx_active    <= '0';
tx_state     <= TX_IDLE;
end if;
end case;
end if;
end if;
end process;
-------------------------------------------------------------------------
-- RX Process
-------------------------------------------------------------------------
process(clk)
begin
if rising_edge(clk) then
if rst = '1' then
rx_state     <= RX_IDLE;
rx_clk_count <= 0;
rx_bit_count <= 0;
rx_data_reg  <= (others => '0');
rx_data_out  <= (others => '0');
rx_valid_reg <= '0';
else
-- Default: clear rx_valid if data was read
if rx_tready = '1' and rx_valid_reg = '1' then
rx_valid_reg <= '0';
end if;
-- Only receive when not transmitting
if tx_enable = '0' then
case rx_state is
when RX_IDLE =>
rx_clk_count <= 0;
rx_bit_count <= 0;
-- Detect falling edge (start bit)
if data_in_prev = '1' and data_in_sync = '0' then
rx_state <= RX_START_BIT;
end if;
when RX_START_BIT =>
-- Wait for half bit period to get to middle of start bit
if rx_clk_count < (CLKS_PER_BIT / 2) - 1 then
rx_clk_count <= rx_clk_count + 1;
else
-- Move to data bits
rx_clk_count <= 0;
rx_bit_count <= 0;
rx_state     <= RX_DATA_BITS;
end if;
when RX_DATA_BITS =>
-- Wait full bit period
if rx_clk_count < CLKS_PER_BIT - 1 then
rx_clk_count <= rx_clk_count + 1;
-- Sample at middle of bit
if rx_clk_count = (CLKS_PER_BIT / 2) then
rx_data_reg(rx_bit_count) <= data_in_sync;
end if;
else
rx_clk_count <= 0;
if rx_bit_count = 7 then
rx_state <= RX_STOP_BIT;
else
rx_bit_count <= rx_bit_count + 1;
end if;
end if;
when RX_STOP_BIT =>
if rx_clk_count < CLKS_PER_BIT - 1 then
rx_clk_count <= rx_clk_count + 1;
else
rx_clk_count <= 0;
-- Verify stop bit and output data
if data_in_sync = '1' then
rx_data_out  <= rx_data_reg;
rx_valid_reg <= '1';
end if;
rx_state <= RX_IDLE;
end if;
end case;
end if;
end if;
end if;
end process;
-- Output assignments
rx_tdata  <= rx_data_out;
rx_tvalid <= rx_valid_reg;
end architecture rtl;

A test bench was also developed.

--------------------------------------------------------------------------------
-- Testbench for Dynamixel XL430-W250 UART PHY Layer Interface
--
-- Description:
--   Tests the half-duplex UART communication:
--   1. Sends multiple bytes to the Dynamixel
--   2. Dynamixel responds 500us after command completion
--   3. Receives response bytes
--------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use std.textio.all;
use ieee.std_logic_textio.all;
entity dynamixel_uart_tb is
end entity dynamixel_uart_tb;
architecture sim of dynamixel_uart_tb is
-- Function to convert std_logic_vector to hex string
function to_hex_string(slv : std_logic_vector(7 downto 0)) return string is
variable result : string(1 to 2);
variable nibble : integer;
constant hex_chars : string(1 to 16) := "0123456789ABCDEF";
begin
nibble := to_integer(unsigned(slv(7 downto 4)));
result(1) := hex_chars(nibble + 1);
nibble := to_integer(unsigned(slv(3 downto 0)));
result(2) := hex_chars(nibble + 1);
return result;
end function;
-- Constants
constant CLK_PERIOD   : time := 10 ns;  -- 100 MHz
constant BAUD_RATE    : integer := 57_600;
constant CLK_FREQ     : integer := 100_000_000;
constant BIT_PERIOD   : time := 1 sec / BAUD_RATE;  -- ~17.36 us
constant RESPONSE_DELAY : time := 500 us;
-- DUT signals
signal clk         : std_logic := '0';
signal rst         : std_logic := '0';
signal data_io     : std_logic := '1';  -- Strong pull-up for clean edges
signal tx_tdata    : std_logic_vector(7 downto 0) := (others => '0');
signal tx_tvalid   : std_logic := '0';
signal tx_tready   : std_logic;
signal rx_tdata    : std_logic_vector(7 downto 0);
signal rx_tvalid   : std_logic;
signal rx_tready   : std_logic := '1';
-- Testbench signals
signal test_done   : boolean := false;
signal servo_active : boolean := false;
-- Test data
type byte_array is array (natural range <>) of std_logic_vector(7 downto 0);
constant TX_TEST_DATA : byte_array(0 to 4) := (
x"FF",  -- Header 1
x"FF",  -- Header 2
x"FD",  -- Header 3
x"00",  -- Reserved
x"01" -- ID
);
constant RX_RESPONSE_DATA : byte_array(0 to 3) := (
x"AA",  -- Response byte 1
x"BB",  -- Response byte 2
x"CC",  -- Response byte 3
x"DD" -- Response byte 4
);
begin
-- Clock generation
clk <= not clk after CLK_PERIOD / 2 when not test_done else '0';
-- DUT instantiation
dut : entity work.dynamixel_uart
generic map (
CLK_FREQ  => CLK_FREQ,
BAUD_RATE => BAUD_RATE
)
port map (
clk       => clk,
rst       => rst,
data_io   => data_io,
tx_tdata  => tx_tdata,
tx_tvalid => tx_tvalid,
tx_tready => tx_tready,
rx_tdata  => rx_tdata,
rx_tvalid => rx_tvalid,
rx_tready => rx_tready
);
-------------------------------------------------------------------------
-- Dynamixel Servo Simulator Process
-- Simulates the servo receiving data and responding after 500us
-------------------------------------------------------------------------
servo_simulator : process
variable rx_byte : std_logic_vector(7 downto 0);
variable byte_count : integer;
variable l : line;
-- Procedure to receive a byte from the UART line
procedure uart_receive_byte(variable data : out std_logic_vector(7 downto 0)) is
begin
-- Wait for start bit (falling edge)
wait until data_io = '0';
wait for BIT_PERIOD / 2;  -- Move to middle of start bit
-- Verify start bit
assert data_io = '0' report "Invalid start bit" severity error;
-- Receive 8 data bits
for i in 0 to 7 loop
wait for BIT_PERIOD;
data(i) := data_io;
end loop;
-- Verify stop bit
wait for BIT_PERIOD;
assert data_io = '1' report "Invalid stop bit" severity error;
end procedure;
-- Procedure to transmit a byte on the UART line
procedure uart_transmit_byte(constant data : in std_logic_vector(7 downto 0)) is
begin
-- Ensure line is high before start bit
data_io <= '1';
wait for 0 ns;
-- Start bit
data_io <= '0';
wait for BIT_PERIOD;
-- Data bits
for i in 0 to 7 loop
data_io <= data(i);
wait for BIT_PERIOD;
end loop;
-- Stop bit
data_io <= '1';
wait for BIT_PERIOD;
end procedure;
begin
data_io <= 'Z';  -- Release bus for DUT to transmit
wait until rst = '0';
wait for 100 ns;
-- Receive command bytes from DUT
report "Servo: Waiting to receive command...";
byte_count := 0;
for i in TX_TEST_DATA'range loop
uart_receive_byte(rx_byte);
report "Servo received byte " & integer'image(i) & ": 0x" &
to_hex_string(rx_byte);
-- Verify received data
assert rx_byte = TX_TEST_DATA(i)
report "Servo received incorrect byte. Expected: 0x" &
to_hex_string(TX_TEST_DATA(i)) & " Got: 0x" & to_hex_string(rx_byte)
severity error;
byte_count := byte_count + 1;
end loop;
report "Servo: Command received. Waiting 500us before responding...";
wait for RESPONSE_DELAY;
-- Drive line high before transmitting
data_io <= '1';
wait for 10 ns;
-- Transmit response
report "Servo: Transmitting response...";
servo_active <= true;
for i in RX_RESPONSE_DATA'range loop
uart_transmit_byte(RX_RESPONSE_DATA(i));
report "Servo transmitted byte " & integer'image(i) & ": 0x" &
to_hex_string(RX_RESPONSE_DATA(i));
end loop;
servo_active <= false;
data_io <= '1';  -- Keep driving high after transmission
report "Servo: Response transmission complete";
wait;
end process;
-------------------------------------------------------------------------
-- Main Test Process
-------------------------------------------------------------------------
test_proc : process
variable rx_count : integer := 0;
begin
-- Reset
rst <= '1';
tx_tvalid <= '0';
rx_tready <= '1';
wait for CLK_PERIOD * 10;
rst <= '0';
wait for CLK_PERIOD * 5;
report "=== Starting Dynamixel UART Test ===";
-------------------------------------------------------------------------
-- Test 1: Transmit command to servo
-------------------------------------------------------------------------
report "Test 1: Transmitting command bytes to Dynamixel...";
for i in TX_TEST_DATA'range loop
-- Wait for ready
wait until tx_tready = '1' and rising_edge(clk);
-- Present data
tx_tdata <= TX_TEST_DATA(i);
tx_tvalid <= '1';
-- Wait for one clock cycle (handshake completes)
wait until rising_edge(clk);
tx_tvalid <= '0';
report "Transmitted byte " & integer'image(i) & ": 0x" &
to_hex_string(TX_TEST_DATA(i));
-- Wait for transmission to complete
wait until tx_tready = '1' and rising_edge(clk);
-- Small delay between bytes
wait for CLK_PERIOD * 10;
end loop;
report "Test 1: All command bytes transmitted";
-------------------------------------------------------------------------
-- Test 2: Receive response from servo
-------------------------------------------------------------------------
report "Test 2: Waiting for response from Dynamixel...";
rx_count := 0;
rx_tready <= '1';
while rx_count < RX_RESPONSE_DATA'length loop
wait until rising_edge(clk);
if rx_tvalid = '1' and rx_tready = '1' then
report "Received byte " & integer'image(rx_count) & ": 0x" &
to_hex_string(rx_tdata);
-- Verify received data
assert rx_tdata = RX_RESPONSE_DATA(rx_count)
report "Received incorrect byte. Expected: 0x" &
to_hex_string(RX_RESPONSE_DATA(rx_count)) &
" Got: 0x" & to_hex_string(rx_tdata)
severity error;
rx_count := rx_count + 1;
end if;
end loop;
report "Test 2: All response bytes received";
-------------------------------------------------------------------------
-- Test Complete
-------------------------------------------------------------------------
wait for 1 ms;
report "=== All Tests Passed Successfully! ===";
test_done <= true;
wait;
end process;
end architecture sim;

Running this simulation shows the transmission and reception of data.

Detail of the Transmission

Details of the Receive

With the UART working as expected the next step is to create a integrate it within a project.

Vivado Design

This design will be implemented using Vivado 2025.2 with its new theme, which I think is a great improvement.

The first thing we need to do is create a new project targeting the board we desire. For this project I am going to target a Arty S7-50 however we can target this to any AMD device as we are using a MicroBlaze V. I would expect more complex robotic solutions to deploy this perhaps with a K26 or K24 SoM or maybe Spartan UltraScale+.

The design is identical for all of AMD device with the exception of the IO, clock and reset allocation.

With the project created the next step is to create a IP Integrator diagram.

Onto this IP integrator design, add in a new MicroBlaze V

Run the block automation and configure the design as a microcontroller, with 32 KB of memory, debug enabled and peripheral AXI IP support.

Once the block automation is complete, from the boards tab drag and drop the USB UART on to the diagram.

Connect it into the system using the block automation

Reconfigure the MicroBlaze V to include the Streaming Links.

Add in the custom UART to the block diagram and connect it to the streaming links on the MicroBlaze. The final diagram should look as below.

On the Arty S7-50 the following XDC was used

set_property PACKAGE_PIN R2 [get_ports clk_in1_0]
set_property IOSTANDARD SSTL135 [get_ports clk_in1_0]
set_property PACKAGE_PIN P17 [get_ports data_io_0]
set_property IOSTANDARD LVCMOS33 [get_ports data_io_0]

set_property PULLTYPE PULLUP [get_ports data_io_0]

To ensure the communications line does not float the output of the FPGA has a pull up enabled.

Build the project, and you will notice the small footprint of the solution.

This means the MicroBlaze V can control multiple intelligent motors either daisy chained or connected to the another streaming link. This allows one MicroBlaze V to off load the control of several intelligent motors.

Now we need a software solution, export the XSA so that we can use it within the Vitis Unified Development Environment.

Vitis Development.

The software layer is what provides the overall control of the motor, for this application we will demonstrate the control of a single motor using a terminal running on the MicroBlaze V.

The end result, will look as below.

To do this there will be several layers of the application. The first is a simple API which enables the main application to control API functions which result in the motor being configured as determined by the higher level application.

The low level FSL driver and header can be seen below.

#include "dxl_fsl.h"
#include "fsl.h"
#include "sleep.h"
#include <stdint.h>
#include <stddef.h>
// Protocol 2.0 constants
#define DXL_HDR0 0xFF
#define DXL_HDR1 0xFF
#define DXL_HDR2 0xFD
#define DXL_RSV  0x00
#define DXL_INST_WRITE 0x03
static inline void dxl_tx_byte(uint8_t b)
{
putfslx((uint32_t)b, DXL_FSL_ID, DXL_FSL_CTRL);
usleep(DXL_INTERBYTE_US);
}
static void dxl_tx_buf(const uint8_t *buf, size_t len)
{
for (size_t i = 0; i < len; i++) {
dxl_tx_byte(buf[i]);
}
}
void dxl_init(void)
{
// Nothing required for Stream Link itself
}
// Correct Dynamixel Protocol 2.0 CRC-16 (poly 0x8005, init 0)
static uint16_t dxl_crc_update(uint16_t crc, const uint8_t *data, size_t len)
{
for (size_t j = 0; j < len; j++) {
crc ^= (uint16_t)data[j] << 8;
for (int i = 0; i < 8; i++) {
if (crc & 0x8000) crc = (uint16_t)((crc << 1) ^ 0x8005);
else             crc = (uint16_t)(crc << 1);
}
}
return crc;
}
static int dxl_write(uint8_t id, uint16_t addr, const uint8_t *data, size_t data_len)
{
// LENGTH = (2 + data_len) + 3 = data_len + 5
const uint16_t length = (uint16_t)(data_len + 5u);
uint8_t pkt[4 + 1 + 2 + 1 + 2 + 32 + 2];
if (data_len > 32) return -1;
size_t k = 0;
pkt[k++] = DXL_HDR0;
pkt[k++] = DXL_HDR1;
pkt[k++] = DXL_HDR2;
pkt[k++] = DXL_RSV;
pkt[k++] = id;
pkt[k++] = (uint8_t)(length & 0xFF);
pkt[k++] = (uint8_t)((length >> 8) & 0xFF);
pkt[k++] = DXL_INST_WRITE;
pkt[k++] = (uint8_t)(addr & 0xFF);
pkt[k++] = (uint8_t)((addr >> 8) & 0xFF);
for (size_t i = 0; i < data_len; i++) {
pkt[k++] = data[i];
}
uint16_t crc = dxl_crc_update(0, pkt, k);
pkt[k++] = (uint8_t)(crc & 0xFF);
pkt[k++] = (uint8_t)((crc >> 8) & 0xFF);
dxl_tx_buf(pkt, k);
return 0;
}
int dxl_write_u8(uint8_t id, uint16_t addr, uint8_t value)
{
return dxl_write(id, addr, &value, 1);
}
int dxl_write_u32(uint8_t id, uint16_t addr, uint32_t value)
{
uint8_t d[4] = {
(uint8_t)(value & 0xFF),
(uint8_t)((value >> 8) & 0xFF),
(uint8_t)((value >> 16) & 0xFF),
(uint8_t)((value >> 24) & 0xFF)
};
return dxl_write(id, addr, d, 4);
}
// ---- XL430 helpers ----
int xl430_set_led(uint8_t id, uint8_t on)
{
return dxl_write_u8(id, XL430_ADDR_LED, on ? 1u : 0u);
}
int xl430_set_torque(uint8_t id, uint8_t enable)
{
return dxl_write_u8(id, XL430_ADDR_TORQUE_ENABLE, enable ? 1u : 0u);
}
int xl430_set_operating_mode(uint8_t id, uint8_t mode)
{
return dxl_write_u8(id, XL430_ADDR_OPERATING_MODE, mode);
}
int xl430_set_profile(uint8_t id, uint32_t accel, uint32_t vel)
{
int rc = 0;
rc |= dxl_write_u32(id, XL430_ADDR_PROFILE_ACCEL, accel);
rc |= dxl_write_u32(id, XL430_ADDR_PROFILE_VELOCITY, vel);
return rc;
}
int xl430_set_goal_position(uint8_t id, uint32_t pos)
{
return dxl_write_u32(id, XL430_ADDR_GOAL_POSITION, pos);
}
// ---- Velocity (wheel-like) ----
int xl430_set_goal_velocity(uint8_t id, int32_t vel)
{
return dxl_write_u32(id, XL430_ADDR_GOAL_VELOCITY, (uint32_t)vel);
}
int xl430_config_velocity_mode(uint8_t id)
{
xl430_set_torque(id, 0);
usleep(20000);
xl430_set_operating_mode(id, XL430_MODE_VELOCITY);
usleep(50000);
xl430_set_torque(id, 1);
usleep(50000);
return 0;
}
// ---- Position mode ----
int xl430_config_position_mode(uint8_t id, uint32_t accel, uint32_t vel)
{
xl430_set_torque(id, 0);
usleep(20000);
xl430_set_operating_mode(id, XL430_MODE_POSITION);
usleep(20000);
xl430_set_profile(id, accel, vel);
usleep(20000);
xl430_set_torque(id, 1);
usleep(20000);
return 0;
}

Header file

#pragma once
#include <stdint.h>
#include <stddef.h>
#ifdef __cplusplus
extern "C" {
#endif
// --------------------
// FSL / Stream Link
// --------------------
#define DXL_FSL_ID        0
#define DXL_FSL_CTRL      0
#define DXL_INTERBYTE_US  200   // keep known-good (you can tighten later)
// --------------------
// XL430 Control Table
// --------------------
#define XL430_ADDR_OPERATING_MODE    11u   // 1 byte (EEPROM)
#define XL430_ADDR_TORQUE_ENABLE     64u   // 1 byte
#define XL430_ADDR_LED               65u   // 1 byte
#define XL430_ADDR_GOAL_VELOCITY     104u  // 4 bytes (signed)
#define XL430_ADDR_PROFILE_ACCEL     108u  // 4 bytes
#define XL430_ADDR_PROFILE_VELOCITY  112u  // 4 bytes
#define XL430_ADDR_GOAL_POSITION     116u  // 4 bytes
// Operating modes (X-series)
#define XL430_MODE_VELOCITY          1u
#define XL430_MODE_POSITION          3u
#define XL430_MODE_EXT_POSITION      4u
#define XL430_MODE_PWM               16u
#define DXL_BROADCAST_ID             254u
void dxl_init(void);
// Generic writes (TX-only)
int dxl_write_u8 (uint8_t id, uint16_t addr, uint8_t value);
int dxl_write_u32(uint8_t id, uint16_t addr, uint32_t value);
// XL430 helpers (TX-only)
int xl430_set_led(uint8_t id, uint8_t on);
int xl430_set_torque(uint8_t id, uint8_t enable);
int xl430_set_operating_mode(uint8_t id, uint8_t mode);
int xl430_set_profile(uint8_t id, uint32_t accel, uint32_t vel);
int xl430_set_goal_position(uint8_t id, uint32_t pos);
// Velocity (wheel-like continuous rotation)
int xl430_set_goal_velocity(uint8_t id, int32_t vel);
int xl430_config_velocity_mode(uint8_t id);
// Position setup
int xl430_config_position_mode(uint8_t id, uint32_t accel, uint32_t vel);
#ifdef __cplusplus
}
#endif

The main application can be seen below.

#include "dxl_fsl.h"
#include "xil_printf.h"
#include "sleep.h"
extern char inbyte(void);
static uint8_t target_id = 1;
static void print_menu(void)
{
xil_printf(
"\r\n==============================\r\n"
"  Dynamixel XL430 Terminal UI  \r\n"
"==============================\r\n"
"Target ID: %d\r\n"
"\r\n"
"1) LED ON\r\n"
"2) LED OFF\r\n"
"3) Blink LED (5x)\r\n"
"\r\n"
"4) Torque ENABLE\r\n"
"5) Torque DISABLE\r\n"
"\r\n"
"6) Configure POSITION mode (mode=3, profile, torque on)\r\n"
"7) Set Goal Position = 0\r\n"
"8) Set Goal Position = 2048\r\n"
"9) Set Goal Position = 4095\r\n"
"p) Set Goal Position (enter 0..4095)\r\n"
"m) Force Position Mode + Move test (0->2048->4095)\r\n"
"\r\n"
"v) Velocity mode: set constant velocity (+/-)\r\n"
"w) Velocity mode: oscillate forward/back (N cycles then return)\r\n"
"s) STOP (Goal Velocity = 0)\r\n"
"\r\n"
"i) Change Target ID\r\n"
"b) Broadcast LED blink (all motors)\r\n"
"h) Help\r\n"
"\r\n> ",
target_id
);
}
static uint32_t read_uint_line(void)
{
uint32_t v = 0;
char c;
while (1) {
c = inbyte();
if (c == '\r' || c == '\n') {
xil_printf("\r\n");
break;
}
if (c >= '0' && c <= '9') {
xil_printf("%c", c);
v = v * 10u + (uint32_t)(c - '0');
}
}
return v;
}
static int32_t read_int_line(void)
{
int sign = 1;
int32_t v = 0;
char c;
c = inbyte();
if (c == '-') { sign = -1; xil_printf("%c", c); }
else if (c == '+') { sign = 1; xil_printf("%c", c); }
else {
// first char might be a digit or newline
if (c >= '0' && c <= '9') {
xil_printf("%c", c);
v = (int32_t)(c - '0');
} else if (c == '\r' || c == '\n') {
xil_printf("\r\n");
return 0;
}
}
while (1) {
c = inbyte();
if (c == '\r' || c == '\n') {
xil_printf("\r\n");
break;
}
if (c >= '0' && c <= '9') {
xil_printf("%c", c);
v = (v * 10) + (int32_t)(c - '0');
}
}
return (int32_t)(sign * v);
}
static void blink_led(uint8_t id, int times)
{
for (int i = 0; i < times; i++) {
xl430_set_led(id, 1);
usleep(150000);
xl430_set_led(id, 0);
usleep(150000);
}
}
static void force_move_test(uint8_t id)
{
xil_printf("Force Position Mode + Move test on ID=%d\r\n", id);
xl430_set_torque(id, 0);
usleep(20000);
xl430_set_operating_mode(id, XL430_MODE_POSITION);
usleep(50000);
xl430_set_torque(id, 1);
usleep(50000);
xl430_set_goal_position(id, 0);
usleep(1500000);
xl430_set_goal_position(id, 2048);
usleep(1500000);
xl430_set_goal_position(id, 4095);
usleep(1500000);
xil_printf("Done.\r\n");
}
static void velocity_set_constant(uint8_t id)
{
xil_printf("Setting VELOCITY mode on ID=%d...\r\n", id);
xl430_config_velocity_mode(id);
xil_printf("Enter velocity (signed, e.g. +200 or -200, 0=stop): ");
int32_t vel = read_int_line();
// If you type 200 without sign, it still works
xl430_set_goal_velocity(id, vel);
xil_printf("Goal Velocity set to %ld\r\n", (long)vel);
}
static void velocity_oscillate_cycles(uint8_t id)
{
xil_printf("Setting VELOCITY mode on ID=%d...\r\n", id);
xl430_config_velocity_mode(id);
xil_printf("Enter speed (positive, e.g. 200): ");
uint32_t sp = read_uint_line();
if (sp == 0) sp = 200;
xil_printf("Enter dwell time ms (e.g. 500): ");
uint32_t dwell_ms = read_uint_line();
if (dwell_ms == 0) dwell_ms = 500;
xil_printf("Enter cycles (e.g. 5): ");
uint32_t cycles = read_uint_line();
if (cycles == 0) cycles = 5;
uint32_t dwell_us = dwell_ms * 1000u;
xil_printf("Oscillating %lu cycles: +%lu then -%lu, dwell %lu ms\r\n",
(unsigned long)cycles,
(unsigned long)sp,
(unsigned long)sp,
(unsigned long)dwell_ms);
for (uint32_t i = 0; i < cycles; i++) {
xl430_set_goal_velocity(id, (int32_t)sp);
usleep(dwell_us);
xl430_set_goal_velocity(id, -(int32_t)sp);
usleep(dwell_us);
}
// Always stop at end so you “recover” to menu safely
xl430_set_goal_velocity(id, 0);
usleep(20000);
xil_printf("Done. Stopped.\r\n");
}
int main(void)
{
dxl_init();
print_menu();
while (1) {
char c = inbyte();
xil_printf("%c\r\n", c);
switch (c) {
case '1': xl430_set_led(target_id, 1); break;
case '2': xl430_set_led(target_id, 0); break;
case '3': blink_led(target_id, 5); break;
case '4': xl430_set_torque(target_id, 1); break;
case '5': xl430_set_torque(target_id, 0); break;
case '6': {
uint32_t accel = 50;
uint32_t vel   = 200;
xil_printf("Configuring POSITION mode on ID=%d\r\n", target_id);
xl430_config_position_mode(target_id, accel, vel);
xil_printf("Done.\r\n");
} break;
case '7': xl430_set_goal_position(target_id, 0); break;
case '8': xl430_set_goal_position(target_id, 2048); break;
case '9': xl430_set_goal_position(target_id, 4095); break;
case 'p': {
xil_printf("Enter Goal Position (0..4095): ");
uint32_t pos = read_uint_line();
if (pos > 4095u) pos = 4095u;
xl430_set_goal_position(target_id, pos);
} break;
case 'm': force_move_test(target_id); break;
case 'v': velocity_set_constant(target_id); break;
case 'w': velocity_oscillate_cycles(target_id); break;
case 's':
xl430_set_goal_velocity(target_id, 0);
xil_printf("STOP: Goal Velocity = 0\r\n");
break;
case 'i': {
xil_printf("Enter new Target ID (0..252): ");
uint32_t nid = read_uint_line();
if (nid <= 252u) target_id = (uint8_t)nid;
else xil_printf("Invalid ID\r\n");
} break;
case 'b': blink_led(DXL_BROADCAST_ID, 3); break;
case 'h':
case '?':
print_menu();
continue;
default:
xil_printf("Unknown. Press 'h' for help.\r\n");
break;
}
xil_printf("> ");
}
}

Integration

Running this application on the board connected to a motor using a Pmod LVLSHFT to give a 3v3 to 5V translation.

We can see the different command being sent into the intelligent motor.

Using the terminal we are able to command functions of the motor from flashing its LED to driving the motor and in this case the wheel connected to it.

We can see the motor working under the control of the MicroBlaze V, providing a simple intelligent motor interface. We can scale this to several motors as the application demands.

Wrap UP

This project has shown we can create a simple Intelligent Motor controller using a MicroBlaze V which is capable of supporting multiple intelligent motors. We can integrate this within any AMD FPGA or SoC enabling a tighter integrated solution and reduced BoM, along with enabling offloading of processing solutions from dedicated processing cores such as A9, A53, A72 or R5 / R5F processors.