From 600afbe04a90268127dcc9f79a7e8c1e6598ff26 Mon Sep 17 00:00:00 2001 From: CarolinePascal Date: Wed, 10 Sep 2025 19:41:20 +0200 Subject: [PATCH 1/2] feat(reboot): adding support for reboot packet for dynamixel protocols v1 and v2 --- src/dynamixel_protocol/mod.rs | 17 +++++++++++++++++ src/dynamixel_protocol/packet.rs | 1 + src/dynamixel_protocol/v1.rs | 17 +++++++++++++++++ src/dynamixel_protocol/v2.rs | 18 ++++++++++++++++++ 4 files changed, 53 insertions(+) diff --git a/src/dynamixel_protocol/mod.rs b/src/dynamixel_protocol/mod.rs index e92c836..89c5f8d 100644 --- a/src/dynamixel_protocol/mod.rs +++ b/src/dynamixel_protocol/mod.rs @@ -118,6 +118,17 @@ impl DynamixelProtocolHandler { } } + /// Send a reboot instruction. + /// + /// Reboot the motor with specified `id`. + /// Returns an [CommunicationErrorKind] if the communication fails. + pub fn reboot(&self, serial_port: &mut dyn serialport::SerialPort, id: u8) -> Result { + match &self.protocol { + ProtocolKind::V1(p) => p.reboot(serial_port, id), + ProtocolKind::V2(p) => p.reboot(serial_port, id), + } + } + /// Reads raw register bytes. /// /// Sends a read instruction to the motor and wait for the status packet in response. @@ -333,6 +344,12 @@ trait Protocol { Ok(self.read_status_packet(port, id).is_ok()) } + fn reboot(&self, port: &mut dyn SerialPort, id: u8) -> Result { + self.send_instruction_packet(port, P::reboot_packet(id).as_ref())?; + + Ok(self.read_status_packet(port, id).is_ok()) + } + fn read(&self, port: &mut dyn SerialPort, id: u8, addr: u8, length: u8) -> Result> { self.send_instruction_packet(port, P::read_packet(id, addr, length).as_ref())?; self.read_status_packet(port, id) diff --git a/src/dynamixel_protocol/packet.rs b/src/dynamixel_protocol/packet.rs index 0780a2d..3e9a34c 100644 --- a/src/dynamixel_protocol/packet.rs +++ b/src/dynamixel_protocol/packet.rs @@ -10,6 +10,7 @@ pub trait Packet { fn get_payload_size(header: &[u8]) -> Result; fn ping_packet(id: u8) -> Box>; + fn reboot_packet(id: u8) -> Box>; fn read_packet(id: u8, addr: u8, length: u8) -> Box>; fn write_packet(id: u8, addr: u8, data: &[u8]) -> Box>; diff --git a/src/dynamixel_protocol/v1.rs b/src/dynamixel_protocol/v1.rs index f99e569..60f8c6c 100644 --- a/src/dynamixel_protocol/v1.rs +++ b/src/dynamixel_protocol/v1.rs @@ -23,6 +23,14 @@ impl Packet for PacketV1 { }) } + fn reboot_packet(id: u8) -> Box> { + Box::new(InstructionPacketV1 { + id, + instruction: InstructionKindV1::Reboot, + params: vec![], + }) + } + fn read_packet(id: u8, addr: u8, length: u8) -> Box> { Box::new(InstructionPacketV1 { id, @@ -222,6 +230,7 @@ pub(crate) enum InstructionKindV1 { Ping, Read, Write, + Reboot, SyncWrite, SyncRead, } @@ -232,6 +241,7 @@ impl InstructionKindV1 { InstructionKindV1::Ping => 0x01, InstructionKindV1::Read => 0x02, InstructionKindV1::Write => 0x03, + InstructionKindV1::Reboot => 0x08, InstructionKindV1::SyncRead => 0x82, InstructionKindV1::SyncWrite => 0x83, } @@ -261,6 +271,13 @@ mod tests { assert_eq!(bytes, [0xFF, 0xFF, 0x01, 0x02, 0x01, 0xFB]); } + #[test] + fn create_reboot_packet() { + let p = PacketV1::reboot_packet(2); + let bytes = p.to_bytes(); + assert_eq!(bytes, [0xFF, 0xFF, 0x02, 0x02, 0x08, 0xF3]); + } + #[test] fn create_read_packet() { let p = PacketV1::read_packet(1, 0x2B, 1); diff --git a/src/dynamixel_protocol/v2.rs b/src/dynamixel_protocol/v2.rs index 8deb469..da9e590 100644 --- a/src/dynamixel_protocol/v2.rs +++ b/src/dynamixel_protocol/v2.rs @@ -39,6 +39,14 @@ impl Packet for PacketV2 { }) } + fn reboot_packet(id: u8) -> Box> { + Box::new(InstructionPacketV2 { + id, + instruction: InstructionKindV2::Reboot, + params: vec![], + }) + } + fn read_packet(id: u8, addr: u8, length: u8) -> Box> { Box::new(InstructionPacketV2 { id, @@ -214,6 +222,7 @@ pub(crate) enum InstructionKindV2 { Ping, Read, Write, + Reboot, SyncRead, SyncWrite, } @@ -224,6 +233,7 @@ impl InstructionKindV2 { InstructionKindV2::Ping => 0x01, InstructionKindV2::Read => 0x02, InstructionKindV2::Write => 0x03, + InstructionKindV2::Reboot => 0x08, InstructionKindV2::SyncRead => 0x82, InstructionKindV2::SyncWrite => 0x83, } @@ -310,6 +320,7 @@ mod tests { assert_eq!(crc.to_le_bytes(), [0x16, 0xd2]); } + #[test] fn create_ping_packet() { let p = PacketV2::ping_packet(2); @@ -320,6 +331,13 @@ mod tests { ); } + #[test] + fn create_reboot_packet() { + let p = PacketV2::reboot_packet(2); + let bytes = p.to_bytes(); + assert_eq!(bytes, [0xff, 0xff, 0xfd, 0x0, 0x2, 0x3, 0x0, 0x8, 0x2f, 0x72]); + } + #[test] fn create_read_packet() { let p = PacketV2::read_packet(1, 0x2B, 2); From bd8289a0465e28e382f7e6f2529ecf9a42efce7f Mon Sep 17 00:00:00 2001 From: CarolinePascal Date: Fri, 19 Sep 2025 11:34:33 +0200 Subject: [PATCH 2/2] chore(formatting): formatting code --- src/dynamixel_protocol/mod.rs | 2 +- src/dynamixel_protocol/v2.rs | 5 ++++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/dynamixel_protocol/mod.rs b/src/dynamixel_protocol/mod.rs index 89c5f8d..aa1f610 100644 --- a/src/dynamixel_protocol/mod.rs +++ b/src/dynamixel_protocol/mod.rs @@ -346,7 +346,7 @@ trait Protocol { fn reboot(&self, port: &mut dyn SerialPort, id: u8) -> Result { self.send_instruction_packet(port, P::reboot_packet(id).as_ref())?; - + Ok(self.read_status_packet(port, id).is_ok()) } diff --git a/src/dynamixel_protocol/v2.rs b/src/dynamixel_protocol/v2.rs index da9e590..7946706 100644 --- a/src/dynamixel_protocol/v2.rs +++ b/src/dynamixel_protocol/v2.rs @@ -335,7 +335,10 @@ mod tests { fn create_reboot_packet() { let p = PacketV2::reboot_packet(2); let bytes = p.to_bytes(); - assert_eq!(bytes, [0xff, 0xff, 0xfd, 0x0, 0x2, 0x3, 0x0, 0x8, 0x2f, 0x72]); + assert_eq!( + bytes, + [0xff, 0xff, 0xfd, 0x0, 0x2, 0x3, 0x0, 0x8, 0x2f, 0x72] + ); } #[test]