A Rust driver for the LX-16A Servo
Introduction
There’s this adage that once you get Rust code to compile, it works. That actually happened to me as I was toying with a new driver for the LX-16A Bus Servo.
I was working on hobby project last weekend, a robot rover based on Roger Cheng’s SAWPPY Rover. And I reached the stage where all the drive and steering servos were wired up.
Eager to test out the new wiring harness and get things moving, I downloaded an LX-16A interface crate and modified the example code to move each of the servos. They worked. Yay!
But when I moved on to writing the real control code for my new rover, I ran into some issues. Long story short, I ended up writing a new servo driver. Here’s the background and my approach to an LX-16A driver.
I’m going to start with a brief overview of the LX-16A bus servo, for those unfamiliar with this creature. Then I’ll describe the difficulties I had using the existing crate. And, finally, I want to describe how the new crate addresses the problem in a robust way.
Note: If you find robotics and embedded rust interesting, follow me, DaveOfThings on Bluesky for updates on this project.
The LX-16A Servo
The LX-16A servo uses a serial bus protocol to provide communications between the servo and a controller.
This is fundamentally different from most servos that use a PWM signal to set their position. The serial bus supports a richer feature set, allows bidirectional data exchange and enables multiple servos to be controlled over a single UART port from the host.
The LX-16A isn’t the only bus servo on the market. It’s one of several designs, each with their own proprietary protocol. The LX-16A protocol is well documented, though, and it’s simple enough to tackle in a weekend.
LX-16A Control with Rust
Since I’m hoping to develop this rover’s control code in Rust, I was pleased to find an LX-16A crate on crates.io. That made it really easy to get the servos moving as I tested the new wiring harness. The examples in that crate enabled me to set each servo’s id and move each one individually.
It was when I started writing the broader control program that I ran into issues.
The Mutable Reference
The robot uses 10 servos. Four are for steering and six drive the wheels. But the LX-16a library’s Servo struct required a mutable reference to its associated bus:
impl<'a> Servo<'a> {
/// Construct a new handle for servo `id` on the provided `bus`.
pub fn new(id: ServoID, bus: &'a mut ServoBus) -> Self { Self { id, bus } }
...
}
And Rust famously allows only one mutable reference to a struct to exist at a given time. That would mean I can only create one servo! Perhaps this isn’t catastrophic. Servos structs are pretty lightweight, containing just that reference and an id number for the associated device. I could just call one into existence every time I needed one, then drop it. But, no, I would prefer to organize my rover’s code so the bus and all the servos are created at startup and referenced by other components like wheel modules and the whole drive train.
Bus Coherence
One challenge when writing an interface to the LX-16A is maintaining bus coherence. Some writes to the servos will result in responses that need to be received before the next write can proceed. These are, effectively, transactions, that can’t be interrupted by competing communications on the bus. The bus is actually half-duplex, with the direction of communications changing mid transaction. Trying to overlap operations to multiple servos could result in lost messages in both directions if care isn’t taken.
With the lx16a-servo crate, this coherence was supported via that single mutable reference to the bus. Since only one execution path can own it, only that transaction uses the bus. If I wanted to organize my code to represent servos independently, I would need to ensure mutual exclusion for bus transactions. This isn’t unusual for other types of busses, like an I2C bus, for example. But a shared serial bus is less common.
Based on serialport::SerialPort
The lx16a_servo crate is based on the serialport::SerialPort trait. This is an overly broad trait for the bus servo to depend on. Many of the operations of that trait are never used. This is fine for host systems like PCs or a Raspberry Pi. But I was hoping to use some lower level microprocessors like the ESP32 and write my control code in a no-std environment. If I want to port my code to that platform, I don’t want to provide (or stub out) extraneous methods for no reason.
I’d prefer the LX-16A driver just rely on the std::io:Read and std::io::Write traits. (Or some no-std alternatives.)
A new crate: lx_16a
Wondering if it would be feasible to write a new crate that met my needs, I started with structure representing the bus as a whole.
pub struct Lx16aBus<T: Read+Write> {
port: Mutex<T>,
}
This eliminates the dependency on serialport::SerialPort and instead uses the std::io::Read and std::io::Write traits. (But see issues, below.)
It also introduces a Mutex that will be the basis for coherent bus access by multiple servos.
The bus, then, acts as a factory to create Lx16a structs, representing servos. And there are private methods, accessible to the Lx16a implementation, that provide mutex-protected write and write_read transactions.
impl<'a, T: Read+Write> Lx16aBus<T> {
// Create an Lx16a bus from anything that implements Read+Write
pub fn new(port: T) -> Lx16aBus<T> {
Lx16aBus { port: Mutex::new(port) }
}
// Get an individual servo
pub fn servo(&'a self, id: u8) -> Lx16a<'a, T> {
Lx16a::new(id, self)
}
// Write data
fn write(&self, out_data: &[u8]) -> Result<(), Error> {
// Get exclusive access to the bus
let mut port = self.port.lock().unwrap();
// write the request
port.write(out_data)?;
port.flush()?;
Ok(())
}
// Write data then read expected response.
fn write_read(&self, out_data: &[u8], rx_data: &mut[u8]) -> Result<usize, Error> {
// Get exclusive access to the bus
let mut port = self.port.lock().unwrap();
// write the request
port.write(out_data)?;
port.flush()?;
// read the response
port.read_exact(rx_data)?;
Ok(rx_data.len())
}
}
An Lx16a, then, consists of an id number and a reference to this bus:
// Represents one LX-16A servo
pub struct Lx16a<'a, T: Read+Write> {
id: u8,
bus: &'a Lx16aBus<T>,
}
The implementation of an Lx16a provides methods for each of the LX-16A operations. The first two I coded up were move_time(), to move a servo, and read_servo_id(), to perform a read transaction.
impl<'a, T: Read+Write> Lx16a<'a, T> {
// Private new method, used by Lx16aBus.
// To create a servo, first create an Lx16aBus, then use servo() factory method.
fn new(id: u8, bus: &'a Lx16aBus<T>) -> Lx16a<'a, T> {
Lx16a { id, bus }
}
// --- Public operations corresponding to servo commands
// Command the servo to move to position <pos> in <time_ms> milliseconds.
pub fn move_time(&self, pos: i16, time_ms: u16) -> Result<(), Error> {
const SERVO_MOVE_TIME_WRITE: u8 = 1;
let mut params = [0; 4];
LittleEndian::write_i16(&mut params[0..2], pos);
LittleEndian::write_u16(&mut params[2..4], time_ms);
self.write(self.id, SERVO_MOVE_TIME_WRITE, ¶ms)?;
Ok(())
}
pub fn read_servo_id(&self) -> Result<u8, Error> {
const SERVO_ID_READ: u8 = 14;
let mut rx_buf = [0; 32];
let params = [];
let response = self.read(self.id, SERVO_ID_READ, ¶ms, &mut rx_buf, 1)?;
Ok(response[0])
}
...
}
Private utility methods write() and read() in the Lx16a implementation take care of formatting packets, computing the checksums and validating response fields. They use the write() and write_read() methods of the underlying bus to actually send and receive packets.
impl<'a, T: Read+Write> Lx16a<'a, T> {
...
// Write command packet with the given parameters
fn write(&self, id: u8, cmd: u8, params: &[u8]) -> Result<(), Error> {
let mut tx_data: [u8; 32] = [0; 32];
let tx_data_len = Self::form_packet(&mut tx_data, id, cmd, params);
self.bus.write(&tx_data[0..tx_data_len])?;
Ok(())
}
// Do a write/read transaction with the given command and parameters.
// params_len specifies the number of parameter bytes expected in the response.
// The provided rx_buf must be large enough to accomodate the whole received packet (params_len + 6)
fn read(&self, id: u8, cmd: u8, params: &[u8], rx_buf: &'a mut[u8], params_len: usize) -> Result<&'a [u8], Error> {
let mut tx_data: [u8; 32] = [0; 32];
let tx_data_len = Self::form_packet(&mut tx_data, id, cmd, params);
let resp_len = params_len + 6;
self.bus.write_read(&tx_data[0..tx_data_len], &mut rx_buf[0..resp_len])?;
// Validate response: header, id, length, cmd, checksum
if Self::checksum(&rx_buf[2..resp_len]) != 0_u8 {
Err(Error::new(InvalidData, "Checksum error"))
}
else if (rx_buf[0] != 0x55) ||
(rx_buf[1] != 0x55) ||
(rx_buf[2] != id) ||
(rx_buf[3] != 3_u8 + params_len as u8) ||
(rx_buf[4] != cmd) {
Err(Error::new(InvalidData, "Bad response"))
}
else {
Ok(&rx_buf[5..5+params_len])
}
}
}
Having written that much, I wanted to see if this might actually work, so I wrote a simple main function to try it out.
const SERIAL_PORT: &str = "/dev/ttyUSB0";
const BAUD: u32 = 115200;
const RIGHT_FRONT_STEER_ID: u8 = 1;
const RIGHT_BACK_STEER_ID: u8 = 4;
const LEFT_BACK_STEER_ID: u8 = 6;
const LEFT_FRONT_STEER_ID: u8 = 9;
fn main() -> anyhow::Result<()> {
let port = serialport::new(SERIAL_PORT, BAUD)
.timeout(Duration::from_millis(10))
.open()
.expect("Failed to open port");
let lx16a_bus = Lx16aBus::new(port);
let right_front = lx16a_bus.servo(RIGHT_FRONT_STEER_ID);
let right_back = lx16a_bus.servo(RIGHT_BACK_STEER_ID);
let left_back = lx16a_bus.servo(LEFT_BACK_STEER_ID);
let left_front = lx16a_bus.servo(LEFT_FRONT_STEER_ID);
println!("Hello LX-16A world.");
for servo in [&right_front, &right_back, &left_back, &left_front] {
let read_id = servo.read_servo_id().unwrap();
println!("id : {read_id}");
}
This was the point where I had to work through numerous compile issues, of course. But, when it all built, it ran and read back the servo ids from all four units:
Hello LX-16A world.
id : 1
id : 4
id : 6
id : 9
I was expecting to spend a few more hours troubleshooting serial communications and mutex access problems, but it seemed the Rust-style access controls made this bullet-proof on the first run.
(Of course, getting the packet format and checksum calculations exactly right was just good luck.)
Next steps
Building on that success, I went ahead and implemented the full LX-16A command set. I’m planning to work this up into a full Rust crate then publish it on crates.io when it’s ready. For now it’s available as a github repository. It will need more work to add documentation and test cases. And it might need a bit of refactoring to address some issues I’m already seeing with it.
Yes, I said it. I already see some things that need rework.
Generic Signatures escape Confinement
In the first design steps, I made the Lx16ABus struct generic, taking a type T that implements Read and Write.
pub struct Lx16aBus<T: Read+Write> {
port: Mutex<T>,
}
This felt like a good choice. Instead of using serialport::SerialPort, we can make a bus out of any serial port implementation that provides those standard traits. But that led to the servo struct, Lx16a, having a generic type as well. And the rover’s WheelModule struct, which used the servos, needed a generic declaration, as did the Drive struct and the Rover struct itself. That generic type signature sort of escaped confinement and polluted all the upstream structures.
I’m sure there’s a better way to handle this so the lx_16a driver doesn’t affect every other struct that uses it.
Support in no-std
Second, I really want to use this driver in a no-std environment. I develop a lot of embedded systems and I’d like to run my rover’s control system on an ESP32. But without std, I can’t use the std::io::Read and std::io::Write traits. I’m going to have to find the right alternatives and work those into this driver before I can break free of the Raspberry Pi controller, after all.
To be continued
For now, I’m going to live with things as they are, though. I need to focus, instead, on some mechanical issues as I get the electronics mounted in their new mobile platform.
Thanks for taking an interest in my project. You can follow me, DaveOfThings at Bluesky, for updates.