Merge pull request #85 from pollen-robotics/scs0009

Scs0009

Author: pierre-rouanet

Cargo.toml

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

README.md

@@ -65,13 +65,21 @@ fn main() {
             .with_protocol_v1()
             .with_serial_port(serial_port);
 
-    let pos = c.read_present_position(&vec![1, 2]).unwrap();
+    let pos = c.sync_read_present_position(&vec![1, 2]).unwrap();
     println!("Motors present position: {:?}", pos);
 
-    c.write_goal_position(&vec![1, 2], &vec![1000, 2000]).unwrap();
+    c.sync_write_goal_position(&vec![1, 2], &vec![1000, 2000]).unwrap();
 }
 ```
 
+## Tools
+
+Simple bus scanning tool:
+
+```bash
+cargo run --bin=scan -- --serialport=/dev/ttyUSB0 --baudrate=1000000 --protocol=v1
+```
+
 ## Documentation
 
 See https://docs.rs/rustypot for more information on APIs and examples.
@@ -88,7 +96,7 @@ To build them locally, you can use [maturin](https://www.maturin.rs).
 maturin build --release --features python
 ```
 
-or, if you want to install them in your local python environment: 
+or, if you want to install them in your local python environment:
 
 ```bash
 maturin develop --release --features python
@@ -108,4 +116,4 @@ This library is licensed under the [Apache License 2.0](./LICENSE).
 ## Support
 
 Rustypot is developed and maintained by [Pollen-Robotics](https://pollen-robotics.com). They developed open-source hardware and tools for robotics.
-Visit https://pollen-robotics.com to learn more or join the [Discord community](https://discord.com/invite/Kg3mZHTKgs) if you have any questions or want to share your projects. 
+Visit https://pollen-robotics.com to learn more or join the [Discord community](https://discord.com/invite/Kg3mZHTKgs) if you have any questions or want to share your projects.

examples/feetech_controller.rs

@@ -21,7 +21,7 @@ fn main() -> Result<(), Box<dyn Error>> {
 
     while start_overall.elapsed() < duration {
         let start_time = std::time::Instant::now();
-        let x = c.read_present_position(&ids);
+        let x = c.sync_read_present_position(&ids);
         let elapsed_time = start_time.elapsed();
 
         println!("present pos: {:?}", x);

python/README.md

@@ -18,27 +18,44 @@ maturin develop --release --features python
 
 ## Usage
 
-The Python bindings exposes the same API as the Controller API in the rust crate. 
+The Python bindings exposes the same API as the Controller API in the rust crate.
 
 You first need to create a Controller object. For instance, to communicate with a serial port to Feetech STS3215 motors, you can do the following:
 
 ```python
-from rustypot.servo import Sts3215SyncController
+from rustypot.servo import Sts3215PyController
 
-c = Sts3215SyncController(serial_port='/dev/ttyUSB0', baudrate=100000, timeout=0.1)
+c = Sts3215PyController(serial_port='/dev/ttyUSB0', baudrate=100000, timeout=0.1)
 ```
 
-Then, you can directly read/write any register of the motor. For instance, to read the present position of the motors with id 1 and 2, you can do:
+
+Then, you can directly read/write any register of the motor. For instance, to read the present position of the motor with id 1, you can do:
+
+```python
+
+pos = c.read_present_position(1)
+print(pos)
+```
+
+You can also write to the motors. For instance, to set the goal position of the motors with id 1 to 90° you can do:
 
 ```python
+import numpy as np
+c.write_goal_position(1, np.deg2rad(90.0))
+```
+
+
+Then, you can also sync_read any registers on multiple motors in a single operations. For instance, to read the present position of the motors with id 1 and 2, you can do:
 
-pos = c.read_present_position([1, 2])
+```python
+
+pos = c.sync_read_present_position([1, 2])
 print(pos)
 ```
 
-You can also write to the motors. For instance, to set the goal position of the motors with id 1 and 2 to 0.0 and 90° respectively, you can do:
+Same with sync_write. For instance, to set the goal position of the motors with id 1 and 2 to 0.0 and 90° respectively, you can do:
 
 ```python
 import numpy as np
