Merge branch 'develop'

Author: pierre-rouanet

.github/workflows/rust.yml

@@ -1,6 +1,6 @@
 name: Rust
 
-on: [push, pull_request]
+on: [pull_request]
 
 env:
   CARGO_TERM_COLOR: always

Cargo.toml

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

Changelog.md

@@ -1,3 +1,8 @@
+## Version 1.3.0
+
+- Add reboot support in core library.
+- Add ping and reboot support in python bindings.
+
 ## Version 1.2.0
 
 - Add support for AX motors (see https://github.com/pollen-robotics/rustypot/pull/93, thanks to @kacper-uminski)

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,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);

src/servo/feetech/sts3215.rs

@@ -28,7 +28,7 @@ generate_servo!(
     reg: (ccw_dead_zone, rw, 27, u8, None),
     reg: (protection_current, rw, 28, u16, None),
     reg: (angular_resolution, rw, 30, u8, None),
-    reg: (offset, rw, 31, i16, AnglePosition),
+    reg: (offset, rw, 31, u16, Offset),
     reg: (mode, rw, 33, u8, None),
     reg: (protective_torque, rw, 34, u8, None),
     reg: (protection_time, rw, 35, u8, None),
@@ -76,3 +76,58 @@ impl Conversion for Velocity {
         value as u16
     }
 }
+
+pub struct Offset;
+const MAX_MAGNITUDE: u16 = 2047;
+
+impl Conversion for Offset {
+    type RegisterType = u16;
+    type UsiType = f64;
+
+    fn from_raw(raw: u16) -> f64 {
+        use std::f64::consts::PI;
+        let negative = (raw >> 11) == 1;
+        let magnitude = raw % (MAX_MAGNITUDE + 1);
+
+        let float_magnitude = PI * f64::from(magnitude) / f64::from(MAX_MAGNITUDE);
+
+        if negative {
+            -float_magnitude
+        } else {
+            float_magnitude
+        }
+    }
+
+    fn to_raw(value: f64) -> u16 {
+        use std::f64::consts::PI;
+
+        let magnitude = (value.abs() * f64::from(MAX_MAGNITUDE) / PI) as u16;
+
+        if value.is_sign_negative() {
+            magnitude | (1 << 11)
+        } else {
+            magnitude
+        }
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    #[test]
+    fn offset_conversions() {
+        use crate::servo::{conversion::Conversion, feetech::sts3215::Offset};
+        use std::f64::consts::{FRAC_PI_2, PI};
+
+        assert_eq!(Offset::to_raw(0.0), 0);
+        assert_eq!(Offset::to_raw(PI), 2047);
+        assert_eq!(Offset::to_raw(-PI), 4095);
+        assert_eq!(Offset::to_raw(FRAC_PI_2), 1023);
+        assert_eq!(Offset::to_raw(-FRAC_PI_2), 3071);
+
+        assert_eq!(Offset::from_raw(0), 0.0);
+        assert_eq!(Offset::from_raw(2047), PI);
+        assert_eq!(Offset::from_raw(4095), -PI);
+        assert_eq!(Offset::from_raw(1023), 1.5700289617109715); // About PI/2
+        assert_eq!(Offset::from_raw(3071), -1.5700289617109715); // About -PI/2
+    }
+}

src/servo/servo_macro.rs

@@ -41,6 +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_addr_read_write!($servo_name);
 
         $(
@@ -117,6 +118,27 @@ macro_rules! generate_protocol_constructor {
     };
 }
 
+#[macro_export]
+macro_rules! generate_ping_reboot {
+    ($servo_macro:ident) => {
+        paste::paste! {
+            impl [<$servo_macro:camel Controller>] {
+                pub fn ping(&mut self, id: u8) -> $crate::Result<bool> {
+                    let dph = self.dph.as_ref().unwrap();
+                    let serial_port = self.serial_port.as_mut().unwrap().as_mut();
+                    dph.ping(serial_port, id)
+                }
+
+                pub fn reboot(&mut self, id: u8) -> $crate::Result<bool> {
+                    let dph = self.dph.as_ref().unwrap();
+                    let serial_port = self.serial_port.as_mut().unwrap().as_mut();
+                    dph.reboot(serial_port, id)
+                }
+            }
+        }
+    };
+}
+
 #[macro_export]
 macro_rules! generate_addr_read_write {
     ($servo_name:ident) => {
@@ -230,6 +252,16 @@ macro_rules! generate_addr_read_write {
                         .map_err(|e| pyo3::exceptions::PyRuntimeError::new_err(e.to_string()))?;
                     Ok(())
                 }
+
+                pub fn ping(&self, id: u8) -> PyResult<bool> {
+                    self.0.lock().unwrap().ping(id)
+                        .map_err(|e| pyo3::exceptions::PyRuntimeError::new_err(e.to_string()))
+                }
+
+                pub fn reboot(&self, id: u8) -> PyResult<bool> {
+                    self.0.lock().unwrap().reboot(id)
+                        .map_err(|e| pyo3::exceptions::PyRuntimeError::new_err(e.to_string()))
+                }
             }
         }
     };