can_motor_control/
motor.rs

1//! Per-motor identity and state cache.
2
3use motor_codec::{Event, MotorTypeId};
4
5/// Vendor fault code as reported in [`motor_codec::Event::Fault`].
6#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
7pub struct FaultCode(pub u16);
8
9/// Identity + state-cache for one motor.
10///
11/// State mutators are `pub(crate)` so user code cannot directly mutate the
12/// cache; updates happen exclusively from the bus-router dispatch path
13/// inside [`crate::Robot::tick`].
14#[derive(Debug, Clone)]
15pub struct Motor {
16    name: String,
17    motor_type: MotorTypeId,
18    send_id: u32,
19    recv_id: u32,
20    position: f64,
21    velocity: f64,
22    torque: f64,
23    t_mos: i16,
24    t_rotor: i16,
25    is_enabled: bool,
26    fault: Option<FaultCode>,
27    state_sequence: u64,
28}
29
30impl Motor {
31    pub(crate) fn new(name: String, motor_type: MotorTypeId, send_id: u32, recv_id: u32) -> Self {
32        Self {
33            name,
34            motor_type,
35            send_id,
36            recv_id,
37            position: 0.0,
38            velocity: 0.0,
39            torque: 0.0,
40            t_mos: 0,
41            t_rotor: 0,
42            is_enabled: false,
43            fault: None,
44            state_sequence: 0,
45        }
46    }
47
48    /// Human-readable motor name from the robot config.
49    pub fn name(&self) -> &str {
50        &self.name
51    }
52
53    /// Vendor type identifier (e.g. `MotorTypeId::Damiao(<DM4340>)`).
54    pub fn motor_type(&self) -> MotorTypeId {
55        self.motor_type
56    }
57
58    /// CAN ID this motor accepts commands on.
59    pub fn send_id(&self) -> u32 {
60        self.send_id
61    }
62
63    /// CAN ID this motor emits state and replies from.
64    pub fn recv_id(&self) -> u32 {
65        self.recv_id
66    }
67
68    /// Most recently received position (radians).
69    pub fn position(&self) -> f64 {
70        self.position
71    }
72
73    /// Most recently received velocity (rad/s).
74    pub fn velocity(&self) -> f64 {
75        self.velocity
76    }
77
78    /// Most recently received torque estimate (Nm).
79    pub fn torque(&self) -> f64 {
80        self.torque
81    }
82
83    /// MOSFET temperature (degrees C).
84    pub fn temperature_mos(&self) -> i16 {
85        self.t_mos
86    }
87
88    /// Rotor temperature (degrees C).
89    pub fn temperature_rotor(&self) -> i16 {
90        self.t_rotor
91    }
92
93    /// True after a successful enable ACK; false after disable.
94    pub fn is_enabled(&self) -> bool {
95        self.is_enabled
96    }
97
98    /// Latched fault code, if any.
99    pub fn fault(&self) -> Option<FaultCode> {
100        self.fault
101    }
102
103    pub(crate) fn state_sequence(&self) -> u64 {
104        self.state_sequence
105    }
106
107    pub(crate) fn apply_event(&mut self, ev: &Event) {
108        match *ev {
109            Event::State {
110                q,
111                dq,
112                tau,
113                t_mos,
114                t_rotor,
115                ..
116            } => {
117                self.position = q;
118                self.velocity = dq;
119                self.torque = tau;
120                self.t_mos = t_mos;
121                self.t_rotor = t_rotor;
122                self.fault = None;
123                self.state_sequence = self.state_sequence.wrapping_add(1);
124                // is_enabled stays whatever the lifecycle setter last set.
125            }
126            Event::Fault { code, .. } => {
127                self.fault = Some(FaultCode(code));
128            }
129            Event::ParamReply { .. } => {
130                // v1 doesn't cache param replies on Motor; user reads them via
131                // a future param-poll API.
132            }
133            _ => {
134                // Non-exhaustive: ignore unknown event variants.
135            }
136        }
137    }
138
139    pub(crate) fn set_enabled(&mut self, on: bool) {
140        self.is_enabled = on;
141    }
142}
143
144#[cfg(test)]
145mod tests {
146    use super::*;
147
148    #[test]
149    fn newly_constructed_has_zero_state() {
150        let m = Motor::new("j0".into(), MotorTypeId::Damiao(3), 0x01, 0x11);
151        assert_eq!(m.name(), "j0");
152        assert_eq!(m.send_id(), 0x01);
153        assert_eq!(m.recv_id(), 0x11);
154        assert_eq!(m.position(), 0.0);
155        assert!(!m.is_enabled());
156        assert!(m.fault().is_none());
157    }
158
159    #[test]
160    fn apply_state_updates_cache() {
161        let mut m = Motor::new("j0".into(), MotorTypeId::Damiao(3), 0x01, 0x11);
162        m.apply_event(&Event::State {
163            motor_id: 0x11,
164            q: 0.5,
165            dq: 0.1,
166            tau: 0.2,
167            t_mos: 30,
168            t_rotor: 35,
169        });
170        assert_eq!(m.position(), 0.5);
171        assert_eq!(m.velocity(), 0.1);
172        assert_eq!(m.torque(), 0.2);
173        assert_eq!(m.temperature_mos(), 30);
174        assert_eq!(m.temperature_rotor(), 35);
175    }
176
177    #[test]
178    fn apply_fault_latches_code() {
179        let mut m = Motor::new("j0".into(), MotorTypeId::Damiao(3), 0x01, 0x11);
180        m.apply_event(&Event::Fault {
181            motor_id: 0x11,
182            code: 7,
183        });
184        assert_eq!(m.fault(), Some(FaultCode(7)));
185    }
186
187    #[test]
188    fn set_enabled_flips_flag() {
189        let mut m = Motor::new("j0".into(), MotorTypeId::Damiao(3), 0x01, 0x11);
190        assert!(!m.is_enabled());
191        m.set_enabled(true);
192        assert!(m.is_enabled());
193        m.set_enabled(false);
194        assert!(!m.is_enabled());
195    }
196}
can_motor_control/motor.rs

can_motor_control/
motor.rs
