Merge pull request #1 from kscalelabs/hexmove

Hexmove imu

examples/hexmove.py

@@ -0,0 +1,34 @@
+"""Example usage of the Hexmove IMU."""
+
+import time
+
+from imu import HexmoveImuReader
+
+
+def main() -> None:
+    # Initialize the IMU reader for the 'can0' interface and imu with serial number 1 and model 1
+    try:
+        imu_reader = HexmoveImuReader("can0", 1, 1)
+    except Exception as e:
+        print(f"Failed to initialize IMU reader: {e}")
+        return
+
+    try:
+        while True:
+            # Get the current IMU data
+            data = imu_reader.get_data()
+            print(
+                f"Angular Position: X={data.x_angle}°, Y={data.y_angle}°, Z={data.z_angle}° | "
+                f"Angular Velocity: X={data.x_velocity}°/s, Y={data.y_velocity}°/s, Z={data.z_velocity}°/s"
+            )
+
+            # Sleep for a short duration to avoid spamming the console
+            time.sleep(0.1)
+    except KeyboardInterrupt:
+        print("Stopping IMU reader...")
+    finally:
+        imu_reader.stop()
+
+
+if __name__ == "__main__":
+    main()

imu/__init__.py

@@ -1,6 +1,8 @@
 """Defines the top-level API for the IMU package."""
 
-__version__ = "0.0.1"
+__version__ = "0.0.2"
 
-# from .bindings import (
-# )
+from .bindings import (
+    PyHexmoveImuData as HexmoveImuData,
+    PyHexmoveImuReader as HexmoveImuReader,
+)

imu/bindings.pyi

@@ -0,0 +1 @@
+bindings/bindings.pyi

imu/bindings/bindings.pyi

@@ -0,0 +1,21 @@
+# This file is automatically generated by pyo3_stub_gen
+# ruff: noqa: E501, F401
+
+
+class PyHexmoveImuData:
+    x_angle: float
+    y_angle: float
+    z_angle: float
+    x_velocity: float
+    y_velocity: float
+    z_velocity: float
+
+class PyHexmoveImuReader:
+    def __new__(cls,interface:str, serial_number:int, model:int): ...
+    def get_data(self) -> PyHexmoveImuData:
+        ...
+
+    def stop(self) -> None:
+        ...
+
+

imu/bindings/src/lib.rs

@@ -1,10 +1,75 @@
+use hexmove::{ImuData as HexmoveImuData, ImuReader as HexmoveImuReader};
 use pyo3::prelude::*;
 use pyo3_stub_gen::define_stub_info_gatherer;
 use pyo3_stub_gen::derive::{gen_stub_pyclass, gen_stub_pymethods};
+use std::sync::{Arc, Mutex};
 
+#[gen_stub_pyclass]
+#[pyclass]
+struct PyHexmoveImuReader {
+    inner: Arc<Mutex<HexmoveImuReader>>,
+}
+
+#[gen_stub_pymethods]
+#[pymethods]
+impl PyHexmoveImuReader {
+    #[new]
+    fn new(interface: String, serial_number: u8, model: u8) -> PyResult<Self> {
+        let imu_reader = HexmoveImuReader::new(&interface, serial_number, model)
+            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
+        Ok(PyHexmoveImuReader {
+            inner: Arc::new(Mutex::new(imu_reader)),
+        })
+    }
+
+    fn get_data(&self) -> PyResult<PyHexmoveImuData> {
+        let imu_reader = self.inner.lock().unwrap();
+        let data = imu_reader.get_data();
+        Ok(PyHexmoveImuData::from(data))
+    }
+
+    fn stop(&self) -> PyResult<()> {
+        let imu_reader = self.inner.lock().unwrap();
+        imu_reader.stop();
+        Ok(())
+    }
+}
+
+#[gen_stub_pyclass]
+#[pyclass]
+#[derive(Clone)]
+struct PyHexmoveImuData {
+    #[pyo3(get)]
+    x_angle: f32,
+    #[pyo3(get)]
+    y_angle: f32,
+    #[pyo3(get)]
+    z_angle: f32,
+    #[pyo3(get)]
+    x_velocity: f32,
+    #[pyo3(get)]
+    y_velocity: f32,
+    #[pyo3(get)]
+    z_velocity: f32,
+}
+
+impl From<HexmoveImuData> for PyHexmoveImuData {
+    fn from(data: HexmoveImuData) -> Self {
+        PyHexmoveImuData {
+            x_angle: data.x_angle,
+            y_angle: data.y_angle,
+            z_angle: data.z_angle,
+            x_velocity: data.x_velocity,
+            y_velocity: data.y_velocity,
+            z_velocity: data.z_velocity,
+        }
+    }
+}
 
 #[pymodule]
 fn bindings(m: &Bound<PyModule>) -> PyResult<()> {
+    m.add_class::<PyHexmoveImuReader>()?;
+    m.add_class::<PyHexmoveImuData>()?;
     Ok(())
 }
 

imu/hexmove/Cargo.toml

@@ -13,11 +13,8 @@ license.workspace = true
 [lib]
 
 name = "hexmove"
-crate-type = ["cdylib", "rlib"]
+crate-type = ["rlib"]
 
 [dependencies]
-
-[[bin]]
-
-name = "hexmove"
-path = "src/bin/hexmove.rs"
+socketcan = "3.3.0"
+log = "0.4"

imu/hexmove/src/bin/log.rs

