tele0592 1.2.0

//! High-level interface with the DC motor driver controller

use crate::device::{Device, Error, Result, WhoAmI};
use core::slice;
use core::time::Duration;
use low_level::{
    CMD_CONTROLLED_MOTOR_SPEED, CMD_COUNTERS, CMD_ENCODER_TICKS, CMD_FIRMWARE_FEATURES,
    CMD_MOTOR_SHUTDOWN_TIMEOUT, CMD_PID_I_ACC, CMD_PID_K_D, CMD_PID_K_I, CMD_PID_K_P,
    CMD_PWM_FREQUENCY, CMD_RAW_ENCODER_TICKS, CMD_RAW_MOTOR_SPEED, CMD_STATUS,
};
#[cfg(feature = "float")]
#[allow(unused)] // Needed for when running `clippy tests -F float`
use micromath::F32Ext as _;

pub const REQUIRED_FIRMWARE_VERSION: (u8, u8, u8) = (1, 2, 0);

/// Commands available when the board is in controller mode.
pub trait Controller: Device {
    /// Check firmware version compatibility with this API and return the version of the
    /// controller firmware running on the board.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails, or
    /// if the firmware version is incompatible with this API version.
    fn check_firmware_version(&mut self) -> Result<(u8, u8, u8), Self::I2cError> {
        if !matches!(self.who_am_i()?, WhoAmI::Controller) {
            return Err(Error::UnknownFirmwareFound);
        };
        let version = self.firmware_version()?;
        if REQUIRED_FIRMWARE_VERSION.0 != version.0
            || REQUIRED_FIRMWARE_VERSION.1 > version.1
            || (REQUIRED_FIRMWARE_VERSION.1 == version.1 && REQUIRED_FIRMWARE_VERSION.2 > version.2)
        {
            Err(Error::InvalidVersion(version))
        } else {
            Ok(version)
        }
    }

    /// Set the PWM frequency used to drive the motors in Hz, between
    /// `1` and `100_000`.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails, or
    /// if the frequency is out of the allowed range.
    fn set_pwm_frequency(&mut self, frequency_hz: u32) -> Result<(), Self::I2cError> {
        if frequency_hz > 100_000 {
            return Err(Error::InvalidFrequency(frequency_hz));
        }
        self.write((u32::from(CMD_PWM_FREQUENCY) + (frequency_hz << 8)).to_le_bytes())
    }