-c.write_goal_position([1, 2], [0.0, np.deg2rad(90.0)])
-```
+c.sync_write_goal_position([1, 2], [0.0, np.deg2rad(90.0)])
+```

python/examples/feetech_sinus.py

@@ -1,12 +1,12 @@
 import time
 import numpy as np
 
-from rustypot.servo import Sts3215SyncController
+from rustypot.servo import Sts3215PyController
 
 def main():
     c = Sts3215SyncController(
-        serial_port='/dev/tty.usbmodem58FA0822621', 
-        baudrate=1000000, 
+        serial_port='/dev/tty.usbmodem58FA0822621',
+        baudrate=1000000,
         timeout=0.1,
     )
 
@@ -24,4 +24,4 @@ def main():
         time.sleep(0.01)
 
 if __name__ == '__main__':
-    main()
+    main()

src/bin/scan.rs

@@ -47,8 +47,10 @@ fn main() -> Result<(), Box<dyn Error>> {
                     let model = dph.read(serial_port.as_mut(), id, 0, 2).unwrap();
                     let model = u16::from_le_bytes([model[0], model[1]]);
                     let model = ServoKind::try_from(model);
-
-                    println!("Found motor with id {id} and model {model:?}");
+                    match model {
+                        Ok(m) => println!("Found motor with id {id} and model: {m:?}"),
+                        Err(e) => println!("Found motor with id {id} with {e:?}"),
+                    }
                 }
             }
             Err(e) => eprintln!("Error: {e}"),

src/lib.rs

@@ -50,10 +50,10 @@
 //!         .with_protocol_v1()
 //!         .with_serial_port(serial_port);
 //!
-//! let pos = c.read_present_position(&vec![1, 2]).unwrap();
+//! let pos = c.sync_read_present_position(&vec![1, 2]).unwrap();
 //! println!("Motors present position: {:?}", pos);
 //!
-//! c.write_goal_position(&vec![1, 2], &vec![0.0, 90.0_f64.to_radians()]).unwrap();
+//! c.sync_write_goal_position(&vec![1, 2], &vec![0.0, 90.0_f64.to_radians()]).unwrap();
 //! ```
 
 pub mod servo;

src/servo/feetech/mod.rs

@@ -1 +1,2 @@
+pub mod scs0009;
 pub mod sts3215;

src/servo/feetech/scs0009.rs

