mecha10_controllers/

l298n.rs

//! L298N Motor Controller
//!
//! High-level motor controller wrapping the L298N driver for differential drive robots.
//!
//! # Overview
//!
//! This controller provides a `MotorController` implementation for L298N-based
//! differential drive robots. It wraps the low-level `L298NDriver` and provides:
//!
//! - Differential drive kinematics (twist to wheel velocities)
//! - Velocity normalization and clamping
//! - Health monitoring and diagnostics
//! - Dead-reckoning odometry estimation (no encoder feedback)
//!
//! # Example (active-low motor driver - default)
//!
//! ```rust,no_run
//! use mecha10_controllers::{Controller, MotorController};
//! use mecha10_controllers::l298n::{L298nMotorController, L298nControllerConfig, L298nMotorPins};
//!
//! #[tokio::main]
//! async fn main() -> anyhow::Result<()> {
//!     // Active-low drivers: pull pin LOW to activate direction
//!     let config = L298nControllerConfig {
//!         gpio_chip: "gpiochip0".to_string(),
//!         motor_a: L298nMotorPins { in1: 19, in2: 13, enable: None, invert: false, active_low: true },
//!         motor_b: L298nMotorPins { in1: 6, in2: 5, enable: None, invert: false, active_low: true },
//!         pwm_frequency_hz: 1000,
//!         wheel_base: 0.3,
//!         max_velocity: 1.0,
//!         max_duty_cycle: 0.8,
//!         init_high_pins: vec![4, 11, 12, 15, 16, 17, 18, 20, 21, 22, 23, 24, 25, 26, 27],
//!     };
//!
//!     let mut controller = L298nMotorController::init(config).await?;
//!     controller.start().await?;
//!
//!     // Set velocity (normalized -1.0 to 1.0)
//!     controller.set_velocity(0.5, 0.5).await?; // Forward
//!     controller.set_velocity(0.5, -0.5).await?; // Rotate
//!
//!     controller.stop().await?;
//!     Ok(())
//! }
//! ```
//!
//! # Example (standard L298N with enable pins - active-high)
//!
//! ```rust,no_run
//! use mecha10_controllers::{Controller, MotorController};
//! use mecha10_controllers::l298n::{L298nMotorController, L298nControllerConfig, L298nMotorPins};
//!
//! #[tokio::main]
//! async fn main() -> anyhow::Result<()> {
//!     // Standard L298N with enable pins and active-high logic
//!     let config = L298nControllerConfig {
//!         gpio_chip: "gpiochip0".to_string(),
//!         motor_a: L298nMotorPins { in1: 17, in2: 27, enable: Some(18), invert: false, active_low: false },
//!         motor_b: L298nMotorPins { in1: 22, in2: 23, enable: Some(12), invert: false, active_low: false },
//!         pwm_frequency_hz: 1000,
//!         wheel_base: 0.3,
//!         max_velocity: 1.0,
//!         max_duty_cycle: 0.8,
//!         init_high_pins: vec![],
//!     };
//!
//!     let mut controller = L298nMotorController::init(config).await?;
//!     controller.start().await?;
//!     // ...
//!     controller.stop().await?;
//!     Ok(())
//! }
//! ```
//!
//! # Limitations
//!
//! - No encoder feedback - odometry is estimated from commanded velocities
//! - No current/torque sensing - cannot detect stalls or measure actual torque
//! - Open-loop control only - no closed-loop position or velocity control

use crate::motor::*;
use crate::{Controller, ControllerCapabilities, ControllerError, ControllerHealth, ControllerState};
use async_trait::async_trait;
use mecha10_core::actuator::MotorStatus;
use mecha10_drivers_motor::l298n::{L298NConfig, L298NDriver, L298NMotorPins as DriverMotorPins};
use mecha10_drivers_motor::{ControlMode, MotorCommand, MotorDriver};
use serde::{Deserialize, Serialize};
use std::collections::HashMap;
use std::time::{SystemTime, UNIX_EPOCH};
use tracing::{debug, info, warn};

