mecha10_controllers/mock/
motor.rs

1//! Mock motor controller for testing
2
3use crate::motor::*;
4use crate::{Controller, ControllerCapabilities, ControllerError, ControllerHealth, ControllerState};
5use async_trait::async_trait;
6use mecha10_core::actuator::MotorStatus;
7use std::time::{SystemTime, UNIX_EPOCH};
8
9#[derive(Debug, Clone)]
10pub struct MockMotorConfig {
11    pub wheel_base: f32,
12    pub max_velocity: f32,
13}
14
15impl Default for MockMotorConfig {
16    fn default() -> Self {
17        Self {
18            wheel_base: 0.3,
19            max_velocity: 1.0,
20        }
21    }
22}
23
24pub struct MockMotorController {
25    config: MockMotorConfig,
26    state: ControllerState,
27    left_velocity: f32,
28    right_velocity: f32,
29    left_encoder: i32,
30    right_encoder: i32,
31    enabled: bool,
32}
33
34#[async_trait]
35impl Controller for MockMotorController {
36    type Config = MockMotorConfig;
37    type Error = ControllerError;
38
39    async fn init(config: Self::Config) -> Result<Self, Self::Error> {
40        Ok(Self {
41            config,
42            state: ControllerState::Initialized,
43            left_velocity: 0.0,
44            right_velocity: 0.0,
45            left_encoder: 0,
46            right_encoder: 0,
47            enabled: false,
48        })
49    }
50
51    async fn start(&mut self) -> Result<(), Self::Error> {
52        self.state = ControllerState::Running;
53        self.enabled = true;
54        Ok(())
55    }
56
57    async fn stop(&mut self) -> Result<(), Self::Error> {
58        self.state = ControllerState::Stopped;
59        self.left_velocity = 0.0;
60        self.right_velocity = 0.0;
61        Ok(())
62    }
63
64    async fn health_check(&self) -> ControllerHealth {
65        match self.state {
66            ControllerState::Running => ControllerHealth::Healthy,
67            _ => ControllerHealth::Unknown,
68        }
69    }
70
71    fn capabilities(&self) -> ControllerCapabilities {
72        ControllerCapabilities::new("motor", "mock")
73            .with_vendor("Mecha10")
74            .with_model("Mock Differential Drive")
75            .with_feature("encoders", true)
76            .with_feature("velocity_control", true)
77    }
78}
79
80#[async_trait]
81impl MotorController for MockMotorController {
82    async fn set_velocity(&mut self, left: f32, right: f32) -> Result<(), Self::Error> {
83        if !self.enabled {
84            return Err(ControllerError::InvalidState("Motors not enabled".to_string()));
85        }
86
87        self.left_velocity = left.clamp(-self.config.max_velocity, self.config.max_velocity);
88        self.right_velocity = right.clamp(-self.config.max_velocity, self.config.max_velocity);
89
90        // Simulate encoder updates
91        self.left_encoder += (self.left_velocity * 100.0) as i32;
92        self.right_encoder += (self.right_velocity * 100.0) as i32;
93
94        Ok(())
95    }
96
97    async fn get_encoders(&mut self) -> Result<(i32, i32), Self::Error> {
98        Ok((self.left_encoder, self.right_encoder))
99    }
100
101    async fn get_status(&mut self) -> Result<MotorStatus, Self::Error> {
102        Ok(MotorStatus {
103            timestamp: SystemTime::now().duration_since(UNIX_EPOCH).unwrap().as_micros() as u64,
104            left_motor_rpm: self.left_velocity * 60.0,
105            right_motor_rpm: self.right_velocity * 60.0,
106            battery_voltage: 12.0,
107            temperature_c: 25.0,
108        })
109    }
110
111    async fn emergency_stop(&mut self) -> Result<(), Self::Error> {
112        self.left_velocity = 0.0;
113        self.right_velocity = 0.0;
114        self.enabled = false;
115        Ok(())
116    }
117
118    async fn enable(&mut self) -> Result<(), Self::Error> {
119        self.enabled = true;
120        Ok(())
121    }
122
123    async fn disable(&mut self) -> Result<(), Self::Error> {
124        self.enabled = false;
125        self.left_velocity = 0.0;
126        self.right_velocity = 0.0;
127        Ok(())
128    }
129
130    async fn reset_encoders(&mut self) -> Result<(), Self::Error> {
131        self.left_encoder = 0;
132        self.right_encoder = 0;
133        Ok(())
134    }
135}
mecha10_controllers/mock/motor.rs

mecha10_controllers/mock/
motor.rs
