rnavp/src/core/motor.rs

use core::fmt::Display;

use serde::{Deserialize, Serialize};

use crate::core::{
    motor::Direction::{CCW, CW},
    units::{Angle, Current, Voltage},
};

///A simple enum for Counterclockwise or Clockwise direction semantics. Makes it easier much clear than a true/false bool
#[derive(Clone, Copy, PartialEq, Eq, Debug, Serialize, Deserialize)]
pub enum Direction {
    ///CounterClockWise
    CCW,
    ///ClockWise
    CW,
}

impl Direction {
    ///Inverts the direction currently stored in this.
    /// Helps with a simple direction change without knowing the current direction.
    pub fn inv_dir(&mut self) {
        match *self {
            Self::CCW => {
                *self = Self::CW;
            }
            Self::CW => {
                *self = Self::CCW;
            }
        }
    }

    ///Changes the current direction based on the sign of the float.
    ///Recommended to use this over the "From" impl for directions that already exist, to prevent werid 0.0 case and NaN problems
    /// This handles 0.0 and NaN by just doing nothing. The "From" impl defualts to CCW for both
    pub fn dir_from_f32(&mut self, value: f32) {
        if value == 0.0 || value.is_nan() {
            return;
        }
        *self = value.into();
    }

    ///Changes the current direction based on the sign of the int.
    ///Recommended to use this over the "From" impl for directions that already exist, to prevent weird 0 case problems
    ///This handles 0 by just doing nothing. The "From" impl defaults to CCW
    pub fn dir_from_i32(&mut self, value: i32) {
        if value == 0 {
            return;
        }
        *self = value.into();
    }
}

///Converts F32 to direction using the number's sign.
/// This defaults to CCW for 0.0 and NaN.
impl From<f32> for Direction {
    fn from(value: f32) -> Self {
        if value < 0.0 { CW } else { CCW }
    }
}

/// Converts i32 to direction using the number's sign.
/// This defaults to CCW for 0.
impl From<i32> for Direction {
    fn from(value: i32) -> Self {
        if value < 0 { CW } else { CCW }
    }
}

///A enum for containerizing speeds easily
/// Lets the backend take in a Rotations per minute value from a user Sensor implementation and convert it to a Radians per second value
///Pay attention to the units of each term
#[derive(Clone, Copy, PartialEq, Debug, Serialize, Deserialize)]
pub enum Speed {
    ///Rotations per minute
    RPM(f32),
    ///Radians per second
    RADS(f32),
}
///Used to convert from Rotations per minute to radians per second
const RPM_TO_RADS: f32 = core::f32::consts::TAU / 60.0;
///Used to convert from Radians per second to Rotations per minute
const RADS_TO_RPM: f32 = 60.0 / core::f32::consts::TAU;

impl Speed {
    ///Returns the value as Radians Per Second regardless of what the enum contains
    /// Uses no runtime division
    #[inline]
    pub fn rads(&self) -> f32 {
        match *self {
            Self::RPM(rpm) => rpm * RADS_TO_RPM,
            Self::RADS(rps) => rps,
        }
    }

    ///Returns the value as Rotations Per minute regardles of what the enum contains
    /// Uses no runtime division
    #[inline]
    pub fn rpm(&self) -> f32 {
        match *self {
            Self::RPM(rpm) => rpm,
            Self::RADS(rps) => rps * RPM_TO_RADS,
        }
    }
}

impl Display for Speed {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(f, "")
    }
}

///This trait is implemented on a motor struct to implement the motor DRIVER (like an ESC) capabalities in how it should control the motor.
/// This will then let other systems use this generic trait and system to control your motor without needing to understand the hardware as much.
pub trait Driver {
    ///Spins in the provided direction
    /// Properly map each direction to CounterClockWise and Clockwise when LOOKING AT THE MOTOR SHAFT. This is VITAL for the way the systems within core::logic will work.
    ///This is best not called manually, use set_speed_and_direction_raw instead
    fn spin(&mut self, dir: Direction);

    ///Commands the motor to stop
    /// This should stop the motor in what ever way you see fit.
    /// Some hardware may have a proper "Stop" or "Off" Signal, some will not.
    /// If there is no proper hardware stop, verify that 0 speed is actually a stopping command and you can easily just set speed to 0 via set_speed_raw in this method.
    /// No default implementation (though this is easily something I could have done) because it SHOULD be something you notice and implement PROPERLY if applicable
    fn stop(&mut self);

    ///This method uses a full u16 to set the motor speed. This does not change spin direction.
    /// You need to map 0 to 65535 properly to your specific hardware range, just because PWM is commonly 0 to 65535 do not assume that ALL ways of controlling a motor cleanly takes a value of 0 to 65535,
    /// you need to verify and map properly.
    /// 65535 should be the max possible speed, while 0 should be stopped.
    ///This is best to not call manually, use set_speed_and_direction_raw instead
    fn set_speed(&mut self, speed: u16);

    ///Commands the motors to use the speed and spin in the same command.
    ///Uses a number between 0 and 65535
    ///This should be used in most cases, unless the motor has the core::logic::motor::controller Trait, then the the methods from that should be used instead.
    fn set_speed_and_direction(&mut self, speed: u16, dir: Direction) {
        //Set direction before speed, so it won't start spinning in one direction, then snap to the other.
        //Most MCUs should execute these two lines of code so fast, that it should be neglible regardless.
        self.spin(dir);
        self.set_speed(speed);
    }
}

///This trait is implemented on motors that have some form of sensory feedback. Note that this does not require the motor ITSELF provide feedback, just that the setup the motor is in supports it. IE, a Current draw based ESC
/// Speed Feedback is REQUIRED, if your device does not provide speed feedback directly,
/// Implement the other conditions and use them to then implement the speed method.
///  
/// Some motors/sensors might have a direct "Current Angle" being reported, the option is there in case yours can, so the communication system can sync it to the host for you.
/// Some sensors may also provide current and voltage feedback, if implemented the communication system can sync them to the host you for.
/// Can also be used for other custom logic. They are unused by RNavP, entirely optional.
pub trait Sensor {
    ///Returns the speed struct value from the motors sensor.
    /// This should be non-blocking, there should be some form of cached value ready to go as soon as the method is called
    ///
    /// IF properly returned in the correct Enum Data field, all other systems will use the value propely, being unit agnostic later.
    fn get_speed(&self) -> Speed;

    ///Returns the current from the sensor, if it capable.
    /// This should be non-blocking. Keep a cached value ready.
    fn get_current(&self) -> Option<Current>;

    ///Returns the current voltage from the sensro, if it is capable.
    /// This should be non-blocking. Keep a cached value ready.
    fn get_voltage(&self) -> Option<Voltage>;

    ///Returns the current angle from the sensor if it is capable.
    /// This should be non-blocking. Keep a cached value ready.
    fn get_angle(&self) -> Option<Angle>;
}