diff --git a/Changelog.md b/Changelog.md index fe49bc0..c1a24ae 100644 --- a/Changelog.md +++ b/Changelog.md @@ -23,6 +23,31 @@ `ServoDefinition`; it fails the build on a servo without a `lock` register. - Python: `Bus.is_open`, and `Bus` as a context manager: `with Bus(...) as bus:` releases the serial port on the way out, an exception raised in the block included. +- `DynamixelProtocolHandler::broadcast_ping` pings every motor at once and returns the ids + that answer, listening for `broadcast_ping_window(baudrate)`: the status packets of + every possible id, 3 ms of turn per id and 16 ms for a USB adapter's latency timer, the + window the vendor SDKs use (about 0.8 s at 1 Mbps). Protocol v2 Dynamixel and Feetech + STS answer it; protocol v1 Dynamixel and Feetech SCS do not. +- `broadcast_scan(ids)` / `broadcast_scan_all()` on `ServoDefinition`, the controllers and + `Bus` (`broadcast_scan(ids=None)` on Python): which ids answer, found with one broadcast + ping, then the model number of each id asked that answered, read under the port's own + timeout. The ping's window does not shrink with the number of ids, so `scan` stays the + fast way to check a few ids and is unchanged; `broadcast_scan` is for sweeping many, + and for USB adapters whose latency timer outlasts `scan`'s short timeout. Servos state + `supports_broadcast_ping: true` (STS3215, XL320, XL330, XL430) on `ServoInfo`, the + Python controller classes and `ServoDefinition`; on any other, `broadcast_scan` is a + `RegisterError::BroadcastPing` (`ValueError` on Python) and nothing is sent. +- `change_id(definition, id, new_id)` and `change_baudrate(definition, id, baudrate)` on + `ServoDefinition` and `Bus` (and the Python `Bus`): torque off and, on servos with a + lock register, lock open, then the write. The baud rate is given in bauds and written + as the value the definition's `baudrates` lists for it; a rate it does not list is a + `RegisterError::Baudrate` (`ValueError` on Python) and nothing is sent; the port stays + at its rate, for the caller to switch with `set_baudrate`. Like `scan`, both reach + motors the bus does not have. +- Python: `baudrates()` (slowest first) and `models()` (by name) on the controller classes + return their dict in a fixed order, like the `ServoDefinition` getters. They came from + a `HashMap`, so the order changed from one process to the next, and so did the order in + which a caller trying each baud rate found a motor. ## Version 1.10.0 diff --git a/README.md b/README.md index d8faf2b..ed67fd6 100644 --- a/README.md +++ b/README.md @@ -28,9 +28,10 @@ If you want to quickly setup/check motors configuration, you can directly use a ## APIs -It exposes two APIs: +It exposes three APIs, from the wire up: * `DynamixelProtocolHandler`: low-level API. It handles the serial communication and the Dynamixel protocol parsing. It can be used for fine-grained control of the shared bus with other communication. -* `Controller`: high-level API for the Dynamixel protocol. Simpler and cleaner API but it takes full ownership of the io (it can still be shared if wrapped with a mutex for instance). +* `Controller`: one per servo model, with a typed accessor for each register. Simpler and cleaner API but it takes full ownership of the io (it can still be shared if wrapped with a mutex for instance). +* `Bus` and `ServoDefinition`: a higher-level, family-agnostic API, designed mostly for the [LeRobot](https://github.com/huggingface/lerobot) library. Motors of several models and of both protocols on one port, registers by name, the facts about each servo, finding and setting up motors, and the torque of a whole bus. Some of its choices are LeRobot conventions rather than vendor rules; the controllers stay the per-servo API underneath. See the examples below for usage. @@ -207,7 +208,9 @@ info.resolution(), info.word_order() # (4096, 'little') info.register("present_position").sign_bit # 15 ``` -### Mixed buses +### Mixed buses, and the LeRobot-oriented API + +`Bus` and `ServoDefinition` are the higher-level, family-agnostic API (see [APIs](#apis)), designed mostly for the [LeRobot](https://github.com/huggingface/lerobot) library; the controllers above stay the per-servo API. A controller speaks one servo definition. When a port carries motors of several definitions, or of both protocols (Reachy Mini has STS3215 motors on protocol v1 and XL330 motors on v2 on one port), open a `Bus` with the definition of each motor: @@ -226,6 +229,15 @@ found = bus.scan(Xl330PyController.definition()) # the v2 motors, as {id: model Each motor is read and written through its own definition and protocol. A sync read or write sends one instruction per group of motors sharing a protocol and the register's address and size, and hands the values back in the order asked: here, one Sync Read for motor 11 and one for motors 1 and 2. A group with a motor that has no Sync Read is read one id at a time. +Beyond register access, they carry what a library driving several motor families needs: + +- `find_model("xl330-m288")` returns the model number and the definition, whose getters give the servo's facts (`resolution`, `baudrates`, `factory_baudrate`, `homing_offset_sign`, `operating_modes`, ...); +- `bus.broadcast_scan(definition)` finds the motors on the port with one broadcast ping, and `bus.change_id` / `bus.change_baudrate` set a new motor up; +- `bus.set_torque(ids, enabled)` turns the torque of every motor on or off, and reports the ones that failed; +- `with Bus(...) as bus:` releases the port on the way out. + +Some of these follow LeRobot's conventions rather than a vendor's: `set_torque` closes the lock of a Feetech servo when the torque goes on and opens it when it goes off. + In Rust the same access is on each definition, taking the protocol handler and the port like the typed module functions: `sts3215::DEFINITION.read_register(&dph_v1, port, 11, "present_position")`. `servo::definition::sync_read_register` and `sync_write_register` reach motors of several definitions sharing a protocol and a layout in one instruction, and `bus::Bus` groups them for a whole bus. ### Threading and the GIL diff --git a/src/bus.rs b/src/bus.rs index 72cd87a..7d174be 100644 --- a/src/bus.rs +++ b/src/bus.rs @@ -5,6 +5,12 @@ //! STS3215 (protocol v1) and XL330 (protocol v2) motors on one port. A [`Bus`] holds the //! port, a handler for each protocol and the definition of each motor, and calls the //! definition's functions with the handler that matches it. +//! +//! With [`ServoDefinition`], it is rustypot's higher-level, family-agnostic API, designed +//! mostly for the LeRobot library: registers by name, the facts about each servo, finding +//! and setting up motors, the torque of a whole bus. Some of its choices are LeRobot +//! conventions rather than vendor rules, such as the Feetech lock following the torque in +//! [`Bus::set_torque`]. The per-servo controllers stay the API underneath. use std::collections::BTreeMap; use std::time::Duration; @@ -193,6 +199,24 @@ impl Bus { self.scan(definition, &ids) } + /// Which of `ids` answer, with their model number read as `definition` lays it out: + /// one broadcast ping, then a read of each id asked that answered. See + /// [`ServoDefinition::broadcast_scan`]. + pub fn broadcast_scan( + &mut self, + definition: ServoDefinition, + ids: &[u8], + ) -> Result> { + let dph = &self.handlers[definition.protocol as usize - 1]; + definition.broadcast_scan(dph, self.serial_port.as_mut(), ids) + } + + /// [`broadcast_scan`](Self::broadcast_scan) every id `definition`'s protocol allows. + pub fn broadcast_scan_all(&mut self, definition: ServoDefinition) -> Result> { + let ids: Vec = (0..=self.handlers[definition.protocol as usize - 1].max_id()).collect(); + self.broadcast_scan(definition, &ids) + } + /// Turn the torque of `ids` on or off. Every motor is tried, even after one fails, /// and a failure on the bus is tried again up to `retries` more times; the motors /// that still failed come back with their error. @@ -224,6 +248,27 @@ impl Bus { failed } + /// Give motor `id` the id `new_id`, through `definition`. See + /// [`ServoDefinition::change_id`]. Like [`scan`](Self::scan), it reaches motors the + /// bus does not have: a new motor answers at its factory id. + pub fn change_id(&mut self, definition: ServoDefinition, id: u8, new_id: u8) -> Result<()> { + let dph = &self.handlers[definition.protocol as usize - 1]; + definition.change_id(dph, self.serial_port.as_mut(), id, new_id) + } + + /// Set motor `id` to talk at `baudrate`, through `definition`. See + /// [`ServoDefinition::change_baudrate`]. The bus's port stays at the old rate: call + /// [`set_baudrate`](Self::set_baudrate) to keep talking to the motor afterwards. + pub fn change_baudrate( + &mut self, + definition: ServoDefinition, + id: u8, + baudrate: u32, + ) -> Result<()> { + let dph = &self.handlers[definition.protocol as usize - 1]; + definition.change_baudrate(dph, self.serial_port.as_mut(), id, baudrate) + } + /// Switch the open serial port to `baudrate`. pub fn set_baudrate(&mut self, baudrate: u32) -> Result<()> { Ok(self.serial_port.set_baud_rate(baudrate)?) @@ -257,7 +302,10 @@ mod python { use super::Bus; use crate::servo::{RegisterError, ServoDefinition}; - /// Motors of several definitions, and of both protocols, on one serial port. + /// Motors of several definitions, and of both protocols, on one serial port: the + /// higher-level, family-agnostic API, designed mostly for the LeRobot library, where + /// the controller classes are the per-servo one. Some of its choices are LeRobot + /// conventions, such as the Feetech lock following the torque in `set_torque`. /// /// ```python /// with Bus("/dev/ttyUSB0", 1_000_000, 0.1, { @@ -487,6 +535,32 @@ mod python { }) } + /// Give motor `id` the id `new_id`, through `definition`: torque off and, on a + /// Feetech motor, lock open first. The motor need not be one of the bus's. + pub fn change_id( + &self, + py: Python, + definition: ServoDefinition, + id: u8, + new_id: u8, + ) -> PyResult<()> { + self.run(py, |bus| bus.change_id(definition, id, new_id)) + } + + /// Set motor `id` to talk at `baudrate`, through `definition`, as `change_id` + /// does it. A rate the servo cannot take raises `ValueError`. The bus's port stays + /// at the old rate: call `set_baudrate(baudrate)` to keep talking to the motor + /// afterwards. + pub fn change_baudrate( + &self, + py: Python, + definition: ServoDefinition, + id: u8, + baudrate: u32, + ) -> PyResult<()> { + self.run(py, |bus| bus.change_baudrate(definition, id, baudrate)) + } + /// Which of `ids` answer, as {id: model number} read as `definition` lays it out, /// over its protocol; every id that protocol allows when `ids` is left out. #[pyo3(signature = (definition, ids = None))] @@ -501,6 +575,26 @@ mod python { None => bus.scan_all(definition), }) } + + /// Which of `ids` answer, as {id: model number} read as `definition` lays it out; + /// every id its protocol allows when `ids` is left out. One broadcast ping, which + /// listens for its whole window (about 0.8 s at 1 Mbps) however few ids are asked, + /// then a read of each id asked that answered: faster than `scan` over many ids, + /// and safe behind a USB adapter with a long latency timer. A servo that does not + /// answer a broadcast ping (`definition.supports_broadcast_ping`) raises + /// `ValueError`. + #[pyo3(signature = (definition, ids = None))] + pub fn broadcast_scan( + &self, + py: Python, + definition: ServoDefinition, + ids: Option>, + ) -> PyResult> { + self.run(py, |bus| match &ids { + Some(ids) => bus.broadcast_scan(definition, ids), + None => bus.broadcast_scan_all(definition), + }) + } } } @@ -592,6 +686,22 @@ mod tests { assert_eq!([written[1][5], written[1][6]], [55, 1]); } + #[test] + fn a_motor_the_bus_does_not_have_can_be_given_its_id() { + // A new STS3215 answers at id 1, which the bus gives to an XL330. + let status = vec![0xFF, 0xFF, 0x01, 0x02, 0x00, 0xFC]; + let port = FakePort::new(vec![status.clone(), status.clone(), status]); + let written = port.written(); + let mut bus = Bus::new(Box::new(port), BTreeMap::from([(1, xl330::DEFINITION)])); + + bus.change_id(sts3215::DEFINITION, 1, 11).unwrap(); + + // Protocol v1 all the way: three writes, the last one the id. + let written = written.lock().unwrap(); + assert_eq!(written.len(), 3); + assert_eq!(written[2][..7], [0xFF, 0xFF, 0x01, 0x04, 0x03, 0x05, 11]); + } + #[test] fn a_value_that_does_not_fit_leaves_the_whole_bus_unwritten() { let port = FakePort::new(vec![]); diff --git a/src/dynamixel_protocol/mod.rs b/src/dynamixel_protocol/mod.rs index e0d2dc9..082bc52 100644 --- a/src/dynamixel_protocol/mod.rs +++ b/src/dynamixel_protocol/mod.rs @@ -178,6 +178,43 @@ impl DynamixelProtocolHandler { } } + /// Ping every motor at once, and return the ids that answer. + /// + /// Each motor that hears the broadcast ping answers with its own status packet, in + /// turn, so finding all of them takes one listening window + /// ([`broadcast_ping_window`](Self::broadcast_ping_window) at the port's baud rate) + /// instead of one timeout per absent id. The port's timeout is put back afterwards. + /// + /// Only some firmwares answer: protocol v2 Dynamixel and Feetech STS do, protocol v1 + /// Dynamixel and Feetech SCS stay silent, which reads as no motor at all. + pub fn broadcast_ping(&self, serial_port: &mut dyn serialport::SerialPort) -> Result> { + let window = self.broadcast_ping_window(serial_port.baud_rate()?); + let timeout = serial_port.timeout(); + let ids = match &self.protocol { + ProtocolKind::V1(p) => p.broadcast_ping(serial_port, window), + ProtocolKind::V2(p, _) => p.broadcast_ping(serial_port, window), + }; + serial_port.set_timeout(timeout)?; + self.sleep_post_delay(); + ids + } + + /// How long a broadcast ping listens at `baudrate`: the window the vendor SDKs use. + /// + /// The status packets of every id the protocol allows (6 bytes each on v1, 14 on v2), + /// 3 ms of turn per id, and 16 ms for the latency timer of a USB serial adapter, which + /// can hold a short answer that long before passing it on. + pub fn broadcast_ping_window(&self, baudrate: u32) -> Duration { + let (status_size, max_id) = match self.protocol { + ProtocolKind::V1(_) => (6, v1::MAX_ID), + ProtocolKind::V2(..) => (14, v2::MAX_ID), + }; + let max_id = u64::from(max_id); + let bits = status_size * max_id * 10; + Duration::from_micros(bits * 1_000_000 / u64::from(baudrate)) + + Duration::from_millis(3 * max_id + 16) + } + /// Send a reboot instruction. /// /// Reboot the motor with specified `id`. @@ -562,6 +599,30 @@ trait Protocol { Ok(self.read_status_packet(port, id).is_ok()) } + /// Ping the broadcast id and keep the id of every status packet that parses until + /// `window` closes. Changes the port's timeout; the caller puts it back. + fn broadcast_ping(&self, port: &mut dyn SerialPort, window: Duration) -> Result> { + self.send_instruction_packet(port, P::ping_packet(P::BROADCAST_ID).as_ref())?; + let deadline = Instant::now() + window; + let mut ids = Vec::new(); + loop { + let left = deadline.saturating_duration_since(Instant::now()); + if left.is_zero() { + break; + } + port.set_timeout(left)?; + // Silence until the deadline, or bytes that do not frame a packet, end it. + let Ok(data) = self.read_status_packet_bytes(port) else { + break; + }; + let id = data[P::ID_POSITION]; + if P::status_packet(&data, id).is_ok() { + ids.push(id); + } + } + Ok(ids) + } + fn reboot(&self, port: &mut dyn SerialPort, id: u8) -> Result { self.send_instruction_packet(port, P::reboot_packet(id).as_ref())?; @@ -762,7 +823,10 @@ trait Protocol { } } -use std::{fmt, time::Duration}; +use std::{ + fmt, + time::{Duration, Instant}, +}; /// The error field of a status packet. /// @@ -952,6 +1016,59 @@ mod tests { ); } + #[test] + fn a_broadcast_ping_lists_every_motor_that_answers() { + // Motors 1 and 3 answer; motor 2's status packet arrives with a broken checksum. + let answers = [ + [0xFF, 0xFF, 0x01, 0x02, 0x00, 0xFC], + [0xFF, 0xFF, 0x02, 0x02, 0x00, 0x00], + [0xFF, 0xFF, 0x03, 0x02, 0x00, 0xFA], + ] + .concat(); + let mut port = FakePort::new(vec![answers]); + let written = port.written(); + let settings = port.settings(); + + let ids = DynamixelProtocolHandler::v1() + .broadcast_ping(&mut port) + .unwrap(); + + assert_eq!(ids, [1, 3]); + // One ping, to the broadcast id. + assert_eq!( + *written.lock().unwrap(), + [vec![0xFF, 0xFF, 0xFE, 0x02, 0x01, 0xFE]] + ); + // The port's own timeout is back once the window closes. + let timeouts = &settings.lock().unwrap().timeouts; + assert_eq!(timeouts.last(), timeouts.first()); + } + + #[test] + fn a_broadcast_ping_reads_protocol_v2_status_packets() { + let answer = vec![ + 0xFF, 0xFF, 0xFD, 0x00, 0x01, 0x08, 0x00, 0x55, 0x00, 0xA6, 0x00, 0x00, 0x00, 0x8C, + 0xC0, + ]; + let mut port = FakePort::new(vec![answer]); + + let ids = DynamixelProtocolHandler::v2() + .broadcast_ping(&mut port) + .unwrap(); + + assert_eq!(ids, [1]); + } + + #[test] + fn the_broadcast_ping_window_leaves_every_id_its_turn() { + // 253 ids at 3 ms each, the 16 ms of a USB latency timer, and 253 six-byte status + // packets at 1 Mbps. + assert_eq!( + DynamixelProtocolHandler::v1().broadcast_ping_window(1_000_000), + Duration::from_micros(253 * 3_000 + 16_000 + 253 * 6 * 10) + ); + } + #[test] fn no_post_delay_configured_means_no_sleep() { let dph = DynamixelProtocolHandler::v1(); diff --git a/src/dynamixel_protocol/packet.rs b/src/dynamixel_protocol/packet.rs index 9d477a5..9a08ec4 100644 --- a/src/dynamixel_protocol/packet.rs +++ b/src/dynamixel_protocol/packet.rs @@ -4,6 +4,10 @@ use crate::Result; pub trait Packet { const HEADER_SIZE: usize; + /// The id every motor listens to. + const BROADCAST_ID: u8; + /// Where a status packet carries the id of the motor that sent it. + const ID_POSITION: usize; type ErrorKind: Debug; type InstructionKind: Debug; diff --git a/src/dynamixel_protocol/v1.rs b/src/dynamixel_protocol/v1.rs index 44868f2..8e33118 100644 --- a/src/dynamixel_protocol/v1.rs +++ b/src/dynamixel_protocol/v1.rs @@ -13,6 +13,8 @@ pub(crate) const MAX_ID: u8 = 253; pub(crate) struct PacketV1; impl Packet for PacketV1 { const HEADER_SIZE: usize = 4; + const BROADCAST_ID: u8 = BROADCAST_ID; + const ID_POSITION: usize = 2; type ErrorKind = DynamixelErrorV1; type InstructionKind = InstructionKindV1; diff --git a/src/dynamixel_protocol/v2.rs b/src/dynamixel_protocol/v2.rs index cd718b7..55867b6 100644 --- a/src/dynamixel_protocol/v2.rs +++ b/src/dynamixel_protocol/v2.rs @@ -148,6 +148,8 @@ fn parse_fast_sync_read_status_with_error( pub(crate) struct PacketV2; impl Packet for PacketV2 { const HEADER_SIZE: usize = 7; + const BROADCAST_ID: u8 = BROADCAST_ID; + const ID_POSITION: usize = 4; type ErrorKind = DynamixelErrorV2; type InstructionKind = InstructionKindV2; diff --git a/src/lib.rs b/src/lib.rs index 2d4d5a1..32319a7 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -10,9 +10,10 @@ //! //! ## APIs //! -//! It exposes two APIs: +//! It exposes three APIs, from the wire up: //! * `DynamixelProtocolHandler`: low-level API. It handles the serial communication and the Dynamixel protocol parsing. It can be used for fine-grained control of the shared bus with other communication. -//! * `Controller`: high-level API for the Dynamixel protocol. Simpler and cleaner API but it takes full ownership of the io (it can still be shared if wrapped with a mutex for instance). +//! * `Controller`: one per servo model, with a typed accessor for each register. Simpler and cleaner API but it takes full ownership of the io (it can still be shared if wrapped with a mutex for instance). +//! * [`bus::Bus`] and [`servo::ServoDefinition`]: a higher-level, family-agnostic API, designed mostly for the [LeRobot](https://github.com/huggingface/lerobot) library. Motors of several models and of both protocols on one port, registers by name, the facts about each servo, finding and setting up motors, and the torque of a whole bus. Some of its choices are LeRobot conventions rather than vendor rules; the controllers stay the per-servo API underneath. //! //! See the examples below for usage. //! diff --git a/src/servo/README.md b/src/servo/README.md index b4ab23b..16190fc 100644 --- a/src/servo/README.md +++ b/src/servo/README.md @@ -9,7 +9,7 @@ pub mod sts3215.rs * Add the servo definition in the new file. You can use the [MX](./servo/dynamixel/mx.rs) as a template. The macro should defined the `name` of the servo, the `protocol version` used and then a list of all registers with their name, address, access, type and conversion type (can be set to None to get the raw register value). -* Between the protocol version and the registers, the definition can state what a caller of the raw address API needs to know to make sense of the bytes. Every entry is optional and they come in this order: `resolution: 4096,` (encoder steps per turn), `word_order: big,` (default `little`), `supports_sync_read: false,` (default `true`), `baudrates: [(1_000_000, 0), ...],` (baud rate and the value of the baud rate register), `factory_baudrate: 1_000_000,`, `homing_offset_sign: -1,` (`present = actual + sign * homing_offset`; needs a `homing_offset` register), `operating_modes: [(position, 0), ...],` (needs an `operating_mode` register) and `encoding: [(present_load, sign_magnitude(10)), (goal_pwm, twos_complement), (goal_position, unsigned)],` for registers whose sign handling differs from their integer type (signed types default to two's complement, everything else to unsigned). See [sts3215.rs](./feetech/sts3215.rs). They are exposed as `INFO`, `MODELS` and the `encoding` of each `RegisterInfo`, and to Python as static methods of the controller class. +* Between the protocol version and the registers, the definition can state what a caller of the raw address API needs to know to make sense of the bytes. Every entry is optional and they come in this order: `resolution: 4096,` (encoder steps per turn), `word_order: big,` (default `little`), `supports_sync_read: false,` (default `true`), `supports_broadcast_ping: true,` (default `false`), `baudrates: [(1_000_000, 0), ...],` (baud rate and the value of the baud rate register), `factory_baudrate: 1_000_000,`, `homing_offset_sign: -1,` (`present = actual + sign * homing_offset`; needs a `homing_offset` register), `operating_modes: [(position, 0), ...],` (needs an `operating_mode` register) and `encoding: [(present_load, sign_magnitude(10)), (goal_pwm, twos_complement), (goal_position, unsigned)],` for registers whose sign handling differs from their integer type (signed types default to two's complement, everything else to unsigned). See [sts3215.rs](./feetech/sts3215.rs). They are exposed as `INFO`, `MODELS` and the `encoding` of each `RegisterInfo`, and to Python as static methods of the controller class. * Finally, add the servo registration in the servo root module [./mod.rs]. You can specify all variants supported by your servo definition. This registration allows for the scan function to detect your new kind of servo. diff --git a/src/servo/definition.rs b/src/servo/definition.rs index 6a035ae..be45ff0 100644 --- a/src/servo/definition.rs +++ b/src/servo/definition.rs @@ -189,16 +189,112 @@ impl ServoDefinition { port.set_timeout(scan_timeout(port.baud_rate()?))?; let found = ids .iter() - .filter_map(|&id| { - let bytes = dph.read(port, id, reg.addr, reg.size).ok()?; - let model = reg.decode(self.info.word_order, &bytes).ok()?; - Some((id, model as u16)) - }) + .filter_map(|&id| self.model_number(dph, port, reg, id)) .collect(); port.set_timeout(timeout)?; Ok(found) } + /// Which of `ids` answer, with their model number read as this definition lays it + /// out: one broadcast ping, then a Model Number read of each id asked that answered, + /// under the port's own timeout. + /// + /// The ping listens for its whole window however few ids are asked + /// ([`broadcast_ping_window`](DynamixelProtocolHandler::broadcast_ping_window), about + /// 0.8 s at 1 Mbps), so this pays off over many ids, where [`scan`](Self::scan) costs a + /// timeout per absent id, and behind a USB adapter whose latency timer outlasts the + /// short timeout `scan` uses. A servo that does not answer a broadcast ping + /// (`supports_broadcast_ping`) is refused before anything is sent. + pub fn broadcast_scan( + &self, + dph: &DynamixelProtocolHandler, + port: &mut dyn SerialPort, + ids: &[u8], + ) -> Result> { + self.check_protocol(dph)?; + let reg = self.named("model_number")?; + if !self.info.supports_broadcast_ping { + return Err(RegisterError::BroadcastPing(self.name).into()); + } + let answered = dph.broadcast_ping(port)?; + Ok(ids + .iter() + .filter(|id| answered.contains(id)) + .filter_map(|&id| self.model_number(dph, port, reg, id)) + .collect()) + } + + /// The model number of motor `id`, or `None` when it does not answer with one. + fn model_number( + &self, + dph: &DynamixelProtocolHandler, + port: &mut dyn SerialPort, + reg: RegisterInfo, + id: u8, + ) -> Option<(u8, u16)> { + let bytes = dph.read(port, id, reg.addr, reg.size).ok()?; + let model = reg.decode(self.info.word_order, &bytes).ok()?; + Some((id, model as u16)) + } + + /// Give motor `id` the id `new_id`. + /// + /// The id is kept in EEPROM, which a motor only writes with its torque off, and a + /// Feetech motor only with its lock open: both are done first, and left that way. + /// The motor answers the write from its old id. + pub fn change_id( + &self, + dph: &DynamixelProtocolHandler, + port: &mut dyn SerialPort, + id: u8, + new_id: u8, + ) -> Result<()> { + self.open_eeprom(dph, port, id)?; + self.write_register(dph, port, id, "id", new_id.into()) + } + + /// Set motor `id` to talk at `baudrate`, one of the rates in this definition's + /// `baudrates`, written as the value its baud rate register takes for it. + /// + /// The EEPROM is opened first, as for [`change_id`](Self::change_id). The motor + /// answers the write at its old rate, then switches; `port` stays at the old rate, so + /// switch it too (`set_baud_rate`) to keep talking to the motor. + pub fn change_baudrate( + &self, + dph: &DynamixelProtocolHandler, + port: &mut dyn SerialPort, + id: u8, + baudrate: u32, + ) -> Result<()> { + let &(_, value) = self + .info + .baudrates + .iter() + .find(|&&(rate, _)| rate == baudrate) + .ok_or(RegisterError::Baudrate { + servo: self.name, + baudrate, + })?; + self.open_eeprom(dph, port, id)?; + self.write_register(dph, port, id, "baud_rate", value.into()) + } + + /// Turn the torque off, and open the lock of a servo that has one. A Feetech lock + /// opens with 0; the lock of an AX or MX only clears at power-up, and writing it 0 + /// changes nothing. + fn open_eeprom( + &self, + dph: &DynamixelProtocolHandler, + port: &mut dyn SerialPort, + id: u8, + ) -> Result<()> { + self.write_register(dph, port, id, "torque_enable", 0)?; + if self.register("lock").is_some() { + self.write_register(dph, port, id, "lock", 0)?; + } + Ok(()) + } + fn motors(&self, ids: &[u8]) -> Vec<(u8, ServoDefinition)> { ids.iter().map(|&id| (id, *self)).collect() } @@ -409,6 +505,13 @@ impl ServoDefinition { self.info.supports_sync_read } + /// Whether the firmware answers a ping sent to the broadcast id, which + /// `broadcast_scan` relies on. + #[getter] + fn supports_broadcast_ping(&self) -> bool { + self.info.supports_broadcast_ping + } + /// Serial rates the servo can be set to, as {baud rate: register value}, slowest /// first. #[getter] @@ -417,6 +520,7 @@ impl ServoDefinition { } /// The baud rate a new servo answers at, or `None` when the definition does not say. + /// True of the units made today: some older motors left the factory at another rate. #[getter] fn factory_baudrate(&self) -> Option { self.info.factory_baudrate @@ -466,8 +570,66 @@ impl ServoDefinition { mod tests { use super::*; use crate::fake_port::FakePort; + use crate::servo::dynamixel::xl330; use crate::servo::feetech::{scs0009, sts3215}; + #[test] + fn changing_an_id_opens_the_eeprom_first() { + // The STS3215 at id 9 answers each of the three writes. + let status = vec![0xFF, 0xFF, 0x09, 0x02, 0x00, 0xF4]; + let mut port = FakePort::new(vec![status.clone(), status.clone(), status]); + let written = port.written(); + + sts3215::DEFINITION + .change_id(&DynamixelProtocolHandler::v1(), &mut port, 9, 3) + .unwrap(); + + // Torque off (address 40), lock open (55), then the id (5), all at id 9. + let writes: Vec<_> = written + .lock() + .unwrap() + .iter() + .map(|packet| (packet[2], packet[5], packet[6])) + .collect(); + assert_eq!(writes, [(9, 40, 0), (9, 55, 0), (9, 5, 3)]); + } + + #[test] + fn a_baud_rate_is_written_as_its_register_value() { + // The XL330 has no lock: torque off (address 64), then 3 for 1 Mbps (address 8). + let status = vec![ + 0xFF, 0xFF, 0xFD, 0x00, 0x01, 0x04, 0x00, 0x55, 0x00, 0xA1, 0x0C, + ]; + let mut port = FakePort::new(vec![status.clone(), status]); + let written = port.written(); + let v2 = DynamixelProtocolHandler::v2(); + + xl330::DEFINITION + .change_baudrate(&v2, &mut port, 1, 1_000_000) + .unwrap(); + + let writes: Vec<_> = written + .lock() + .unwrap() + .iter() + .map(|packet| (packet[8], packet[10])) + .collect(); + assert_eq!(writes, [(64, 0), (8, 3)]); + + // A rate the servo cannot take is refused before anything is sent. + let err = xl330::DEFINITION + .change_baudrate(&v2, &mut port, 1, 250_000) + .unwrap_err(); + assert!(matches!( + err.downcast_ref::(), + Some(RegisterError::Baudrate { + servo: "XL330", + baudrate: 250_000 + }) + )); + assert_eq!(written.lock().unwrap().len(), 2); + } + #[test] fn a_definition_reads_through_the_handler_and_port_it_is_given() { // Present position 2048 from motor 1, little-endian. diff --git a/src/servo/dynamixel/xl320.rs b/src/servo/dynamixel/xl320.rs index 60fdbaf..b8b13b9 100644 --- a/src/servo/dynamixel/xl320.rs +++ b/src/servo/dynamixel/xl320.rs @@ -7,6 +7,7 @@ use crate::generate_servo; generate_servo!( XL320, v2, resolution: 1024, + supports_broadcast_ping: true, reg: (model_number, r, 0, u16, None), reg: (firmware_version, r, 2, u8, None), reg: (id, rw, 3, u8, None), diff --git a/src/servo/dynamixel/xl330.rs b/src/servo/dynamixel/xl330.rs index c7c9b72..4aed3ae 100644 --- a/src/servo/dynamixel/xl330.rs +++ b/src/servo/dynamixel/xl330.rs @@ -9,6 +9,7 @@ use crate::{generate_servo, servo::conversion::Conversion}; generate_servo!( XL330, v2, resolution: 4096, + supports_broadcast_ping: true, baudrates: [ (9_600, 0), (57_600, 1), (115_200, 2), (1_000_000, 3), (2_000_000, 4), (3_000_000, 5), (4_000_000, 6), diff --git a/src/servo/dynamixel/xl430.rs b/src/servo/dynamixel/xl430.rs index 958fb8f..975e19d 100644 --- a/src/servo/dynamixel/xl430.rs +++ b/src/servo/dynamixel/xl430.rs @@ -7,6 +7,7 @@ use crate::generate_servo; generate_servo!( XL430, v2, resolution: 4096, + supports_broadcast_ping: true, baudrates: [ (9_600, 0), (57_600, 1), (115_200, 2), (1_000_000, 3), (2_000_000, 4), (3_000_000, 5), (4_000_000, 6), diff --git a/src/servo/feetech/sts3215.rs b/src/servo/feetech/sts3215.rs index 0ac1133..b460d71 100644 --- a/src/servo/feetech/sts3215.rs +++ b/src/servo/feetech/sts3215.rs @@ -7,6 +7,7 @@ use crate::servo::dynamixel::mx::AnglePosition; generate_servo!( STS3215, v1, resolution: 4096, + supports_broadcast_ping: true, baudrates: [ (1_000_000, 0), (500_000, 1), (250_000, 2), (128_000, 3), (115_200, 4), (57_600, 5), (38_400, 6), (19_200, 7), diff --git a/src/servo/info.rs b/src/servo/info.rs index 232a931..b1c7d2a 100644 --- a/src/servo/info.rs +++ b/src/servo/info.rs @@ -154,6 +154,10 @@ pub enum RegisterError { UnknownMotor(u8), /// A sync write needs one value per id. ValueCount { ids: usize, values: usize }, + /// The servo does not answer a ping sent to the broadcast id. + BroadcastPing(&'static str), + /// The servo cannot be set to this baud rate. + Baudrate { servo: &'static str, baudrate: u32 }, } impl fmt::Display for RegisterError { @@ -182,6 +186,12 @@ impl fmt::Display for RegisterError { RegisterError::ValueCount { ids, values } => { write!(f, "{values} values for {ids} ids") } + RegisterError::BroadcastPing(servo) => { + write!(f, "{servo} does not answer a broadcast ping") + } + RegisterError::Baudrate { servo, baudrate } => { + write!(f, "{servo} cannot be set to {baudrate} baud") + } } } } @@ -232,9 +242,13 @@ pub struct ServoInfo { pub word_order: WordOrder, /// Whether the firmware answers the Sync Read instruction. pub supports_sync_read: bool, + /// Whether the firmware answers a ping sent to the broadcast id. + pub supports_broadcast_ping: bool, /// Serial rates the servo can be set to, as (baud rate, baud rate register value). pub baudrates: &'static [(u32, u8)], - /// The baud rate a new servo answers at. + /// The baud rate a new servo answers at. A fact about the units made today, not a + /// guarantee for every unit: some older motors left the factory at another rate, a + /// change that came with the move from protocol v1 to v2. pub factory_baudrate: Option, /// How the homing offset moves the position the servo reports: /// `present = actual + sign * homing_offset`. -1 on Feetech, +1 on Dynamixel. diff --git a/src/servo/mod.rs b/src/servo/mod.rs index 165d988..bda7ae2 100644 --- a/src/servo/mod.rs +++ b/src/servo/mod.rs @@ -218,6 +218,10 @@ mod tests { assert_eq!(scs0009::INFO.resolution, Some(1024)); assert_eq!(scs0009::INFO.word_order, WordOrder::Big); assert!(!scs0009::INFO.supports_sync_read); + assert!(sts3215::INFO.supports_broadcast_ping); + assert!(!scs0009::INFO.supports_broadcast_ping); + assert!(xl330::INFO.supports_broadcast_ping); + assert!(!mx::INFO.supports_broadcast_ping); // Feetech: present = actual - offset; Dynamixel: present = actual + offset. assert_eq!(sts3215::INFO.homing_offset_sign, Some(-1)); @@ -494,6 +498,62 @@ mod tests { assert_eq!(written.lock().unwrap().len(), 253); } + #[test] + fn a_broadcast_scan_pings_once_then_reads_the_motors_that_answered() { + use crate::fake_port::FakePort; + + // Motors 2 and 5 answer the broadcast ping; 5 was not asked for. Motor 2 then + // gives its model number, 777: bytes 9, 3 at address 3. + let port = FakePort::new(vec![ + [ + [0xFF, 0xFF, 0x02, 0x02, 0x00, 0xFB], + [0xFF, 0xFF, 0x05, 0x02, 0x00, 0xF8], + ] + .concat(), + vec![0xFF, 0xFF, 0x02, 0x04, 0x00, 0x09, 0x03, 0xED], + ]); + let written = port.written(); + let mut c = sts3215::Sts3215Controller::new() + .with_serial_port(Box::new(port)) + .with_protocol_v1(); + + let found = c.broadcast_scan(&[1, 2, 3]).unwrap(); + + assert_eq!(found, std::collections::BTreeMap::from([(2, 777)])); + // The ping, to the broadcast id, then one read. + let written = written.lock().unwrap(); + assert_eq!(written.len(), 2); + assert_eq!(written[0][2], 0xFE); + + // Nothing answers on protocol v2 either: a single instruction for every id. + let port = FakePort::new(vec![]); + let written = port.written(); + let mut c = xl330::Xl330Controller::new() + .with_serial_port(Box::new(port)) + .with_protocol_v2(); + assert!(c.broadcast_scan_all().unwrap().is_empty()); + assert_eq!(written.lock().unwrap().len(), 1); + } + + #[test] + fn a_broadcast_scan_refuses_a_servo_that_does_not_answer_one() { + use crate::fake_port::FakePort; + + let port = FakePort::new(vec![]); + let written = port.written(); + let mut c = scs0009::Scs0009Controller::new() + .with_serial_port(Box::new(port)) + .with_protocol_v1(); + + let err = c.broadcast_scan_all().unwrap_err(); + + assert_eq!( + err.downcast_ref::(), + Some(&super::RegisterError::BroadcastPing("SCS0009")) + ); + assert!(written.lock().unwrap().is_empty()); + } + #[test] fn the_scan_timeout_follows_the_baud_rate_down_to_a_floor() { use std::time::Duration; diff --git a/src/servo/servo_macro.rs b/src/servo/servo_macro.rs index 2c17a15..d1111ee 100644 --- a/src/servo/servo_macro.rs +++ b/src/servo/servo_macro.rs @@ -4,6 +4,7 @@ macro_rules! generate_servo { $(resolution: $resolution:expr,)? $(word_order: $word_order:ident,)? $(supports_sync_read: $supports_sync_read:expr,)? + $(supports_broadcast_ping: $supports_broadcast_ping:expr,)? $(baudrates: [$(($baud:expr, $baud_code:expr)),* $(,)?],)? $(factory_baudrate: $factory_baudrate:expr,)? $(homing_offset_sign: $homing_offset_sign:expr,)? @@ -160,9 +161,17 @@ macro_rules! generate_servo { INFO.supports_sync_read } - /// Serial rates the servo can be set to, as {baud rate: register value}. + /// Whether the firmware answers a ping sent to the broadcast id, which + /// `broadcast_scan` relies on. #[staticmethod] - pub fn baudrates() -> std::collections::HashMap { + pub fn supports_broadcast_ping() -> bool { + INFO.supports_broadcast_ping + } + + /// Serial rates the servo can be set to, as {baud rate: register value}, + /// slowest first. + #[staticmethod] + pub fn baudrates() -> std::collections::BTreeMap { INFO.baudrates.iter().copied().collect() } @@ -184,6 +193,7 @@ macro_rules! generate_servo { resolution: $crate::servo_option!($($resolution)?), word_order: $crate::servo_word_order!($($word_order)?), supports_sync_read: $crate::servo_supports_sync_read!($($supports_sync_read)?), + supports_broadcast_ping: $crate::servo_supports_broadcast_ping!($($supports_broadcast_ping)?), baudrates: &[$($(($baud, $baud_code)),*)?], factory_baudrate: $crate::servo_option!($($factory_baudrate)?), homing_offset_sign: $crate::servo_option!($($homing_offset_sign)?), @@ -983,6 +993,20 @@ macro_rules! generate_scan { let ids: Vec = (0..=self.dph.as_ref().unwrap().max_id()).collect(); self.scan(&ids) } + + /// Which of `ids` answer, with their model number: one broadcast ping, + /// then a read of each id asked that answered. See + /// [`ServoDefinition::broadcast_scan`](crate::servo::ServoDefinition::broadcast_scan). + pub fn broadcast_scan(&mut self, ids: &[u8]) -> $crate::Result> { + let (dph, port) = self.wire(); + DEFINITION.broadcast_scan(dph, port, ids) + } + + /// [`broadcast_scan`](Self::broadcast_scan) every id the protocol allows. + pub fn broadcast_scan_all(&mut self) -> $crate::Result> { + let ids: Vec = (0..=self.dph.as_ref().unwrap().max_id()).collect(); + self.broadcast_scan(&ids) + } } #[cfg(feature = "python")] @@ -1006,6 +1030,26 @@ macro_rules! generate_scan { None => c.scan_all(), }) } + + /// Which of `ids` answer, as {id: model number}; every id the protocol + /// allows when `ids` is left out. + /// + /// One broadcast ping, which listens for its whole window (about 0.8 s at + /// 1 Mbps) however few ids are asked, then a read of each id asked that + /// answered. Faster than `scan` over many ids, and safe behind a USB + /// adapter with a long latency timer. A servo that does not answer a + /// broadcast ping (`supports_broadcast_ping()`) raises `ValueError`. + #[pyo3(signature = (ids = None))] + pub fn broadcast_scan( + &self, + py: Python, + ids: Option>, + ) -> PyResult> { + self.by_name(py, |c| match &ids { + Some(ids) => c.broadcast_scan(ids), + None => c.broadcast_scan_all(), + }) + } } } }; @@ -1099,6 +1143,17 @@ macro_rules! servo_supports_sync_read { }; } +#[doc(hidden)] +#[macro_export] +macro_rules! servo_supports_broadcast_ping { + () => { + false + }; + ($supports_broadcast_ping:expr) => { + $supports_broadcast_ping + }; +} + #[macro_export] macro_rules! generate_reg_access { ($servo_name:ident, $reg_name:ident, r, $reg_addr:expr, $reg_type:ty, $conv:ident) => { @@ -1870,9 +1925,10 @@ macro_rules! register_servo { #[gen_stub_pymethods] #[pymethods] impl $group::[<$servo:lower>]::[<$servo:camel PyController>] { - /// Model numbers of the servos this definition covers, as {name: number}. + /// Model numbers of the servos this definition covers, as {name: number}, + /// by name. #[staticmethod] - pub fn models() -> std::collections::HashMap { + pub fn models() -> std::collections::BTreeMap { $group::[<$servo:lower>]::[<$servo:camel Controller>]::MODELS .iter() .map(|&(name, number)| (name.to_string(), number))