    /// Get the PWM frequency used to drive the motors in Hz.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn get_pwm_frequency(&mut self) -> Result<u32, Self::I2cError> {
        let mut data = [0; 4];
        self.write_read([CMD_PWM_FREQUENCY], &mut data[..3])?;
        Ok(u32::from_le_bytes(data))
    }

    /// Set the raw PID coefficients used for the controlled mode.
    /// The coefficients will be scaled by the controller as fixed
    /// point number values with [`PID_FRACTIONAL_BITS`] bits.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn set_raw_pid_coefficients(
        &mut self,
        k_p: i32,
        k_i: i32,
        k_d: i32,
    ) -> Result<(), Self::I2cError> {
        let mut buf = [0; 5];
        for (command, data) in [(CMD_PID_K_P, k_p), (CMD_PID_K_I, k_i), (CMD_PID_K_D, k_d)] {
            buf[0] = command;
            buf[1..5].copy_from_slice(&data.to_le_bytes());
            self.write(buf)?;
        }
        Ok(())
    }

    /// Get the raw PID coefficients used for the controlled mode.
    /// The coefficients have been scaled by the controller as fixed
    /// point number values with [`PID_FRACTIONAL_BITS`] bits.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn get_raw_pid_coefficients(&mut self) -> Result<(i32, i32, i32), Self::I2cError> {
        let mut result = [0i32; 3];
        let mut buf = [0; 4];
        for (command, data) in [CMD_PID_K_P, CMD_PID_K_I, CMD_PID_K_D]
            .into_iter()
            .zip(result.iter_mut())
        {
            self.write_read([command], &mut buf)?;
            *data = i32::from_le_bytes(buf);
        }
        Ok(result.into())
    }

    /// Retrieve the raw value of the PID I accumulators in controlled mode.
    /// The values have been scaled by the controlled as fixed point number
    /// values with [`PID_FRACTIONAL_BITS`] bits.
    ///
    /// In non-controlled mode, the value of (0, 0) is returned.
    ///
    /// _Note: this method is only available when the `float` feature is selected.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn get_raw_pid_i_accumulator(&mut self) -> Result<(i32, i32), Self::I2cError> {
        let mut buf = [0; 8];
        self.write_read([CMD_PID_I_ACC], &mut buf)?;
        Ok((
            i32::from_le_bytes(buf[..4].try_into().unwrap()),
            i32::from_le_bytes(buf[4..].try_into().unwrap()),
        ))
    }

    #[cfg(feature = "float")]
    /// Set the PID coefficients used for the controlled mode.
    /// Some precision may be lost as only [`PID_FRACTIONAL_BITS`] bits
    /// will be used to represent fixed point values.
    ///
    /// _Note: this method is only available when the `float` feature is selected.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn set_pid_coefficients(&mut self, k_p: f32, k_i: f32, k_d: f32) -> Result<(), Self::I2cError> {
        self.set_raw_pid_coefficients(to_i32(k_p), to_i32(k_i), to_i32(k_d))
    }

    #[cfg(feature = "float")]
    /// Get the PID coefficients used for the controlled mode.
    ///
    /// _Note: this method is only available when the `float` feature is selected.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn get_pid_coefficients(&mut self) -> Result<(f32, f32, f32), Self::I2cError> {
        let (k_p, k_i, k_d) = self.get_raw_pid_coefficients()?;
        Ok((to_f32(k_p), to_f32(k_i), to_f32(k_d)))
    }

    #[cfg(feature = "float")]
    /// Retrieve the value of the PID I accumulators in controlled mode.
    /// In non-controlled mode, the value of (0.0, 0.0) is returned.
    ///
    /// _Note: this method is only available when the `float` feature is selected.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn get_pid_i_accumulator(&mut self) -> Result<(f32, f32), Self::I2cError> {
        let (left, right) = self.get_raw_pid_i_accumulator()?;
        Ok((to_f32(left), to_f32(right)))
    }

    /// Set the timeout after which motors will shut down if no
    /// further I²C communication takes place.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn set_motor_shutdown_timeout(&mut self, delay: Duration) -> Result<(), Self::I2cError> {
        let timeout = delay.as_millis();
        if timeout > 10_000 {
            return Err(Error::InvalidDuration(delay));
        }
        let timeout = u8::try_from((timeout + 50) / 100).unwrap();
        self.write([CMD_MOTOR_SHUTDOWN_TIMEOUT, timeout])
    }

    /// Retrieve the current motors shutdown timeout.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn get_motor_shutdown_timeout(&mut self) -> Result<Duration, Self::I2cError> {
        let mut delay = 0;
        self.write_read([CMD_MOTOR_SHUTDOWN_TIMEOUT], slice::from_mut(&mut delay))?;
        Ok(Duration::from_millis(u64::from(delay) * 100))
    }

    /// Set the raw motor speed between -127 and 127 for each. `None` means
    /// that the speed of the corresponding motor is not modified, unless
    /// both sides use `None` in which case the motors will be put in standby
    /// mode.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails or if
    /// an invalid raw speed (-128) is given.
    #[expect(clippy::cast_sign_loss)]
    fn set_raw_motor_speed(
        &mut self,
        left: Option<i8>,
        right: Option<i8>,
    ) -> Result<(), Self::I2cError> {
        let (left, right) = (left.map(|v| v as u8), right.map(|v| v as u8));
        if left == Some(0x80) || right == Some(0x80) {
            return Err(Error::InvalidRawSpeed);
        }
        self.write([
            CMD_RAW_MOTOR_SPEED,
            left.unwrap_or(0x80),
            right.unwrap_or(0x80),
        ])
    }

    /// Get the raw motor speed, or `None` if the motors are in standby mode.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails or if
    /// an invalid speed is received from the controller.
    #[expect(clippy::cast_possible_wrap)]
    fn get_raw_motor_speed(&mut self) -> Result<Option<(i8, i8)>, Self::I2cError> {
        let mut buf = [0; 2];
        self.write_read([CMD_RAW_MOTOR_SPEED], &mut buf)?;
        match buf {
            [0x80, 0x80] => Ok(None),
            [0x80, _] | [_, 0x80] => Err(Error::InvalidRawSpeed),
            [left, right] => Ok(Some((left as i8, right as i8))),
        }
    }

    /// Put both motors in standby mode.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn standby(&mut self) -> Result<(), Self::I2cError> {
        self.set_raw_motor_speed(None, None)
    }

    /// Set the left and right motor speeds. The speed will be controlled
    /// using the internal PID.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn set_motor_speed(&mut self, left: i16, right: i16) -> Result<(), Self::I2cError> {
        let mut buf = [CMD_CONTROLLED_MOTOR_SPEED, 0, 0, 0, 0];
        buf[1..3].copy_from_slice(&left.to_le_bytes());
        buf[3..5].copy_from_slice(&right.to_le_bytes());
        self.write(buf)
    }

    /// Get the left and right motor speeds. If the motors are in
    /// standby mode, or if a raw speed has been explicitly set,
    /// this method will return `None`.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn get_motor_speed(&mut self) -> Result<Option<(i16, i16)>, Self::I2cError> {
        let mut buf = [0; 4];
        self.write_read([CMD_CONTROLLED_MOTOR_SPEED], &mut buf)?;
        Ok((buf != [0x80, 0x00, 0x80, 0x00]).then(|| {
            (
                i16::from_le_bytes(buf[0..2].try_into().unwrap()),
                i16::from_le_bytes(buf[2..4].try_into().unwrap()),
            )
        }))
    }

    /// Retrieve the number of left and right encoder ticks since the
    /// last retrieval.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    #[deprecate_until::deprecate_until(
        remove = ">= 2.x",
        note = "use `Controller::get_relative_encoder_ticks()` instead"
    )]
    fn get_encoder_ticks(&mut self) -> Result<(i16, i16), Self::I2cError> {
        let mut buf = [0; 4];
        self.write_read([CMD_ENCODER_TICKS], &mut buf)?;
        Ok((
            i16::from_le_bytes(buf[0..2].try_into().unwrap()),
            i16::from_le_bytes(buf[2..4].try_into().unwrap()),
        ))
    }

    /// Retrieve the number of left and right encoder ticks as a 16 bit
    /// value for each encoder using wraparound arithmetic.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn get_raw_encoder_ticks(&mut self) -> Result<(u16, u16), Self::I2cError> {
        let mut buf = [0; 4];
        self.write_read([CMD_RAW_ENCODER_TICKS], &mut buf)?;
        Ok((
            u16::from_le_bytes(buf[0..2].try_into().unwrap()),
            u16::from_le_bytes(buf[2..4].try_into().unwrap()),
        ))
    }

    /// Initialize a new [`RelativeEncoders`] from the current encoder values.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn new_relative(&mut self) -> Result<RelativeEncoders, Self::I2cError> {
        let (latest_left, latest_right) = self.get_raw_encoder_ticks()?;
        Ok(RelativeEncoders {
            latest_left,
            latest_right,
        })
    }

    /// Retrieve the number of left and right encoder ticks since the
    /// last time the `relative` value was used.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    #[expect(clippy::cast_possible_wrap)]
    fn get_relative_encoder_ticks(
        &mut self,
        relative: &mut RelativeEncoders,
    ) -> Result<(i16, i16), Self::I2cError> {
        let (new_left, new_right) = self.get_raw_encoder_ticks()?;
        let (left, right) = (
            new_left.wrapping_sub(relative.latest_left) as i16,
            new_right.wrapping_sub(relative.latest_right) as i16,
        );
        *relative = RelativeEncoders {
            latest_left: new_left,
            latest_right: new_right,
        };
        Ok((left, right))
    }

    /// Get the current status of the board.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn get_status(&mut self) -> Result<Status, Self::I2cError> {
        let mut status = 0;
        self.write_read([CMD_STATUS], slice::from_mut(&mut status))?;
        Ok(Status(status))
    }

    /// Get some counters from the board. The counters are, in 256-wrapping arithmetic:
    ///
    /// - The number of BTF events during the hat reception of I²C commands
    /// - The number of unknown I²C command received
    /// - The number of known I²C command whose processing returned an error
    /// - The number of times the watchdog had to stop the moving robot
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn get_counters(&mut self) -> Result<Counters, Self::I2cError> {
        let mut counters = Counters::default();
        self.write_read([CMD_COUNTERS], counters.as_mut())?;
        Ok(counters)
    }

    /// Retrieve the firmware features implemented on the controller.
    ///
    /// # Errors
    /// This method may fail if the communication with the board fails.
    fn firmware_features(&mut self) -> Result<FirmwareFeatures, Self::I2cError> {
        let mut features = 0;
        self.write_read([CMD_FIRMWARE_FEATURES], slice::from_mut(&mut features))?;
        Ok(FirmwareFeatures(features))
    }
}