/// L298N controller configuration
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct L298nControllerConfig {
    /// GPIO chip device (e.g., "gpiochip0")
    pub gpio_chip: String,

    /// Motor A (left) pin configuration
    pub motor_a: L298nMotorPins,

    /// Motor B (right) pin configuration
    pub motor_b: L298nMotorPins,

    /// PWM frequency in Hz
    #[serde(default = "default_pwm_frequency")]
    pub pwm_frequency_hz: u32,

    /// Wheel base (distance between wheels) in meters
    #[serde(default = "default_wheel_base")]
    pub wheel_base: f32,

    /// Maximum velocity (normalized, for scaling)
    #[serde(default = "default_max_velocity")]
    pub max_velocity: f32,

    /// Maximum duty cycle (0.0-1.0) as safety limit
    #[serde(default = "default_max_duty_cycle")]
    pub max_duty_cycle: f32,

    /// Additional GPIO pins to set HIGH on init (for motor driver boards that
    /// need other pins in a known state). Motor pins are excluded automatically.
    #[serde(default = "default_init_high_pins")]
    pub init_high_pins: Vec<u32>,
}

fn default_init_high_pins() -> Vec<u32> {
    vec![4, 11, 12, 15, 16, 17, 18, 20, 21, 22, 23, 24, 25, 26, 27]
}

fn default_pwm_frequency() -> u32 {
    1000
}

fn default_wheel_base() -> f32 {
    0.3
}

fn default_max_velocity() -> f32 {
    1.0
}

fn default_max_duty_cycle() -> f32 {
    1.0
}

impl Default for L298nControllerConfig {
    fn default() -> Self {
        Self {
            gpio_chip: "gpiochip0".to_string(),
            motor_a: L298nMotorPins::default_motor_a(),
            motor_b: L298nMotorPins::default_motor_b(),
            pwm_frequency_hz: default_pwm_frequency(),
            wheel_base: default_wheel_base(),
            max_velocity: default_max_velocity(),
            max_duty_cycle: default_max_duty_cycle(),
            init_high_pins: default_init_high_pins(),
        }
    }
}

/// Pin configuration for a single motor
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct L298nMotorPins {
    /// Direction control pin 1 (IN1 or IN3) - forward direction
    pub in1: u32,

    /// Direction control pin 2 (IN2 or IN4) - reverse direction
    pub in2: u32,

    /// Enable/PWM pin (ENA or ENB) - optional for drivers that use PWM on direction pins
    #[serde(default)]
    pub enable: Option<u32>,

    /// Invert motor direction
    #[serde(default)]
    pub invert: bool,

    /// Active-low logic: pull pin LOW to activate (default: false = active-high)
    /// When true: Forward = IN1 LOW, IN2 HIGH; Stop = both HIGH
    /// When false: Forward = IN1 HIGH, IN2 LOW; Stop = both LOW
    #[serde(default = "default_active_low")]
    pub active_low: bool,
}

fn default_active_low() -> bool {
    true
}

impl L298nMotorPins {
    /// Default pins for Motor A (left motor)
    ///
    /// Default configuration is for Viam Rover 2:
    /// - Pins 19/13 for left motor, active-low, inverted
    pub fn default_motor_a() -> Self {
        Self {
            in1: 19, // Forward
            in2: 13, // Backward
            enable: None,
            invert: true, // Viam Rover 2 requires inversion
            active_low: true,
        }
    }

    /// Default pins for Motor B (right motor)
    ///
    /// Default configuration is for Viam Rover 2:
    /// - Pins 6/5 for right motor, active-low, inverted
    pub fn default_motor_b() -> Self {
        Self {
            in1: 6, // Forward
            in2: 5, // Backward
            enable: None,
            invert: true, // Viam Rover 2 requires inversion
            active_low: true,
        }
    }
}

impl From<L298nMotorPins> for DriverMotorPins {
    fn from(pins: L298nMotorPins) -> Self {
        DriverMotorPins {
            in1: pins.in1,
            in2: pins.in2,
            enable: pins.enable,
            invert: pins.invert,
            active_low: pins.active_low,
        }
    }
}

