diff --git a/src/dynamixel_protocol/mod.rs b/src/dynamixel_protocol/mod.rs index e92c836..aa1f610 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..7946706 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,16 @@ 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);