@@ -0,0 +1,134 @@
+use crate::generate_servo;
+use crate::servo::conversion::Conversion;
+
+generate_servo!(
+    SCS0009, v1,
+    reg: (model, r, 3, u16, None),
+    reg: (id, rw, 5, u8, None),
+    reg: (baudrate, rw, 6, u8, None),
+    reg: (return_delay_time, rw, 7, u8, None), //RESERVED?
+    reg: (response_status_level, rw, 8, u8, None),
+    reg: (min_angle_limit, rw, 9, i16, AnglePosition),
+    reg: (max_angle_limit, rw, 11, i16, AnglePosition),
+    reg: (max_temperature_limit, rw, 13, u8, None),
+    reg: (max_voltage_limit, rw, 14, u8, None),
+    reg: (min_voltage_limit, rw, 15, u8, None),
+    reg: (max_torque_limit, rw, 16, u16, TorqueLimit),
+    reg: (phase, rw, 18, u8, None), //SPECIAL REG
+    reg: (unloading_condition, rw, 19, u8, None),
+    reg: (led_alarm_condition, rw, 20, u8, None),
+    reg: (p_coefficient, rw, 21, u8, None),
+    reg: (d_coefficient, rw, 22, u8, None),
+    reg: (i_coefficient, rw, 23, u8, None),
+    reg: (minimum_startup_force, rw, 24, u16, BigEndian_u16),
+    reg: (cw_dead_zone, rw, 26, u8, None),
+    reg: (ccw_dead_zone, rw, 27, u8, None),
+    reg: (hysteresis_loop, rw, 27, u8, None),
+
+    reg: (protective_torque, rw, 37, u8, None),
+    reg: (protection_time, rw, 38, u8, None),
+    reg: (overload_torque, rw, 39, u8, None),
+
+    reg: (torque_enable, rw, 40, u8, None),
+
+    reg: (goal_position, rw, 42, i16, AnglePosition),
+    reg: (goal_time, rw, 44, u16, BigEndian_u16),
+    reg: (goal_speed, rw, 46, u16, Velocity),
+
+    reg: (lock, rw, 48, u8, bool),
+    reg: (present_position, r, 56, i16, AnglePosition),
+    reg: (present_speed, r, 58, u16, Velocity),
+    reg: (present_load, r, 60, u16, BigEndian_i16),
+
+    reg: (present_voltage, r, 62, u8, None),
+    reg: (present_temperature, r, 63, u8, None),
+
+    reg: (status, r, 65, u8, None),
+
+    reg: (moving, r, 66, u8, bool),
+
+);
+
+pub struct Velocity;
+
+impl Conversion for Velocity {
+    type RegisterType = u16;
+    type UsiType = f64;
+
+    fn from_raw(raw: u16) -> f64 {
+        // println!("DEBUG SPEED: {:?}", raw.to_be());
+        if raw.to_be() > (1 << 15) {
+            -300.0_f64.to_radians() / 1024.0 * (raw.to_be() & 0x3ff) as f64
+        } else {
+            300.0_f64.to_radians() / 1024.0 * (raw.to_be() & 0x3ff) as f64
+        }
+    }
+
+    fn to_raw(value: f64) -> u16 {
+        ((value / (300.0_f64.to_radians() / 1024.0)) as u16).to_be()
+    }
+}
+
+pub struct AnglePosition;
+
+impl Conversion for AnglePosition {
+    type RegisterType = i16;
+    type UsiType = f64;
+
+    fn from_raw(raw: i16) -> f64 {
+        300.0_f64.to_radians() * (((raw.to_be() & 0x3ff) - 511) as f64) / 1024.0
+    }
+
+    fn to_raw(value: f64) -> i16 {
+        let a = (1024.0 * (value) / (300.0_f64.to_radians()) + 511.0) as i16;
+        a.to_be()
+    }
+}
+
+#[allow(non_camel_case_types)]
+pub struct BigEndian_u16;
+impl Conversion for BigEndian_u16 {
+    type RegisterType = u16;
+    type UsiType = u16;
+
+    fn from_raw(raw: u16) -> u16 {
+        raw.to_be()
+    }
+
+    fn to_raw(value: u16) -> u16 {
+        value.to_be()
+    }
+}
+
+#[allow(non_camel_case_types)]
+pub struct BigEndian_i16;
+impl Conversion for BigEndian_i16 {
+    type RegisterType = u16;
+    type UsiType = i16;
+
+    fn from_raw(raw: u16) -> i16 {
+        if raw.to_be() > (1 << 10) {
+            -((raw.to_be() & 0x3ff) as i16)
+        } else {
+            (raw.to_be() & 0x3ff) as i16
+        }
+    }
+
+    fn to_raw(value: i16) -> u16 {
+        value.to_be() as u16
+    }
+}
+
+pub struct TorqueLimit;
+impl Conversion for TorqueLimit {
+    type RegisterType = u16;
+    type UsiType = f64;
+
+    fn from_raw(raw: u16) -> f64 {
+        raw.to_be() as f64 * 0.1
+    }
+
+    fn to_raw(value: f64) -> u16 {
+        (value.clamp(0.0, 100.0) as u16 * 10).to_be()
+    }
+}

src/servo/mod.rs

@@ -25,6 +25,9 @@ crate::register_servo!(
     servo: (feetech, STS3215,
         (STS3215, 2307)
     ),
+    servo: (feetech, SCS0009,
+        (SCS0009, 1280)
+    ),
     servo: (orbita, orbita2d_poulpe,
         (orbita2d_poulpe, 10020)
     ),