/// L298N Motor Controller
///
/// Provides differential drive control for robots using L298N dual H-bridge driver.
pub struct L298nMotorController {
    config: L298nControllerConfig,
    driver: L298NDriver,
    state: ControllerState,
    left_velocity: f32,
    right_velocity: f32,
    // Estimated encoder counts (dead reckoning)
    left_encoder: i32,
    right_encoder: i32,
    last_update_us: u64,
}

#[async_trait]
impl Controller for L298nMotorController {
    type Config = L298nControllerConfig;
    type Error = ControllerError;

    async fn init(config: Self::Config) -> Result<Self, Self::Error> {
        info!("Initializing L298N motor controller");
        info!("Wheel base: {} m", config.wheel_base);
        info!("Max velocity: {}", config.max_velocity);
        info!("Max duty cycle: {}%", config.max_duty_cycle * 100.0);

        // Create driver config from controller config
        let driver_config = L298NConfig {
            gpio_chip: config.gpio_chip.clone(),
            motor_a: config.motor_a.clone().into(),
            motor_b: config.motor_b.clone().into(),
            pwm_frequency_hz: config.pwm_frequency_hz,
            max_duty_cycle: config.max_duty_cycle,
            update_rate_hz: 50.0,
            init_high_pins: config.init_high_pins.clone(),
        };

        let mut driver = L298NDriver::new(driver_config);
        driver
            .init()
            .await
            .map_err(|e| ControllerError::InitializationFailed(e.to_string()))?;

        Ok(Self {
            config,
            driver,
            state: ControllerState::Initialized,
            left_velocity: 0.0,
            right_velocity: 0.0,
            left_encoder: 0,
            right_encoder: 0,
            last_update_us: now_us(),
        })
    }

    async fn start(&mut self) -> Result<(), Self::Error> {
        info!("Starting L298N motor controller");

        // Enable both motors
        self.driver
            .enable(0)
            .await
            .map_err(|e| ControllerError::CommunicationError(e.to_string()))?;
        self.driver
            .enable(1)
            .await
            .map_err(|e| ControllerError::CommunicationError(e.to_string()))?;

        self.state = ControllerState::Running;
        self.last_update_us = now_us();
        info!("L298N motor controller started");

        Ok(())
    }

    async fn stop(&mut self) -> Result<(), Self::Error> {
        info!("Stopping L298N motor controller");

        // Stop motors first
        self.set_velocity(0.0, 0.0).await?;

        // Disable motors
        self.driver
            .disable(0)
            .await
            .map_err(|e| ControllerError::CommunicationError(e.to_string()))?;
        self.driver
            .disable(1)
            .await
            .map_err(|e| ControllerError::CommunicationError(e.to_string()))?;

        self.state = ControllerState::Stopped;
        info!("L298N motor controller stopped");

        Ok(())
    }

    async fn health_check(&self) -> ControllerHealth {
        match self.state {
            ControllerState::Running => ControllerHealth::Healthy,
            ControllerState::Initialized => ControllerHealth::Degraded {
                reason: "Controller initialized but not started".to_string(),
            },
            ControllerState::Stopped => ControllerHealth::Degraded {
                reason: "Controller stopped".to_string(),
            },
            ControllerState::Error => ControllerHealth::Unhealthy {
                reason: "Controller in error state".to_string(),
            },
            ControllerState::Uninitialized => ControllerHealth::Unknown,
        }
    }

    fn capabilities(&self) -> ControllerCapabilities {
        ControllerCapabilities::new("motor", "l298n")
            .with_vendor("STMicroelectronics")
            .with_model("L298N Dual H-Bridge")
            .with_feature("velocity_control", true)
            .with_feature("encoders", false) // No encoder feedback
            .with_feature("position_control", false)
            .with_feature("torque_control", false)
            .with_feature("current_sensing", false)
            .with_feature("temperature_sensing", false)
            .with_metadata("wheel_base", format!("{} m", self.config.wheel_base))
            .with_metadata("max_duty_cycle", format!("{}%", self.config.max_duty_cycle * 100.0))
    }