impl<D: Device> Controller for D {}

/// Controller status returned by the [`Controller::get_status()`] method.
#[derive(Clone, Copy)]
pub struct Status(u8);

impl Status {
    /// Check if any of the motors are moving.
    #[must_use]
    pub fn is_moving(self) -> bool {
        self.0 & 1 != 0
    }

    /// Check if the speed is currently controlled by the internal PID process.
    #[must_use]
    pub fn is_controlled(self) -> bool {
        self.0 & 2 != 0
    }
}

/// Firmware features enabled in the controller returned by the
/// [`Controller::firmware_features()`] method.
#[derive(Clone, Copy)]
pub struct FirmwareFeatures(u8);

impl FirmwareFeatures {
    /// Check if the controller has been compiled with bootloader
    /// support (`true`), or if it is compiled as a standalone
    /// application (`false`).
    #[must_use]
    pub fn has_bootloader_support(self) -> bool {
        self.0 & 1 != 0
    }
}

/// Internal controller counters. All values are stored in 8 bit
/// wrapped arithmetic.
#[repr(C)]
#[derive(Clone, Default)]
pub struct Counters {
    /// - The number of BTF events during the hat reception of I²C commands
    pub btf: u8,
    /// - The number of unknown I²C command received
    pub unknown_command: u8,
    /// - The number of known I²C command whose processing returned an error
    pub incorrect_processing: u8,
    /// - The number of times the watchdog had to stop the moving robot
    pub emergency_stops: u8,
}

