diff --git a/examples/hexmove.py b/examples/hexmove.py new file mode 100644 index 0000000..2962f58 --- /dev/null +++ b/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() diff --git a/imu/__init__.py b/imu/__init__.py index 505b5d0..f022545 100644 --- a/imu/__init__.py +++ b/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, +) diff --git a/imu/bindings.pyi b/imu/bindings.pyi new file mode 120000 index 0000000..99c2419 --- /dev/null +++ b/imu/bindings.pyi @@ -0,0 +1 @@ +bindings/bindings.pyi \ No newline at end of file diff --git a/imu/bindings/bindings.pyi b/imu/bindings/bindings.pyi new file mode 100644 index 0000000..d5010d2 --- /dev/null +++ b/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: + ... + + diff --git a/imu/bindings/src/lib.rs b/imu/bindings/src/lib.rs index 89b7cb0..c34082b 100644 --- a/imu/bindings/src/lib.rs +++ b/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>, +} + +#[gen_stub_pymethods] +#[pymethods] +impl PyHexmoveImuReader { + #[new] + fn new(interface: String, serial_number: u8, model: u8) -> PyResult { + let imu_reader = HexmoveImuReader::new(&interface, serial_number, model) + .map_err(|e| PyErr::new::(e.to_string()))?; + Ok(PyHexmoveImuReader { + inner: Arc::new(Mutex::new(imu_reader)), + }) + } + + fn get_data(&self) -> PyResult { + 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 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) -> PyResult<()> { + m.add_class::()?; + m.add_class::()?; Ok(()) } diff --git a/imu/hexmove/Cargo.toml b/imu/hexmove/Cargo.toml index ef49e5e..6beb8c6 100644 --- a/imu/hexmove/Cargo.toml +++ b/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" diff --git a/imu/hexmove/src/bin/log.rs b/imu/hexmove/src/bin/log.rs new file mode 100644 index 0000000..1e1d26b --- /dev/null +++ b/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)); + } +} diff --git a/imu/hexmove/src/lib.rs b/imu/hexmove/src/lib.rs index e69de29..de00170 100644 --- a/imu/hexmove/src/lib.rs +++ b/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, + data: Arc>, + running: Arc>, +} + +impl ImuReader { + pub fn new( + interface: &str, + serial_number: u8, + model: u8, + ) -> Result> { + 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(); + } +} diff --git a/pyproject.toml b/pyproject.toml index ca999df..0450ab2 100644 --- a/pyproject.toml +++ b/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]