owb_core/utils/controllers/
i2c.rs

1//! I2C device management for the Omni-Wheel Bot.
2//!
3//! This module provides abstractions for initializing and controlling motor PWM drivers
4//! and the IMU sensor over a shared I2C bus. Commands are received via `I2C_CHANNEL`.
5
6use crate::utils;
7use core::cell::RefCell;
8
9use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
10use embedded_hal::i2c::I2c;
11use embedded_hal_bus::i2c::RefCellDevice;
12use icm42670::{
13    accelerometer::{Accelerometer, Error as AccelerometerError},
14    Address as ImuAddress, Error as ImuError, Icm42670, PowerMode,
15};
16use pwm_pca9685::{Address as PwmAddress, Channel, Error as PwmError, Pca9685};
17use serde::{Deserialize, Serialize};
18
19/// Channel used to receive I2C commands (`I2CCommand` messages).
20pub static I2C_CHANNEL: embassy_sync::channel::Channel<CriticalSectionRawMutex, I2CCommand, 16> =
21    embassy_sync::channel::Channel::new();
22
23/// Errors that can occur when interacting with I2C-based devices.
24#[derive(Debug)]
25pub enum DeviceError<E: core::fmt::Debug> {
26    PwmError(PwmError<E>),
27    ImuError(ImuError<E>),
28    AccelError(AccelerometerError<ImuError<E>>),
29    ImuNotInitialized,
30    PwmNotInitialized,
31}
32
33/// I2C command variants for motion control and device management.
34///
35/// Serialized as JSON with tag `"ic"`.
36#[derive(Debug, Serialize, Deserialize, Clone, Copy)]
37#[serde(tag = "ic", rename_all = "snake_case")]
38pub enum I2CCommand {
39    // Motion Control Variants
40    /// Omnidirectional translation (no rotation).
41    T { d: f32, s: f32 },
42    /// Pure rotation in place (yaw).
43    Y { s: f32, o: Option<f32> },
44    /// Combined translational and rotational command.
45    O {
46        d: f32,
47        s: f32,
48        rs: f32,
49        o: Option<f32>,
50    },
51
52    // Device Management Variants
53    /// Read IMU sensor data (accelerometer, gyro, temperature).
54    ReadIMU,
55    /// Enable I2C-connected devices.
56    Enable,
57    /// Disable I2C-connected devices.
58    Disable,
59}
60
61/// High-level driver for PWM motor controller and IMU over a shared I2C bus.
62pub struct I2CDevices<'a, I2C: 'static> {
63    #[allow(dead_code)]
64    i2c: &'a RefCell<I2C>,
65    pub pwm: Option<Pca9685<RefCellDevice<'a, I2C>>>,
66    imu: Option<Icm42670<RefCellDevice<'a, I2C>>>,
67    motor_channels: [(Channel, Channel); 3],
68    embodied: utils::ek,
69}
70
71impl<'a, I2C, E> I2CDevices<'a, I2C>
72where
73    I2C: I2c<Error = E> + 'static,
74    E: core::fmt::Debug,
75{
76    /// Create a new I2CDevices manager with specified wheel and robot dimensions.
77    pub fn new(
78        i2c_bus: &'a RefCell<I2C>,
79        wheel_radius: f32,
80        robot_radius: f32,
81    ) -> Self {
82        I2CDevices {
83            i2c: i2c_bus,
84            pwm: None,
85            imu: None,
86            motor_channels: [
87                (Channel::C6, Channel::C7),
88                (Channel::C2, Channel::C3),
89                (Channel::C4, Channel::C5),
90            ],
91            embodied: utils::ek::new(wheel_radius, robot_radius),
92        }
93    }
94    /// Initialize the IMU and PWM motor controller on the I2C bus.
95    ///
96    /// On success, both `self.imu` and `self.pwm` are set. Returns error if initialization fails.
97    pub fn init_devices(&mut self) -> Result<(), DeviceError<E>> {
98        let imu = Icm42670::new(RefCellDevice::new(&self.i2c), ImuAddress::Primary)
99            .map_err(DeviceError::ImuError)?;
100        let pwm = Pca9685::new(RefCellDevice::new(&self.i2c), PwmAddress::from(0x55))
101            .map_err(DeviceError::PwmError)?;
102
103        self.imu = Some(imu);
104        self.pwm = Some(pwm);
105        Ok(())
106    }
107    /// Scan the I2C bus for devices and log any found addresses.
108    pub fn scan_bus(&self) {
109        let mut bus = self.i2c.borrow_mut();
110        for addr in 0x03..0x78 {
111            if bus.write(addr, &[]).is_ok() {
112                tracing::warn!("I2C device found at 0x{:02X}", addr);
113            }
114        }
115    }
116    /// Configure and enable the PWM motor driver (prescale to 60Hz).
117    pub fn configure_pwm(&mut self) -> Result<(), DeviceError<E>> {
118        if let Some(pca) = &mut self.pwm {
119            pca.enable().map_err(DeviceError::PwmError)?;
120            tracing::info!("PWM enabled");
121            pca.set_prescale(100).map_err(DeviceError::PwmError)?;
122            tracing::info!("PWM prescale set to 60Hz");
123        } else {
124            tracing::error!("PWM not initialized");
125        }
126
127        Ok(())
128    }
129    /// Perform an initial IMU data read and log accelerometer, gyro, and temperature.
130    pub fn init_imu_data(&mut self) {
131        match self.read_imu() {
132            Ok((accel, gyro, temp)) => {
133                tracing::info!("Initial IMU read successful:");
134                tracing::info!("Accelerometer: {:?}", accel);
135                tracing::info!("Gyroscope: {:?}", gyro);
136                tracing::info!("Temperature: {:?}", temp);
137            }
138            Err(e) => {
139                tracing::error!("Failed to read IMU data: {:?}", e);
140            }
141        }
142    }
143
144    /// Execute a high-level `I2CCommand`, performing motion or sensor operations.
145    ///
146    /// Returns sensor data for `ReadIMU` or `None` for other commands.
147    pub fn execute_command(
148        &mut self,
149        command: I2CCommand,
150    ) -> Result<Option<((f32, f32, f32), (f32, f32, f32), f32)>, DeviceError<E>> {
151        match command {
152            I2CCommand::T { d, s } => {
153                self.set_motor_velocities_strafe(d, s)?;
154                Ok(None)
155            }
156            I2CCommand::Y { s, o } => {
157                self.set_motor_velocities_rotate(s, o)?;
158                Ok(None)
159            }
160            I2CCommand::O { d, s, rs, o } => {
161                let orientation = o.unwrap_or(0.0);
162                let new_orientation = (orientation + rs) % 360.0;
163                let wheel_speeds =
164                    self.embodied
165                        .compute_wheel_velocities(s, d, new_orientation, rs);
166                self.apply_wheel_speeds(&wheel_speeds)?;
167                Ok(None)
168            }
169            I2CCommand::ReadIMU => Ok(Some(self.read_imu()?)),
170            I2CCommand::Enable => {
171                self.enable()?;
172                Ok(None)
173            }
174            I2CCommand::Disable => {
175                self.disable()?;
176                Ok(None)
177            }
178        }
179    }
180
181    /// Computes and applies motor speeds for strafing.
182    fn set_motor_velocities_strafe(
183        &mut self,
184        direction: f32,
185        speed: f32,
186    ) -> Result<(), DeviceError<E>> {
187        let wheel_speeds = self
188            .embodied
189            .compute_wheel_velocities(speed, direction, 0.0, 0.0);
190        self.apply_wheel_speeds(&wheel_speeds)
191    }
192
193    /// Computes and applies motor speeds for rotation.
194    fn set_motor_velocities_rotate(
195        &mut self,
196        speed: f32,
197        orientation: Option<f32>,
198    ) -> Result<(), DeviceError<E>> {
199        let new_orientation = (orientation.unwrap_or(0.0) + speed) % 360.0;
200        let wheel_speeds = self
201            .embodied
202            .compute_wheel_velocities(0.0, 0.0, new_orientation, speed);
203        self.apply_wheel_speeds(&wheel_speeds)
204    }
205
206    /// Applies calculated motor speeds using the PWM driver.
207    pub fn apply_wheel_speeds(
208        &mut self,
209        wheel_speeds: &[f32],
210    ) -> Result<(), DeviceError<E>> {
211        const MAX_DUTY: u16 = 4095;
212
213        for (i, &(phase_channel, enable_channel)) in self.motor_channels.iter().enumerate() {
214            let speed = wheel_speeds[i].abs().min(1.0);
215            let direction = wheel_speeds[i] >= 0.0;
216
217            if let Some(pca) = &mut self.pwm {
218                pca.set_channel_on_off(phase_channel, 0, if direction { 0 } else { MAX_DUTY })
219                    .map_err(DeviceError::PwmError)?;
220                pca.set_channel_on_off(enable_channel, 0, (speed * MAX_DUTY as f32) as u16)
221                    .map_err(DeviceError::PwmError)?;
222            } else {
223                tracing::error!("PWM not initialized");
224            }
225        }
226        Ok(())
227    }
228    #[allow(dead_code)]
229    fn apply_wheels_bulk(
230        &mut self,
231        _wheels: &[f32],
232    ) -> Result<(), DeviceError<E>> {
233        todo!("Need to implement function for bulk all on and off for simulations changes")
234    }
235
236    /// Read accelerometer, gyroscope, and temperature data from the IMU.
237    ///
238    /// # Returns
239    ///
240    /// `Ok(((ax, ay, az), (gx, gy, gz), temp))` on success.
241    pub fn read_imu(&mut self) -> Result<((f32, f32, f32), (f32, f32, f32), f32), DeviceError<E>> {
242        let imu = self.imu.as_mut().ok_or(DeviceError::ImuNotInitialized)?;
243        let accel = imu.accel_norm().map_err(DeviceError::AccelError)?;
244        let gyro = imu.gyro_norm().map_err(DeviceError::ImuError)?;
245        let temp = imu.temperature().map_err(DeviceError::ImuError)?;
246
247        Ok(((accel.x, accel.y, accel.z), (gyro.x, gyro.y, gyro.z), temp))
248    }
249
250    /// Enable the PWM motor controller and power up the IMU sensor.
251    pub fn enable(&mut self) -> Result<(), DeviceError<E>> {
252        if let Some(pca) = self.pwm.as_mut() {
253            pca.enable().map_err(DeviceError::PwmError)?;
254        }
255
256        if let Some(imu) = self.imu.as_mut() {
257            imu.set_power_mode(PowerMode::SixAxisLowNoise)
258                .map_err(DeviceError::ImuError)?;
259        }
260
261        Ok(())
262    }
263
264    /// Disable motor PWM and put the IMU into sleep mode.
265    pub fn disable(&mut self) -> Result<(), DeviceError<E>> {
266        if let Some(pca) = self.pwm.as_mut() {
267            pca.disable().map_err(DeviceError::PwmError)?;
268        }
269
270        if let Some(imu) = self.imu.as_mut() {
271            imu.set_power_mode(PowerMode::Sleep)
272                .map_err(DeviceError::ImuError)?;
273        }
274
275        Ok(())
276    }
277}
owb_core/utils/controllers/i2c.rs

owb_core/utils/controllers/
i2c.rs
