feat(reboot): adding support for reboot packet for dynamixel protocols v1 and v2

Author: CarolinePascal

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<bool> {
+        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<P: Packet> {
         Ok(self.read_status_packet(port, id).is_ok())
     }
 
+    fn reboot(&self, port: &mut dyn SerialPort, id: u8) -> Result<bool> {
+        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<Vec<u8>> {
         self.send_instruction_packet(port, P::read_packet(id, addr, length).as_ref())?;
         self.read_status_packet(port, id)

src/dynamixel_protocol/packet.rs

@@ -10,6 +10,7 @@ pub trait Packet {
     fn get_payload_size(header: &[u8]) -> Result<usize>;
 
     fn ping_packet(id: u8) -> Box<dyn InstructionPacket<Self>>;
+    fn reboot_packet(id: u8) -> Box<dyn InstructionPacket<Self>>;
 
     fn read_packet(id: u8, addr: u8, length: u8) -> Box<dyn InstructionPacket<Self>>;
     fn write_packet(id: u8, addr: u8, data: &[u8]) -> Box<dyn InstructionPacket<Self>>;

src/dynamixel_protocol/v1.rs

@@ -23,6 +23,14 @@ impl Packet for PacketV1 {
         })
     }
 
+    fn reboot_packet(id: u8) -> Box<dyn InstructionPacket<Self>> {
+        Box::new(InstructionPacketV1 {
+            id,
+            instruction: InstructionKindV1::Reboot,
+            params: vec![],
+        })
+    }
+
     fn read_packet(id: u8, addr: u8, length: u8) -> Box<dyn InstructionPacket<Self>> {
         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);

src/dynamixel_protocol/v2.rs

@@ -39,6 +39,14 @@ impl Packet for PacketV2 {
         })
     }
 
+    fn reboot_packet(id: u8) -> Box<dyn InstructionPacket<Self>> {
+        Box::new(InstructionPacketV2 {
+            id,
+            instruction: InstructionKindV2::Reboot,
+            params: vec![],
+        })
+    }
+
     fn read_packet(id: u8, addr: u8, length: u8) -> Box<dyn InstructionPacket<Self>> {
         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);