    async fn diagnostics(&self) -> HashMap<String, String> {
        let mut diag = HashMap::new();
        diag.insert("state".to_string(), format!("{:?}", self.state));
        diag.insert("left_velocity".to_string(), format!("{:.2}", self.left_velocity));
        diag.insert("right_velocity".to_string(), format!("{:.2}", self.right_velocity));
        diag.insert("left_encoder_est".to_string(), self.left_encoder.to_string());
        diag.insert("right_encoder_est".to_string(), self.right_encoder.to_string());
        diag.insert("gpio_chip".to_string(), self.config.gpio_chip.clone());
        diag
    }
}

#[async_trait]
impl MotorController for L298nMotorController {
    async fn set_velocity(&mut self, left: f32, right: f32) -> Result<(), Self::Error> {
        if self.state != ControllerState::Running {
            return Err(ControllerError::InvalidState("Controller not running".to_string()));
        }

        // Clamp velocities
        let max_vel = self.config.max_velocity;
        let left_clamped = left.clamp(-max_vel, max_vel);
        let right_clamped = right.clamp(-max_vel, max_vel);

        // Normalize to -1.0 to 1.0 range for PWM
        let left_norm = left_clamped / max_vel;
        let right_norm = right_clamped / max_vel;

        // Update estimated encoder counts based on velocity
        self.update_encoder_estimates(left_norm, right_norm);

        // Store velocities
        self.left_velocity = left_clamped;
        self.right_velocity = right_clamped;

        // Send commands to driver
        // Motor A = left (ID 0), Motor B = right (ID 1)
        let left_cmd = MotorCommand {
            motor_id: 0,
            mode: ControlMode::PWM,
            target: left_norm as f64,
            velocity_limit: None,
            torque_limit: None,
            profile_velocity: None,
            profile_acceleration: None,
        };

        let right_cmd = MotorCommand {
            motor_id: 1,
            mode: ControlMode::PWM,
            target: right_norm as f64,
            velocity_limit: None,
            torque_limit: None,
            profile_velocity: None,
            profile_acceleration: None,
        };

        self.driver
            .send_command(&left_cmd)
            .await
            .map_err(|e| ControllerError::CommunicationError(e.to_string()))?;

        self.driver
            .send_command(&right_cmd)
            .await
            .map_err(|e| ControllerError::CommunicationError(e.to_string()))?;

        debug!(
            "Set velocity: left={:.2}, right={:.2} (norm: {:.2}, {:.2})",
            left_clamped, right_clamped, left_norm, right_norm
        );

        Ok(())
    }

    async fn get_encoders(&mut self) -> Result<(i32, i32), Self::Error> {
        // L298N has no encoders - return estimated values from dead reckoning
        warn!("L298N has no encoders - returning estimated values");
        Ok((self.left_encoder, self.right_encoder))
    }

    async fn get_status(&mut self) -> Result<MotorStatus, Self::Error> {
        // Convert velocity to RPM (assuming some wheel radius)
        let wheel_radius = 0.05; // 5cm wheels assumed
        let vel_to_rpm = 60.0 / (2.0 * std::f32::consts::PI * wheel_radius);

        Ok(MotorStatus {
            timestamp: now_us(),
            left_motor_rpm: self.left_velocity * vel_to_rpm,
            right_motor_rpm: self.right_velocity * vel_to_rpm,
            battery_voltage: 0.0, // Unknown - no sensing
            temperature_c: 0.0,   // Unknown - no sensing
        })
    }

    async fn emergency_stop(&mut self) -> Result<(), Self::Error> {
        info!("Emergency stop triggered!");

        // Immediately stop motors
        self.left_velocity = 0.0;
        self.right_velocity = 0.0;

        // Send stop commands
        let stop_cmd = MotorCommand {
            motor_id: 0,
            mode: ControlMode::PWM,
            target: 0.0,
            velocity_limit: None,
            torque_limit: None,
            profile_velocity: None,
            profile_acceleration: None,
        };

        // Stop both motors
        let _ = self.driver.send_command(&stop_cmd).await;
        let mut stop_cmd_b = stop_cmd;
        stop_cmd_b.motor_id = 1;
        let _ = self.driver.send_command(&stop_cmd_b).await;

        // Disable motors
        let _ = self.driver.disable(0).await;
        let _ = self.driver.disable(1).await;

        self.state = ControllerState::Stopped;
        Ok(())
    }