impl AsMut<[u8; 4]> for Counters {
    fn as_mut(&mut self) -> &mut [u8; 4] {
        unsafe { (&raw mut *self).cast::<[u8; 4]>().as_mut().unwrap() }
    }
}

/// Structure to remember the previous values of the encoders, to be used
/// with [`Controller::get_relative_encoder_ticks()`]. A new value can
/// be created using [`Controller::new_relative()`].
pub struct RelativeEncoders {
    latest_left: u16,
    latest_right: u16,
}

/// Number of bits by which the PID coefficients are scaled internally
/// by the microcontroller to make a fixed point numbers from an integral
/// number.
pub const PID_FRACTIONAL_BITS: usize = 8;

#[cfg(feature = "float")]
#[expect(clippy::cast_precision_loss)]
fn to_f32(v: i32) -> f32 {
    v as f32 / (1 << PID_FRACTIONAL_BITS) as f32
}

#[cfg(feature = "float")]
#[expect(clippy::cast_possible_truncation, clippy::cast_precision_loss)]
fn to_i32(v: f32) -> i32 {
    (v * (1 << PID_FRACTIONAL_BITS) as f32).round() as i32
}

// Constants used in the low-level protocol between the host and the
// controller firmware.
pub(crate) mod low_level {
    pub const CMD_PWM_FREQUENCY: u8 = 0x10;
    pub const CMD_PID_K_P: u8 = 0x20;
    pub const CMD_PID_K_I: u8 = 0x21;
    pub const CMD_PID_K_D: u8 = 0x22;
    pub const CMD_PID_I_ACC: u8 = 0x26;
    pub const CMD_MOTOR_SHUTDOWN_TIMEOUT: u8 = 0x28;
    pub const CMD_RAW_MOTOR_SPEED: u8 = 0x30;
    pub const CMD_CONTROLLED_MOTOR_SPEED: u8 = 0x31;
    pub const CMD_ENCODER_TICKS: u8 = 0x32;
    pub const CMD_RAW_ENCODER_TICKS: u8 = 0x33;
    pub const CMD_STATUS: u8 = 0x36;
    pub const CMD_COUNTERS: u8 = 0x38;
    pub const CMD_FIRMWARE_FEATURES: u8 = 0xfe;
}