@@ -0,0 +1,31 @@
+use hexmove::ImuReader;
+use std::thread;
+use std::time::Duration;
+
+fn main() {
+    // Create a new ImuReader for the 'can0' interface
+    let imu_reader = match ImuReader::new("can0", 1, 1) {
+        Ok(reader) => reader,
+        Err(e) => {
+            eprintln!("Failed to initialize IMU reader: {}", e);
+            return;
+        }
+    };
+
+    // Continuously read and print IMU data
+    loop {
+        let data = imu_reader.get_data();
+        println!(
+            "Angular Position: X={}°, Y={}°, Z={}° | Angular Velocity: X={}°/s, Y={}°/s, Z={}°/s",
+            data.x_angle,
+            data.y_angle,
+            data.z_angle,
+            data.x_velocity,
+            data.y_velocity,
+            data.z_velocity
+        );
+
+        // Sleep for a short duration to avoid spamming the console
+        thread::sleep(Duration::from_millis(500));
+    }
+}

imu/hexmove/src/lib.rs

@@ -0,0 +1,120 @@
+use log::error;
+use socketcan::{CanFrame, CanSocket, EmbeddedFrame, ExtendedId, Id, Socket};
+use std::sync::{Arc, RwLock};
+use std::thread;
+
+#[derive(Debug, Default, Clone)]
+pub struct ImuData {
+    pub x_angle: f32,
+    pub y_angle: f32,
+    pub z_angle: f32,
+    pub x_velocity: f32,
+    pub y_velocity: f32,
+    pub z_velocity: f32,
+}
+
+pub struct ImuReader {
+    socket: Arc<CanSocket>,
+    data: Arc<RwLock<ImuData>>,
+    running: Arc<RwLock<bool>>,
+}
+
+impl ImuReader {
+    pub fn new(
+        interface: &str,
+        serial_number: u8,
+        model: u8,
+    ) -> Result<Self, Box<dyn std::error::Error>> {
+        let socket = Arc::new(CanSocket::open(interface)?);
+        let data = Arc::new(RwLock::new(ImuData::default()));
+        let running = Arc::new(RwLock::new(true));
+
+        let imu_reader = ImuReader {
+            socket: socket.clone(),
+            data: Arc::clone(&data),
+            running: Arc::clone(&running),
+        };
+
+        imu_reader.start_reading_thread(serial_number, model);
+
+        Ok(imu_reader)
+    }
+
+    fn start_reading_thread(&self, serial_number: u8, model: u8) {
+        let data = Arc::clone(&self.data);
+        let running = Arc::clone(&self.running);
+        let socket = Arc::clone(&self.socket);
+
+        thread::spawn(move || {
+            while *running.read().unwrap() {
+                match socket.read_frame() {
+                    Ok(CanFrame::Data(data_frame)) => {
+                        let received_data = data_frame.data();
+                        let id = data_frame.id();
+
+                        let base_id =
+                            0x0B000000 | (serial_number as u32) << 16 | (model as u32) << 8;
+
+                        if id == Id::Extended(ExtendedId::new(base_id | 0xB1).unwrap()) {
+                            let x_angle = i16::from_le_bytes([received_data[0], received_data[1]])
+                                as f32
+                                * 0.01;
+                            let y_angle = i16::from_le_bytes([received_data[2], received_data[3]])
+                                as f32
+                                * 0.01;
+                            let z_angle = i16::from_le_bytes([received_data[4], received_data[5]])
+                                as f32
+                                * 0.01;
+
+                            let mut imu_data = data.write().unwrap();
+                            imu_data.x_angle = x_angle;
+                            imu_data.y_angle = y_angle;
+                            imu_data.z_angle = z_angle;
+                        }
+
+                        if id == Id::Extended(ExtendedId::new(base_id | 0xB2).unwrap()) {
+                            let x_velocity =
+                                i16::from_le_bytes([received_data[0], received_data[1]]) as f32
+                                    * 0.01;
+                            let y_velocity =
+                                i16::from_le_bytes([received_data[2], received_data[3]]) as f32
+                                    * 0.01;
+                            let z_velocity =
+                                i16::from_le_bytes([received_data[4], received_data[5]]) as f32
+                                    * 0.01;
+
+                            let mut imu_data = data.write().unwrap();
+                            imu_data.x_velocity = x_velocity;
+                            imu_data.y_velocity = y_velocity;
+                            imu_data.z_velocity = z_velocity;
+                        }
+                    }
+                    Ok(CanFrame::Remote(_)) => {
+                        // Ignore remote frames
+                    }
+                    Ok(CanFrame::Error(_)) => {
+                        // Ignore error frames
+                    }
+                    Err(e) => {
+                        error!("Error reading IMU data: {}", e);
+                    }
+                }
+            }
+        });
+    }
+
+    pub fn get_data(&self) -> ImuData {
+        self.data.read().unwrap().clone()
+    }
+
+    pub fn stop(&self) {
+        let mut running = self.running.write().unwrap();
+        *running = false;
+    }
+}
+
+impl Drop for ImuReader {
+    fn drop(&mut self) {
+        self.stop();
+    }
+}

pyproject.toml

@@ -40,7 +40,7 @@ ignore_missing_imports = true
 
 [[tool.mypy.overrides]]
 
-module = ["actuator.bindings"]
+module = ["imu.bindings"]
 
 disable_error_code = ["no-untyped-def"]
 
@@ -73,7 +73,7 @@ dummy-variable-rgx = "^(_+|(_+[a-zA-Z0-9_]*[a-zA-Z0-9]+?))$"
 
 [tool.ruff.lint.isort]
 
-known-first-party = ["actuator", "tests"]
+known-first-party = ["imu", "tests"]
 combine-as-imports = true
 
 [tool.ruff.lint.pydocstyle]