Merge pull request #106 from pollen-robotics/105-add-factory-reset-option

Add factory reset instr.

Author: pierre-rouanet

Cargo.toml

@@ -1,6 +1,6 @@
 [package]
 name = "rustypot"
-version = "1.3.0"
+version = "1.4.0"
 edition = "2021"
 license = "Apache-2.0"
 authors = ["Pollen Robotics"]

Changelog.md

@@ -1,3 +1,7 @@
+## Version 1.4.0
+
+- Add support for factory reset in core library and python bindings.
+
 ## Version 1.3.0
 
 - Add reboot support in core library.

src/dynamixel_protocol/mod.rs

@@ -129,6 +129,30 @@ impl DynamixelProtocolHandler {
         }
     }
 
+    /// Factory reset instruction.
+    ///
+    /// Reset the Control Table of DYNAMIXEL to the factory default values.
+    /// Please note that conserving ID and/or Baudrate is only supported on protocol v2.
+    pub fn factory_reset(
+        &self,
+        serial_port: &mut dyn serialport::SerialPort,
+        id: u8,
+        conserve_id_only: bool,
+        conserve_id_and_baudrate: bool,
+    ) -> Result<()> {
+        match &self.protocol {
+            ProtocolKind::V1(p) => {
+                if conserve_id_only || conserve_id_and_baudrate {
+                    return Err(Box::new(CommunicationErrorKind::Unsupported));
+                }
+                p.factory_reset(serial_port, id, conserve_id_only, conserve_id_and_baudrate)
+            }
+            ProtocolKind::V2(p) => {
+                p.factory_reset(serial_port, id, conserve_id_only, conserve_id_and_baudrate)
+            }
+        }
+    }
+
     /// Reads raw register bytes.
     ///
     /// Sends a read instruction to the motor and wait for the status packet in response.
@@ -350,6 +374,20 @@ trait Protocol<P: Packet> {
         Ok(self.read_status_packet(port, id).is_ok())
     }
 
+    fn factory_reset(
+        &self,
+        port: &mut dyn SerialPort,
+        id: u8,
+        conserve_id_only: bool,
+        conserve_id_and_baudrate: bool,
+    ) -> Result<()> {
+        self.send_instruction_packet(
+            port,
+            P::factory_reset_packet(id, conserve_id_only, conserve_id_and_baudrate).as_ref(),
+        )?;
+        self.read_status_packet(port, id).map(|_| ())
+    }
+
     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

@@ -11,6 +11,11 @@ pub trait Packet {
 
     fn ping_packet(id: u8) -> Box<dyn InstructionPacket<Self>>;
     fn reboot_packet(id: u8) -> Box<dyn InstructionPacket<Self>>;
+    fn factory_reset_packet(
+        id: u8,
+        conserve_id_only: bool,
+        conserve_id_and_baudrate: bool,
+    ) -> 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

@@ -31,6 +31,18 @@ impl Packet for PacketV1 {
         })
     }
 
