mecha10_controllers/
motor.rs

1//! Motor controller trait and types
2
3use crate::{Controller, ControllerError};
4use async_trait::async_trait;
5use mecha10_core::actuator::{MotorStatus, Twist};
6use serde::{Deserialize, Serialize};
7
8/// Motor controller trait
9///
10/// Provides a unified interface for different motor types:
11/// - Differential drive robots
12/// - Servo motors (Dynamixel, etc.)
13/// - Stepper motors
14/// - Brushless DC motors (ODrive, VESC)
15#[async_trait]
16pub trait MotorController: Controller {
17    /// Set velocity command
18    ///
19    /// For differential drive, this sets linear and angular velocities.
20    /// For other motor types, the interpretation may vary.
21    async fn set_velocity(&mut self, left: f32, right: f32) -> Result<(), Self::Error>;
22
23    /// Set twist command (linear and angular velocities)
24    ///
25    /// Higher-level command for differential drive robots.
26    async fn set_twist(&mut self, twist: Twist) -> Result<(), Self::Error> {
27        // Convert twist to left/right wheel velocities
28        // This is a default implementation assuming differential drive
29        let wheel_base = 0.3; // Default wheel base in meters
30        let left = twist.linear - (twist.angular * wheel_base / 2.0);
31        let right = twist.linear + (twist.angular * wheel_base / 2.0);
32        self.set_velocity(left, right).await
33    }
34
35    /// Get motor encoder readings
36    ///
37    /// Returns (left, right) encoder counts or positions.
38    async fn get_encoders(&mut self) -> Result<(i32, i32), Self::Error>;
39
40    /// Get motor status (velocities, current, temperature, etc.)
41    async fn get_status(&mut self) -> Result<MotorStatus, Self::Error>;
42
43    /// Emergency stop - immediately halt all motors
44    async fn emergency_stop(&mut self) -> Result<(), Self::Error>;
45
46    /// Enable motors (remove safe stop)
47    async fn enable(&mut self) -> Result<(), Self::Error> {
48        Ok(())
49    }
50
51    /// Disable motors (safe stop, motors can freewheel)
52    async fn disable(&mut self) -> Result<(), Self::Error> {
53        Ok(())
54    }
55
56    /// Reset encoder counts
57    async fn reset_encoders(&mut self) -> Result<(), Self::Error>
58    where
59        Self::Error: From<ControllerError>,
60    {
61        Err(Self::Error::from(ControllerError::NotSupported(
62            "Encoder reset not supported".to_string(),
63        )))
64    }
65
66    /// Set PID gains (if supported)
67    async fn set_pid_gains(&mut self, _gains: PidGains) -> Result<(), Self::Error>
68    where
69        Self::Error: From<ControllerError>,
70    {
71        Err(Self::Error::from(ControllerError::NotSupported(
72            "PID control not supported".to_string(),
73        )))
74    }
75
76    /// Get current PID gains (if supported)
77    async fn get_pid_gains(&self) -> Result<PidGains, Self::Error>
78    where
79        Self::Error: From<ControllerError>,
80    {
81        Err(Self::Error::from(ControllerError::NotSupported(
82            "PID control not supported".to_string(),
83        )))
84    }
85}
86
87// ============================================================================
88// Motor Configuration
89// ============================================================================
90
91/// Common motor configuration
92#[derive(Debug, Clone, Serialize, Deserialize)]
93pub struct MotorConfig {
94    /// Control mode
95    pub mode: MotorControlMode,
96
97    /// Maximum velocity (motor-specific units)
98    pub max_velocity: f32,
99
100    /// Maximum acceleration (motor-specific units/s)
101    pub max_acceleration: f32,
102
103    /// Wheel base for differential drive (meters)
104    pub wheel_base: Option<f32>,
105
106    /// Wheel radius for differential drive (meters)
107    pub wheel_radius: Option<f32>,
108
109    /// Encoder ticks per revolution
110    pub encoder_tpr: Option<u32>,
111
112    /// Gear ratio
113    pub gear_ratio: Option<f32>,
114
115    /// PID gains
116    pub pid_gains: Option<PidGains>,
117
118    /// Safety limits
119    pub safety: SafetyLimits,
120}
121
122impl Default for MotorConfig {
123    fn default() -> Self {
124        Self {
125            mode: MotorControlMode::Velocity,
126            max_velocity: 1.0,
127            max_acceleration: 0.5,
128            wheel_base: Some(0.3),
129            wheel_radius: Some(0.05),
130            encoder_tpr: Some(1024),
131            gear_ratio: Some(1.0),
132            pid_gains: None,
133            safety: SafetyLimits::default(),
134        }
135    }
136}
137
138/// Motor control mode
139#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
140pub enum MotorControlMode {
141    /// Open-loop PWM control
142    OpenLoop,
143
144    /// Closed-loop velocity control
145    Velocity,
146
147    /// Closed-loop position control
148    Position,
149
150    /// Torque/current control
151    Torque,
152
153    /// Trajectory following
154    Trajectory,
155}
156
157/// PID control gains
158#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
159pub struct PidGains {
160    /// Proportional gain
161    pub kp: f32,
162
163    /// Integral gain
164    pub ki: f32,
165
166    /// Derivative gain
167    pub kd: f32,
168}
169
170impl Default for PidGains {
171    fn default() -> Self {
172        Self {
173            kp: 1.0,
174            ki: 0.1,
175            kd: 0.01,
176        }
177    }
178}
179
180impl PidGains {
181    pub fn new(kp: f32, ki: f32, kd: f32) -> Self {
182        Self { kp, ki, kd }
183    }
184}
185
186/// Safety limits for motors
187#[derive(Debug, Clone, Serialize, Deserialize)]
188pub struct SafetyLimits {
189    /// Maximum current per motor (Amps)
190    pub max_current: Option<f32>,
191
192    /// Maximum temperature (Celsius)
193    pub max_temperature: Option<f32>,
194
195    /// Maximum voltage
196    pub max_voltage: Option<f32>,
197
198    /// Minimum voltage (battery low threshold)
199    pub min_voltage: Option<f32>,
200
201    /// Enable watchdog timeout
202    pub watchdog_enabled: bool,
203
204    /// Watchdog timeout (seconds)
205    pub watchdog_timeout: f32,
206}
207
208impl Default for SafetyLimits {
209    fn default() -> Self {
210        Self {
211            max_current: Some(5.0),
212            max_temperature: Some(80.0),
213            max_voltage: Some(16.0),
214            min_voltage: Some(10.0),
215            watchdog_enabled: true,
216            watchdog_timeout: 1.0,
217        }
218    }
219}
220
221// ============================================================================
222// Motor Status and Diagnostics
223// ============================================================================
224
225/// Detailed motor status
226#[derive(Debug, Clone, Serialize, Deserialize)]
227pub struct MotorStatusDetailed {
228    /// Timestamp
229    pub timestamp: u64,
230
231    /// Left motor state
232    pub left: MotorState,
233
234    /// Right motor state
235    pub right: MotorState,
236
237    /// Battery voltage
238    pub battery_voltage: f32,
239
240    /// System temperature
241    pub temperature: f32,
242
243    /// Emergency stop active
244    pub emergency_stop: bool,
245
246    /// Motors enabled
247    pub enabled: bool,
248
249    /// Error codes
250    pub errors: Vec<MotorError>,
251}
252
253/// Individual motor state
254#[derive(Debug, Clone, Serialize, Deserialize)]
255pub struct MotorState {
256    /// Current velocity (motor-specific units)
257    pub velocity: f32,
258
259    /// Current position/encoder count
260    pub position: i32,
261
262    /// Current draw (Amps)
263    pub current: f32,
264
265    /// Motor temperature (Celsius)
266    pub temperature: f32,
267
268    /// PWM duty cycle (0.0 - 1.0)
269    pub pwm: f32,
270
271    /// Motor errors
272    pub errors: Vec<String>,
273}
274
275impl Default for MotorState {
276    fn default() -> Self {
277        Self {
278            velocity: 0.0,
279            position: 0,
280            current: 0.0,
281            temperature: 25.0,
282            pwm: 0.0,
283            errors: Vec::new(),
284        }
285    }
286}
287
288/// Motor error codes
289#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
290pub enum MotorError {
291    /// Overcurrent detected
292    Overcurrent,
293
294    /// Overtemperature detected
295    Overtemperature,
296
297    /// Undervoltage (battery low)
298    Undervoltage,
299
300    /// Overvoltage
301    Overvoltage,
302
303    /// Encoder failure
304    EncoderFailure,
305
306    /// Communication timeout
307    CommunicationTimeout,
308
309    /// Motor stall detected
310    MotorStall,
311
312    /// Hardware fault
313    HardwareFault,
314
315    /// Invalid command
316    InvalidCommand,
317
318    /// Watchdog timeout
319    WatchdogTimeout,
320}
321
322impl std::fmt::Display for MotorError {
323    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
324        match self {
325            Self::Overcurrent => write!(f, "Overcurrent"),
326            Self::Overtemperature => write!(f, "Overtemperature"),
327            Self::Undervoltage => write!(f, "Undervoltage"),
328            Self::Overvoltage => write!(f, "Overvoltage"),
329            Self::EncoderFailure => write!(f, "Encoder failure"),
330            Self::CommunicationTimeout => write!(f, "Communication timeout"),
331            Self::MotorStall => write!(f, "Motor stall"),
332            Self::HardwareFault => write!(f, "Hardware fault"),
333            Self::InvalidCommand => write!(f, "Invalid command"),
334            Self::WatchdogTimeout => write!(f, "Watchdog timeout"),
335        }
336    }
337}
338
339// ============================================================================
340// Differential Drive Helpers
341// ============================================================================
342
343/// Differential drive kinematics helper
344pub struct DifferentialDrive {
345    /// Wheel base (distance between wheels) in meters
346    pub wheel_base: f32,
347
348    /// Wheel radius in meters
349    pub wheel_radius: f32,
350}
351
352impl DifferentialDrive {
353    pub fn new(wheel_base: f32, wheel_radius: f32) -> Self {
354        Self {
355            wheel_base,
356            wheel_radius,
357        }
358    }
359
360    /// Convert twist to wheel velocities
361    pub fn twist_to_wheels(&self, twist: &Twist) -> (f32, f32) {
362        let left = twist.linear - (twist.angular * self.wheel_base / 2.0);
363        let right = twist.linear + (twist.angular * self.wheel_base / 2.0);
364        (left, right)
365    }
366
367    /// Convert wheel velocities to twist
368    pub fn wheels_to_twist(&self, left: f32, right: f32) -> Twist {
369        let linear = (left + right) / 2.0;
370        let angular = (right - left) / self.wheel_base;
371        Twist { linear, angular }
372    }
373
374    /// Convert wheel velocities (rad/s) to linear velocities (m/s)
375    pub fn angular_to_linear(&self, left_rad: f32, right_rad: f32) -> (f32, f32) {
376        let left = left_rad * self.wheel_radius;
377        let right = right_rad * self.wheel_radius;
378        (left, right)
379    }
380
381    /// Convert linear velocities (m/s) to angular velocities (rad/s)
382    pub fn linear_to_angular(&self, left_m: f32, right_m: f32) -> (f32, f32) {
383        let left = left_m / self.wheel_radius;
384        let right = right_m / self.wheel_radius;
385        (left, right)
386    }
387}
388
389// ============================================================================
390// Trajectory Types
391// ============================================================================
392
393/// Trajectory point for trajectory following mode
394#[derive(Debug, Clone, Serialize, Deserialize)]
395pub struct TrajectoryPoint {
396    /// Time from start (seconds)
397    pub time: f32,
398
399    /// Position setpoint
400    pub position: f32,
401
402    /// Velocity setpoint
403    pub velocity: f32,
404
405    /// Acceleration setpoint
406    pub acceleration: f32,
407}
408
409/// Trajectory for motor control
410#[derive(Debug, Clone, Serialize, Deserialize)]
411pub struct Trajectory {
412    /// Trajectory points
413    pub points: Vec<TrajectoryPoint>,
414
415    /// Total duration (seconds)
416    pub duration: f32,
417}
418
419impl Trajectory {
420    pub fn new() -> Self {
421        Self {
422            points: Vec::new(),
423            duration: 0.0,
424        }
425    }
426
427    pub fn add_point(&mut self, point: TrajectoryPoint) {
428        if point.time > self.duration {
429            self.duration = point.time;
430        }
431        self.points.push(point);
432    }
433
434    pub fn is_empty(&self) -> bool {
435        self.points.is_empty()
436    }
437}
438
439impl Default for Trajectory {
440    fn default() -> Self {
441        Self::new()
442    }
443}
mecha10_controllers/motor.rs

mecha10_controllers/
motor.rs
