motor_codec/
command.rs

1//! Vendor-agnostic motor command surface.
2
3/// A control command directed at a single motor.
4///
5/// Vendor-specific commands (e.g. Damiao's `0x7FF` parameter sub-protocol) do
6/// not appear here; reach them via vendor-extension traits exposed by the
7/// vendor's codec crate.
8#[derive(Copy, Clone, Debug, PartialEq)]
9#[non_exhaustive]
10pub enum Command {
11    /// MIT impedance control: kp, kd gains plus q (position), dq (velocity),
12    /// tau (feed-forward torque) setpoints.
13    Mit {
14        /// Position gain.
15        kp: f64,
16        /// Velocity gain.
17        kd: f64,
18        /// Position setpoint (radians).
19        q: f64,
20        /// Velocity setpoint (rad/s).
21        dq: f64,
22        /// Feed-forward torque (Nm).
23        tau: f64,
24    },
25    /// Position + velocity setpoint control.
26    PosVel {
27        /// Position setpoint (radians).
28        q: f64,
29        /// Velocity setpoint (rad/s).
30        dq: f64,
31    },
32    /// Pure velocity control.
33    Vel {
34        /// Velocity setpoint (rad/s).
35        dq: f64,
36    },
37    /// Position + velocity + current-percent-units control.
38    PosForce {
39        /// Position setpoint (radians).
40        q: f64,
41        /// Velocity setpoint (rad/s).
42        dq: f64,
43        /// Current setpoint in per-unit (0.0 to 1.0).
44        i_pu: f64,
45    },
46}
47
48/// Tagged discriminant of [`Command`], suitable for error-reporting
49/// "this codec does not support mode X".
50#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
51#[non_exhaustive]
52pub enum CommandKind {
53    /// Corresponds to [`Command::Mit`].
54    Mit,
55    /// Corresponds to [`Command::PosVel`].
56    PosVel,
57    /// Corresponds to [`Command::Vel`].
58    Vel,
59    /// Corresponds to [`Command::PosForce`].
60    PosForce,
61}
62
63impl Command {
64    /// Tagged discriminant.
65    pub fn kind(&self) -> CommandKind {
66        match self {
67            Command::Mit { .. } => CommandKind::Mit,
68            Command::PosVel { .. } => CommandKind::PosVel,
69            Command::Vel { .. } => CommandKind::Vel,
70            Command::PosForce { .. } => CommandKind::PosForce,
71        }
72    }
73}
74
75#[cfg(test)]
76mod tests {
77    use super::*;
78
79    #[test]
80    fn all_four_modes_construct() {
81        let mit = Command::Mit {
82            kp: 50.0,
83            kd: 1.0,
84            q: 0.0,
85            dq: 0.0,
86            tau: 0.0,
87        };
88        let pv = Command::PosVel { q: 1.0, dq: 2.0 };
89        let v = Command::Vel { dq: 1.0 };
90        let pf = Command::PosForce {
91            q: 1.0,
92            dq: 2.0,
93            i_pu: 0.5,
94        };
95        assert_eq!(mit.kind(), CommandKind::Mit);
96        assert_eq!(pv.kind(), CommandKind::PosVel);
97        assert_eq!(v.kind(), CommandKind::Vel);
98        assert_eq!(pf.kind(), CommandKind::PosForce);
99    }
100
101    #[test]
102    fn debug_round_trip() {
103        let cmd = Command::Mit {
104            kp: 50.0,
105            kd: 1.0,
106            q: 0.0,
107            dq: 0.0,
108            tau: 0.0,
109        };
110        let s = alloc::format!("{:?}", cmd);
111        assert!(s.contains("Mit"));
112        assert!(s.contains("50"));
113    }
114}
motor_codec/command.rs

motor_codec/
command.rs