+    fn factory_reset_packet(
+        id: u8,
+        _conserve_id_only: bool,
+        _conserve_id_and_baudrate: bool,
+    ) -> Box<dyn InstructionPacket<Self>> {
+        Box::new(InstructionPacketV1 {
+            id,
+            instruction: InstructionKindV1::FactoryReset,
+            params: vec![],
+        })
+    }
+
     fn read_packet(id: u8, addr: u8, length: u8) -> Box<dyn InstructionPacket<Self>> {
         Box::new(InstructionPacketV1 {
             id,
@@ -230,6 +242,7 @@ pub(crate) enum InstructionKindV1 {
     Ping,
     Read,
     Write,
+    FactoryReset,
     Reboot,
     SyncWrite,
     SyncRead,
@@ -241,6 +254,7 @@ impl InstructionKindV1 {
             InstructionKindV1::Ping => 0x01,
             InstructionKindV1::Read => 0x02,
             InstructionKindV1::Write => 0x03,
+            InstructionKindV1::FactoryReset => 0x06,
             InstructionKindV1::Reboot => 0x08,
             InstructionKindV1::SyncRead => 0x82,
             InstructionKindV1::SyncWrite => 0x83,

src/dynamixel_protocol/v2.rs

@@ -47,6 +47,26 @@ impl Packet for PacketV2 {
         })
     }
 
+    fn factory_reset_packet(
+        id: u8,
+        conserve_id_only: bool,
+        conserve_id_and_baudrate: bool,
+    ) -> Box<dyn InstructionPacket<Self>> {
+        // See https://emanual.robotis.com/docs/en/dxl/protocol2/
+        let param = match (conserve_id_only, conserve_id_and_baudrate) {
+            (false, false) => 0xFF,
+            (true, false) => 0x01,
+            (true, true) => 0x02,
+            (false, true) => 0x02, // Same as (true, true)
+        };
+
+        Box::new(InstructionPacketV2 {
+            id,
+            instruction: InstructionKindV2::FactoryReset,
+            params: vec![param],
+        })
+    }
+
     fn read_packet(id: u8, addr: u8, length: u8) -> Box<dyn InstructionPacket<Self>> {
         Box::new(InstructionPacketV2 {
             id,
@@ -222,6 +242,7 @@ pub(crate) enum InstructionKindV2 {
     Ping,
     Read,
     Write,
+    FactoryReset,
     Reboot,
     SyncRead,
     SyncWrite,
@@ -233,6 +254,7 @@ impl InstructionKindV2 {
             InstructionKindV2::Ping => 0x01,
             InstructionKindV2::Read => 0x02,
             InstructionKindV2::Write => 0x03,
+            InstructionKindV2::FactoryReset => 0x06,
             InstructionKindV2::Reboot => 0x08,
             InstructionKindV2::SyncRead => 0x82,
             InstructionKindV2::SyncWrite => 0x83,

src/servo/servo_macro.rs

@@ -41,7 +41,7 @@ macro_rules! generate_servo {
         use pyo3_stub_gen::derive::*;
 
         $crate::generate_protocol_constructor!($servo_name, $protocol);
-        $crate::generate_ping_reboot!($servo_name);
+        $crate::generate_special_instructions!($servo_name);
         $crate::generate_addr_read_write!($servo_name);
 
         $(
@@ -119,7 +119,7 @@ macro_rules! generate_protocol_constructor {
 }
 
 #[macro_export]
-macro_rules! generate_ping_reboot {
+macro_rules! generate_special_instructions {
     ($servo_macro:ident) => {
         paste::paste! {
             impl [<$servo_macro:camel Controller>] {
@@ -134,6 +134,17 @@ macro_rules! generate_ping_reboot {
                     let serial_port = self.serial_port.as_mut().unwrap().as_mut();
                     dph.reboot(serial_port, id)
                 }
+
+                pub fn factory_reset(
+                    &mut self,
+                    id: u8,
+                    conserve_id_only: bool,
+                    conserve_id_and_baudrate: bool,
+                ) -> $crate::Result<()> {
+                    let dph = self.dph.as_ref().unwrap();
+                    let serial_port = self.serial_port.as_mut().unwrap().as_mut();
+                    dph.factory_reset(serial_port, id, conserve_id_only, conserve_id_and_baudrate)
+                }
             }
         }
     };
@@ -262,6 +273,21 @@ macro_rules! generate_addr_read_write {
                     self.0.lock().unwrap().reboot(id)
                         .map_err(|e| pyo3::exceptions::PyRuntimeError::new_err(e.to_string()))
                 }
+
+                #[pyo3(signature = (
+                    id,
+                    conserve_id_only = true,
+                    conserve_id_and_baudrate = true
+                ))]
+                pub fn factory_reset(
+                    &self,
+                    id: u8,
+                    conserve_id_only: bool,
+                    conserve_id_and_baudrate: bool,
+                ) -> PyResult<()> {
+                    self.0.lock().unwrap().factory_reset(id, conserve_id_only, conserve_id_and_baudrate)
+                        .map_err(|e| pyo3::exceptions::PyRuntimeError::new_err(e.to_string()))
+                }
             }
         }
     };