    async fn enable(&mut self) -> Result<(), Self::Error> {
        self.driver
            .enable(0)
            .await
            .map_err(|e| ControllerError::CommunicationError(e.to_string()))?;
        self.driver
            .enable(1)
            .await
            .map_err(|e| ControllerError::CommunicationError(e.to_string()))?;
        Ok(())
    }

    async fn disable(&mut self) -> Result<(), Self::Error> {
        // Stop first
        self.set_velocity(0.0, 0.0).await?;

        self.driver
            .disable(0)
            .await
            .map_err(|e| ControllerError::CommunicationError(e.to_string()))?;
        self.driver
            .disable(1)
            .await
            .map_err(|e| ControllerError::CommunicationError(e.to_string()))?;
        Ok(())
    }

    async fn reset_encoders(&mut self) -> Result<(), Self::Error> {
        self.left_encoder = 0;
        self.right_encoder = 0;
        info!("Reset estimated encoder counts");
        Ok(())
    }
}

impl L298nMotorController {
    /// Update estimated encoder counts based on velocity (dead reckoning)
    fn update_encoder_estimates(&mut self, left_norm: f32, right_norm: f32) {
        let now = now_us();
        let dt_us = now.saturating_sub(self.last_update_us);
        let dt_s = dt_us as f32 / 1_000_000.0;

        // Assume 1000 encoder counts per second at full speed
        let counts_per_second = 1000.0;

        self.left_encoder += (left_norm * counts_per_second * dt_s) as i32;
        self.right_encoder += (right_norm * counts_per_second * dt_s) as i32;

        self.last_update_us = now;
    }

    /// Get current state
    pub fn state(&self) -> ControllerState {
        self.state
    }

    /// Get configuration
    pub fn config(&self) -> &L298nControllerConfig {
        &self.config
    }
}

/// Get current time in microseconds
fn now_us() -> u64 {
    SystemTime::now().duration_since(UNIX_EPOCH).unwrap().as_micros() as u64
}

#[cfg(test)]
mod tests {
    use super::*;

    #[tokio::test]
    async fn test_controller_init() {
        let config = L298nControllerConfig::default();
        let controller = L298nMotorController::init(config).await;
        assert!(controller.is_ok());
    }

    #[tokio::test]
    async fn test_controller_start_stop() {
        let config = L298nControllerConfig::default();
        let mut controller = L298nMotorController::init(config).await.unwrap();

        controller.start().await.unwrap();
        assert_eq!(controller.state(), ControllerState::Running);

        controller.stop().await.unwrap();
        assert_eq!(controller.state(), ControllerState::Stopped);
    }

    #[tokio::test]
    async fn test_set_velocity() {
        let config = L298nControllerConfig::default();
        let mut controller = L298nMotorController::init(config).await.unwrap();
        controller.start().await.unwrap();

        let result = controller.set_velocity(0.5, 0.5).await;
        assert!(result.is_ok());
    }

    #[tokio::test]
    async fn test_emergency_stop() {
        let config = L298nControllerConfig::default();
        let mut controller = L298nMotorController::init(config).await.unwrap();
        controller.start().await.unwrap();

        controller.set_velocity(1.0, 1.0).await.unwrap();
        controller.emergency_stop().await.unwrap();

        assert_eq!(controller.state(), ControllerState::Stopped);
    }

    #[tokio::test]
    async fn test_capabilities() {
        let config = L298nControllerConfig::default();
        let controller = L298nMotorController::init(config).await.unwrap();

        let caps = controller.capabilities();
        assert_eq!(caps.controller_type, "motor");
        assert_eq!(caps.implementation, "l298n");
        assert!(!caps.supports("encoders"));
        assert!(caps.supports("velocity_control"));
    }
}