Dynamixel Servos in Grbl_ESP32

Dynamixel Servos

Dynamixel servos are a series of robot servos from Robotis. They are controlled using a daisy chained serial port with a 3 or 4 wire cable. Each servo has 2 connectors, so you can link one servo to the next.

The servos I used are XL430_250T. These use a 3 wire cable. (Power, Ground and Data). The data is a 5V TTL signal. This allows you to connect directly to a microcontroller’s UART in a half duplex setup. Half duplex means you send a command, give control of the data line to the servo and wait for a response. Each servo has a unique address. The basic specs of the XL430-250T are below.

  • Rotation: 360° (or continuous)
  • Resolution: 4096 count contactless absolute encoder
  • Torque: 1.5 n-m (212 oz-in)
  • Speed: 61 RPM max.
  • Voltage: 6.5V-12V
  • Price: $49 USD

ESP32 Half Duplex Mode

ESP32 has a basic half duplex mode. It still uses a separate TX and RX pin. It also requires a data direction pin. The direction pin goes high when you transmit and then goes low after all characters are sent. The servos are now free to use the data line.

Here is what that looks like. The second channel is the data pin on the servos. It has both TX and RX data. The last channel is the direction pin. You can see going high while the ESP32 is transmitting.

This requires some external hardware to combine the TX and RX lines. I used a 74LS241. This chip is a little overkill, but is cheap and easy to solder. It has tri-state drivers where half of them have an enable and the other have a not enable. This allows the direction pin to enable half of the gates and disable the other half. Here is what the circuit looks like.

The ESP32 has 3.3V I/O and the servos use 5V. The 74LS241 is OK with the 3.3V signal, but the 5V RX out of the 74LS241 needs to be dropped down to 3.3V. I used a simple voltage divider to do this. Here is a snippet of my schematic.

My Controller

Here is an image of the controller I designed. The capacitor on the ESP32 fixes the bootloader problem many ESP32 have.

  • ESP32 Dev Module Socket (extra connectors for testing)
  • 74LS241 This is the large IC to the right of the ESP32
  • Power Connector This is the main voltage for the servos.
  • Dynamixel Connector. White connector in the upper right.
    • Extra stuff (not needed for Dynamixels)
      • 5V DC-DC P/S. This is the circuit in the lower right.
      • (3) Hobby Servo connectors along the right edge
      • (3) High current MOSFETs for relays, solenoids, etc along the top edge.
      • (6) Switch inputs along the left edge.
      • I2C connector for a display at the top left.
      • SD Card under the ESP32

ESP32 UART Setup

Here is the code you need to setup the UART on the ESP32.

#include "driver/uart.h" 

#define DYNAMIXEL_TXD           GPIO_NUM_4
#define DYNAMIXEL_RXD           GPIO_NUM_13
#define DYNAMIXEL_RTS           GPIO_NUM_17
#define DXL_BUF_SIZE            127
#define DXL_BAUD_RATE           57600 // 57600 is the XL430 default
#define DXL_RESPONSE_WAIT_TICKS 20 // how long to wait for a response

 // setup the comm port as half duplex
uart_config_t uart_config = {
	.baud_rate = DXL_BAUD_RATE,
	.data_bits = UART_DATA_8_BITS,
	.stop_bits = UART_STOP_BITS_1,
	.rx_flow_ctrl_thresh = 122,

// Configure UART parameters    
uart_param_config(uart_num, &uart_config);
uart_driver_install(uart_num, DXL_BUF_SIZE * 2, 0, 0, NULL, 0);
uart_set_mode(uart_num, UART_MODE_RS485_HALF_DUPLEX);

ESP32 Communications

My servos use Robotis’ protocol 2. I won’t full go into the implementation here, but you can look at my code. The protocol uses a few header bytes, a servo address, some data and a CRC. The packet of byte is first sent. You need to wait for the servo to respond before you send another packet or the 2 sides might try to send at the same time.

With a real time motion controller, waiting cannot be done with basic delays. Fortunately the ESP32 uses an RTOS, so there is a way to safely wait for a specific length of data to be received. Here is some basic code on how to do that.

// transmit the packet
uart_write_bytes(uart_num, msg, msg_len);

// wait for expected number of characters
rx_length = uart_read_bytes(DYNAMIXEL_UART_PORT, 
			dxl_rx_message, resp_length, 

// validate the response length and process the response					
if (rx_length == 0) {
	// Error no response
} else if (rx_length != resp_length) {
	// Error wrong response length
} else {
	// process response

Grbl_ESP32 Integration

Grbl_ESP32 runs virtual (no I/O) stepper motors for a 3 axis machine. A 33Hz (rate adjustable) task is setup to sync the servo to those axes. Each time the task runs it sends a position target to the servos based on where the virtual axes are at that time.

If the virtual stepper motors are disabled, the servo torque is turned off. This allows the servos to be moved manually. Each time the task runs it will query the position of the servos and update Grbl_ESP32. The XYZ numbers on your GUI will update as the servos are moved. When the steppers are enabled, the servo torque is turned back on locking them into their current location.

Example Project

The Dynamixel integration was done for a delta project. Here is a video of that machine. The motion is quite smooth and reasonably accurate. The servos only use half of their travel. This means the resolution is 2048 counts and that is mapped over an work area of about 200mm x 200mm x 150mm

TO DO List

  • Wifi, Bluetooth Issues: Currently the servo communication uses the same core as the Wifi and Bluetooth. When I have either of those on, they interfere with the servos. I hope a little tweaking/tuning will fix this.
  • Better Grbl_ESP32 Integration. Allow you to easily select the use of Dynamixels servos for any axis via the cpu map file, like is done with steppers and PWM servos.
  • Smaller Chip. The 74LS241 is huge. I noticed the Dynamixel shield uses a tiny little NC7WZ241 chip that has the perfect number of gates.
Share and Enjoy:
  • Print
  • Digg
  • StumbleUpon
  • del.icio.us
  • Facebook
  • Yahoo! Buzz
  • Twitter
  • Google Bookmarks

1 Response to “Dynamixel Servos in Grbl_ESP32”

  1. Bob

    Any plans to make a kit of this board available on tindie? Seems like a really fun machine!

    What are you planning to use this machine for?