hex_motor/cia402/
sequences.rs

1//! 纯函数：CiA402 控制类操作的 SDO 序列规划。
2//!
3//! 不做 I/O。Manager 拿到 `Vec<SdoWrite>` 后用 `canopen::sdo::download` 逐条
4//! 下发，每条之间按 [`INTER_WRITE_DELAY`] (默认 10 ms) 给电机一点时间应用变化。
5//!
6//! ## 设计基线 —— "统一保险路径"
7//!
8//! v0.1 不去判"当前状态是否允许直接切",  统一走最保险的顺序：
9//! `(可选 fault reset) → 失能 → 写模式 → CiA402 enable ramp`。
10//! 多几个 SDO 不影响上位机使用。详见 `DESIGN.md` §8。
11
12use std::time::Duration;
13
14use crate::canopen::tpdo_config::SdoWrite;
15use crate::error::{Error, Result};
16use crate::types::{MotorMode, MotorTarget};
17
18use super::codec::mode_to_cia402_code;
19use super::types::Logic;
20
21/// 控制字 `0x6040:00`。
22const OD_CONTROL_WORD: u16 = 0x6040;
23/// 操作模式 `0x6060:00`（i8）。
24const OD_MODE_OF_OPERATION: u16 = 0x6060;
25/// PP 目标位置 `0x607A:00`。HexMeow CiA402 按 **f32 Rev** 写（vendor-specific；
26/// 标准 CiA402 是 i32 user-units）。
27const OD_TARGET_POSITION: u16 = 0x607A;
28/// PT 目标力矩 `0x6071:00`，i16 = ‰ of peak_torque (`0x6076`)，范围 -1000..=1000。
29const OD_TARGET_TORQUE: u16 = 0x6071;
30/// PV 速度目标 `0x60FF:00`。HexMeow CiA402 电机按 **f32 Rev/s** 写
31/// （vendor-specific；标准 CiA402 是 i32 user-units）。
32const OD_TARGET_VELOCITY: u16 = 0x60FF;
33/// MIT 控制参数 `0x2003`（uncompressed REAL32 形态，子项见 [`mit`]）。
34const OD_MIT_CONTROL_PARAM: u16 = 0x2003;
35mod mit {
36    pub const SUB_POSITION: u8 = 0x01; // REAL32, Rev
37    pub const SUB_VELOCITY: u8 = 0x02; // REAL32, Rev/s
38    pub const SUB_TORQUE: u8 = 0x03; // REAL32, Nm (feedforward)
39    pub const SUB_KP: u8 = 0x04; // UNSIGNED16, 0..=10000 (kp_int)
40    pub const SUB_KD: u8 = 0x05; // UNSIGNED16, 0..=10000 (kd_int)
41}
42
43/// CiA402 控制字常用值。
44mod cw {
45    /// `0x06`: Shutdown.
46    pub const SHUTDOWN: u16 = 0x0006;
47    /// `0x07`: Switch On.
48    pub const SWITCH_ON: u16 = 0x0007;
49    /// `0x0F`: Enable Operation（bits 0..3 = 1）。
50    pub const ENABLE_OPERATION: u16 = 0x000F;
51    /// `0x80`: Fault Reset (bit 7)。
52    pub const FAULT_RESET: u16 = 0x0080;
53    /// `0x2F = 0x0F | bit5`：Enable Operation + Change Set Immediately
54    /// (PP mode)。每次 `set_target(Position)` 前先写这个把 bit4 落回 0。
55    pub const ENABLE_PP_NEW_SP_CLEARED: u16 = 0x002F;
56    /// `0x3F = 0x0F | bit4 | bit5`：Enable Operation + New Set-Point +
57    /// Change Set Immediately (PP mode)。bit4 的上升沿告诉电机"收下新
58    /// 目标"。
59    pub const ENABLE_PP_NEW_SP_LATCHED: u16 = 0x003F;
60}
61
62/// `set_target` 时除 target 本身外需要从 Manager 缓存里取的上下文。
63///
64/// `current_mode` 必填（从 [`MotorEntry`] 的 `target_mode` 取）；其它
65/// 字段只有相应模式才需要：
66/// - `peak_torque_nm`：`Torque` target 把 Nm → `0x6071` i16 ‰ 时用
67/// - `mit_kp_kd_factor`：`Mit` target 把 Nm/Rev → `0x2003:04/05` u16
68///   `kp_int`/`kd_int` 时用
69///
70/// [`MotorEntry`]: crate::cia402::motor_entry::MotorEntry
71#[derive(Debug, Clone, Copy, Default)]
72pub struct SetTargetContext {
73    pub current_mode: Option<MotorMode>,
74    pub peak_torque_nm: Option<f32>,
75    pub mit_kp_kd_factor: Option<f32>,
76}
77
78/// 两次 SDO 写之间建议的 settle 间隔。
79///
80/// CiA402 控制字 ramp 的状态机切换不是瞬时的；10 ms 是经验值，保证最坏
81/// 情况也能稳定地观察到上一条 SDO 的效果再下一条。
82pub const INTER_WRITE_DELAY: Duration = Duration::from_millis(10);
83
84/// `set_mode`：把电机切到指定模式。
85///
86/// 序列（统一保险路径）：
87/// 1. 若 `current_logic` 是 `Logic::Error`，前置 `CW=0x80` (fault reset)
88/// 2. `CW=0x06` (Shutdown → SwitchOnDisabled / ReadyToSwitchOn)
89/// 3. `0x6060 = mode_code`（写期望模式）
90/// 4. `CW=0x06`（再来一次 Shutdown，确保 ReadyToSwitchOn）
91/// 5. `CW=0x07` (SwitchOn → SwitchedOn)
92/// 6. `CW=0x0F` (EnableOperation → OperationEnabled)
93pub fn build_set_mode_writes(target: MotorMode, current_logic: Option<&Logic>) -> Vec<SdoWrite> {
94    let mut out = Vec::with_capacity(7);
95    if matches!(current_logic, Some(Logic::Error { .. })) {
96        out.push(SdoWrite::u16(OD_CONTROL_WORD, 0, cw::FAULT_RESET));
97    }
98    out.push(SdoWrite::u16(OD_CONTROL_WORD, 0, cw::SHUTDOWN));
99    out.push(SdoWrite::i8(
100        OD_MODE_OF_OPERATION,
101        0,
102        mode_to_cia402_code(target),
103    ));
104    out.push(SdoWrite::u16(OD_CONTROL_WORD, 0, cw::SHUTDOWN));
105    out.push(SdoWrite::u16(OD_CONTROL_WORD, 0, cw::SWITCH_ON));
106    out.push(SdoWrite::u16(OD_CONTROL_WORD, 0, cw::ENABLE_OPERATION));
107    out
108}
109
110/// `disable`：写控制字 `0x06`（短刹车 / 移出 OperationEnabled）。
111pub fn build_disable_writes() -> Vec<SdoWrite> {
112    vec![SdoWrite::u16(OD_CONTROL_WORD, 0, cw::SHUTDOWN)]
113}
114
115/// `clear_error`：写控制字 `0x80` (fault reset)。
116pub fn build_clear_error_writes() -> Vec<SdoWrite> {
117    vec![SdoWrite::u16(OD_CONTROL_WORD, 0, cw::FAULT_RESET)]
118}
119
120/// `set_target`：构造往电机写目标值需要的 SDO 序列。
121///
122/// `ctx.current_mode` 是 Manager 缓存的"上一次 `set_mode` 设的模式"。
123/// `target` 的 enum variant 必须和它匹配，否则返回
124/// [`Error::TargetModeMismatch`]。
125///
126/// **v0.1 全模式覆盖**（HexMeow CiA402 电机约定）：
127///
128/// | target / 当前模式 | SDO 序列 |
129/// |---|---|
130/// | `Disable` (任意模式) | `0x6040 = 0x06` |
131/// | `Velocity{rev_per_s}` + `ProfileVelocity` | `0x60FF = f32(rev_per_s)` |
132/// | `Position{rev}` + `ProfilePosition` | `0x6040 = 0x2F`（清掉 bit4） → `0x607A = f32(rev)` → `0x6040 = 0x3F`（bit4 上升沿，Change Set Immediately） |
133/// | `Torque{nm}` + `Torque` | `0x6071 = i16(round(nm / peak * 1000))`，需要 `ctx.peak_torque_nm` |
134/// | `Mit{pos,vel,tor,kp,kd}` + `Mit` | `0x2003:01 = f32(pos)` → `:02 = f32(vel)` → `:03 = f32(tor)` → `:04 = u16(round(kp / factor))` → `:05 = u16(round(kd / factor))`，需要 `ctx.mit_kp_kd_factor` |
135///
136/// 缺少必需的 `peak_torque_nm` / `mit_kp_kd_factor` 时返回 [`Error::Internal`]
137/// （通常意味着 `initialize()` 时电机没暴露 `0x6076` / `0x2003:07`）。
138pub fn build_set_target_writes(
139    target: &MotorTarget,
140    ctx: SetTargetContext,
141) -> Result<Vec<SdoWrite>> {
142    // Disable 全模式通用。
143    if matches!(target, MotorTarget::Disable) {
144        return Ok(build_disable_writes());
145    }
146
147    let Some(mode) = ctx.current_mode else {
148        return Err(Error::Internal(
149            "set_target: motor mode unknown (call set_mode first)".into(),
150        ));
151    };
152
153    if !target.matches_mode(mode) {
154        return Err(Error::TargetModeMismatch {
155            expected: format!("{:?}", mode),
156            given: target.variant_name(),
157        });
158    }
159
160    match (target, mode) {
161        (MotorTarget::Velocity { rev_per_s }, MotorMode::ProfileVelocity) => {
162            Ok(vec![SdoWrite::f32(OD_TARGET_VELOCITY, 0, *rev_per_s)])
163        }
164        (MotorTarget::Position { rev }, MotorMode::ProfilePosition) => {
165            Ok(build_pp_position_writes(*rev))
166        }
167        (MotorTarget::Torque { nm }, MotorMode::Torque) => build_torque_writes(*nm, &ctx),
168        (MotorTarget::Mit { pos, vel, tor, kp, kd }, MotorMode::Mit) => {
169            build_mit_writes(*pos, *vel, *tor, *kp, *kd, &ctx)
170        }
171        // 已被上面的 matches_mode 拒绝过；这里是 exhaustiveness 兜底。
172        _ => Err(Error::TargetModeMismatch {
173            expected: format!("{:?}", mode),
174            given: target.variant_name(),
175        }),
176    }
177}
178
179/// `set_target(Position)` 的 PP / Change-Set-Immediately 序列。详见
180/// CiA402 §6.4.2.1 (Profile Position Mode)。
181///
182/// 三条写：
183/// 1. CW = `0x002F` —— 把 bit4 (`new_setpoint`) 落回 0，保持 enable + CSI
184///    bit5 = 1。第一次调用时上一条 CW 来自 `set_mode` 末尾的 `0x000F`，
185///    所以 bit4 本来就是 0；但写一次保证之后多次调用的 bit4 0→1 上升沿都
186///    存在。
187/// 2. `0x607A` = f32 Rev —— 新目标位置（vendor-specific f32；标准 CiA402 是
188///    i32 user-units）。
189/// 3. CW = `0x003F` —— bit4 0→1 上升沿告诉电机"latch 新目标"；CSI bit5 = 1
190///    要求立刻替换当前目标，不等当前 motion profile 跑完。
191fn build_pp_position_writes(rev: f32) -> Vec<SdoWrite> {
192    vec![
193        SdoWrite::u16(OD_CONTROL_WORD, 0, cw::ENABLE_PP_NEW_SP_CLEARED),
194        SdoWrite::f32(OD_TARGET_POSITION, 0, rev),
195        SdoWrite::u16(OD_CONTROL_WORD, 0, cw::ENABLE_PP_NEW_SP_LATCHED),
196    ]
197}
198
199/// `set_target(Torque)`：Nm → i16 (‰ of peak_torque)。
200fn build_torque_writes(nm: f32, ctx: &SetTargetContext) -> Result<Vec<SdoWrite>> {
201    let peak = ctx.peak_torque_nm.ok_or_else(|| {
202        Error::Internal(
203            "set_target(Torque): peak_torque not cached. \
204             initialize() must read 0x6076 first; this motor may not expose it."
205                .into(),
206        )
207    })?;
208    if !peak.is_finite() || peak.abs() < f32::EPSILON {
209        return Err(Error::Internal(format!(
210            "set_target(Torque): cached peak_torque is {peak} Nm; cannot convert"
211        )));
212    }
213    let permille = (nm / peak * 1000.0).round();
214    let clamped = permille.clamp(-1000.0, 1000.0) as i16;
215    Ok(vec![SdoWrite::i16(OD_TARGET_TORQUE, 0, clamped)])
216}
217
218/// `set_target(Mit)`：写 0x2003:01..=05 五条。
219///
220/// - kp/kd 物理单位 [Nm/Rev] / [Nm·s/Rev] → u16 dimensionless：
221///   `kp_int = round(kp_phys / factor)`，clamp 到 `0..=10000`。
222fn build_mit_writes(
223    pos: f32,
224    vel: f32,
225    tor: f32,
226    kp: f32,
227    kd: f32,
228    ctx: &SetTargetContext,
229) -> Result<Vec<SdoWrite>> {
230    let factor = ctx.mit_kp_kd_factor.ok_or_else(|| {
231        Error::Internal(
232            "set_target(Mit): mit_kp_kd_factor not cached. \
233             initialize() must read 0x2003:07 first; this motor may not expose it."
234                .into(),
235        )
236    })?;
237    if !factor.is_finite() || factor.abs() < f32::EPSILON {
238        return Err(Error::Internal(format!(
239            "set_target(Mit): cached mit_kp_kd_factor is {factor}; cannot convert"
240        )));
241    }
242    let kp_int = (kp / factor).round().clamp(0.0, u16::MAX as f32) as u16;
243    let kd_int = (kd / factor).round().clamp(0.0, u16::MAX as f32) as u16;
244    // OD-08 文档说 KP / KD 范围 0..=10000；再加一层 clamp 保险。
245    let kp_int = kp_int.min(10_000);
246    let kd_int = kd_int.min(10_000);
247    Ok(vec![
248        SdoWrite::f32(OD_MIT_CONTROL_PARAM, mit::SUB_POSITION, pos),
249        SdoWrite::f32(OD_MIT_CONTROL_PARAM, mit::SUB_VELOCITY, vel),
250        SdoWrite::f32(OD_MIT_CONTROL_PARAM, mit::SUB_TORQUE, tor),
251        SdoWrite::u16(OD_MIT_CONTROL_PARAM, mit::SUB_KP, kp_int),
252        SdoWrite::u16(OD_MIT_CONTROL_PARAM, mit::SUB_KD, kd_int),
253    ])
254}
255
256#[cfg(test)]
257mod tests {
258    use super::*;
259
260    fn cw_writes_only(writes: &[SdoWrite]) -> Vec<u16> {
261        writes
262            .iter()
263            .filter(|w| w.index == OD_CONTROL_WORD)
264            .map(|w| u16::from_le_bytes([w.data[0], w.data[1]]))
265            .collect()
266    }
267
268    #[test]
269    fn set_mode_default_ramp_no_fault_reset() {
270        let w = build_set_mode_writes(MotorMode::ProfileVelocity, None);
271        // CW=0x06, 0x6060=3, CW=0x06, CW=0x07, CW=0x0F
272        assert_eq!(w.len(), 5);
273        assert_eq!(
274            cw_writes_only(&w),
275            vec![cw::SHUTDOWN, cw::SHUTDOWN, cw::SWITCH_ON, cw::ENABLE_OPERATION]
276        );
277        let mode_w = &w[1];
278        assert_eq!(mode_w.index, OD_MODE_OF_OPERATION);
279        assert_eq!(mode_w.data, vec![3u8]);
280    }
281
282    #[test]
283    fn set_mode_prepends_fault_reset_when_in_error() {
284        let logic = Logic::Error {
285            kind: crate::types::MotorErrorKind::OverCurrent,
286            raw_code: 0x2310,
287        };
288        let w = build_set_mode_writes(MotorMode::Mit, Some(&logic));
289        assert_eq!(w.len(), 6);
290        // 第一条必须是 fault reset
291        assert_eq!(
292            u16::from_le_bytes([w[0].data[0], w[0].data[1]]),
293            cw::FAULT_RESET
294        );
295        // 第三条应该是 0x6060 = 5 (Mit)
296        assert_eq!(w[2].index, OD_MODE_OF_OPERATION);
297        assert_eq!(w[2].data, vec![5u8]);
298    }
299
300    #[test]
301    fn set_mode_for_each_mode_writes_correct_code() {
302        for (m, code) in [
303            (MotorMode::ProfilePosition, 1u8),
304            (MotorMode::ProfileVelocity, 3),
305            (MotorMode::Torque, 4),
306            (MotorMode::Mit, 5),
307        ] {
308            let w = build_set_mode_writes(m, None);
309            let mode_w = w.iter().find(|w| w.index == OD_MODE_OF_OPERATION).unwrap();
310            assert_eq!(mode_w.data[0], code, "mode {m:?}");
311        }
312    }
313
314    #[test]
315    fn disable_is_single_shutdown() {
316        let w = build_disable_writes();
317        assert_eq!(w.len(), 1);
318        assert_eq!(w[0].index, OD_CONTROL_WORD);
319        assert_eq!(u16::from_le_bytes([w[0].data[0], w[0].data[1]]), cw::SHUTDOWN);
320    }
321
322    #[test]
323    fn clear_error_is_single_fault_reset() {
324        let w = build_clear_error_writes();
325        assert_eq!(w.len(), 1);
326        assert_eq!(
327            u16::from_le_bytes([w[0].data[0], w[0].data[1]]),
328            cw::FAULT_RESET
329        );
330    }
331
332    fn ctx_pv() -> SetTargetContext {
333        SetTargetContext {
334            current_mode: Some(MotorMode::ProfileVelocity),
335            ..Default::default()
336        }
337    }
338
339    #[test]
340    fn target_disable_works_in_any_mode() {
341        assert!(build_set_target_writes(&MotorTarget::Disable, SetTargetContext::default()).is_ok());
342        assert!(build_set_target_writes(&MotorTarget::Disable, ctx_pv()).is_ok());
343    }
344
345    #[test]
346    fn target_velocity_in_pv_mode_writes_60ff_f32() {
347        let w = build_set_target_writes(&MotorTarget::Velocity { rev_per_s: 1.5 }, ctx_pv()).unwrap();
348        assert_eq!(w.len(), 1);
349        assert_eq!(w[0].index, OD_TARGET_VELOCITY);
350        assert_eq!(w[0].subindex, 0);
351        assert_eq!(w[0].data.len(), 4);
352        let v = f32::from_le_bytes([w[0].data[0], w[0].data[1], w[0].data[2], w[0].data[3]]);
353        assert!((v - 1.5).abs() < f32::EPSILON);
354    }
355
356    #[test]
357    fn target_velocity_without_known_mode_errs() {
358        let r = build_set_target_writes(
359            &MotorTarget::Velocity { rev_per_s: 1.0 },
360            SetTargetContext::default(),
361        );
362        assert!(matches!(r, Err(Error::Internal(_))));
363    }
364
365    #[test]
366    fn target_velocity_in_wrong_mode_errs() {
367        let r = build_set_target_writes(
368            &MotorTarget::Velocity { rev_per_s: 1.0 },
369            SetTargetContext {
370                current_mode: Some(MotorMode::Torque),
371                ..Default::default()
372            },
373        );
374        assert!(matches!(r, Err(Error::TargetModeMismatch { .. })));
375    }
376
377    // ===== Profile Position =====
378
379    #[test]
380    fn target_position_in_pp_mode_emits_csi_handshake() {
381        let w = build_set_target_writes(
382            &MotorTarget::Position { rev: 0.25 },
383            SetTargetContext {
384                current_mode: Some(MotorMode::ProfilePosition),
385                ..Default::default()
386            },
387        )
388        .unwrap();
389        // 三条：CW=0x2F, 0x607A=f32(0.25), CW=0x3F
390        assert_eq!(w.len(), 3);
391        assert_eq!(w[0].index, OD_CONTROL_WORD);
392        assert_eq!(u16::from_le_bytes([w[0].data[0], w[0].data[1]]), 0x002F);
393        assert_eq!(w[1].index, OD_TARGET_POSITION);
394        let pos = f32::from_le_bytes([w[1].data[0], w[1].data[1], w[1].data[2], w[1].data[3]]);
395        assert!((pos - 0.25).abs() < f32::EPSILON);
396        assert_eq!(w[2].index, OD_CONTROL_WORD);
397        assert_eq!(u16::from_le_bytes([w[2].data[0], w[2].data[1]]), 0x003F);
398    }
399
400    #[test]
401    fn target_position_in_wrong_mode_errs() {
402        let r = build_set_target_writes(
403            &MotorTarget::Position { rev: 0.5 },
404            SetTargetContext {
405                current_mode: Some(MotorMode::ProfileVelocity),
406                ..Default::default()
407            },
408        );
409        assert!(matches!(r, Err(Error::TargetModeMismatch { .. })));
410    }
411
412    // ===== Torque =====
413
414    #[test]
415    fn target_torque_converts_nm_to_permille_of_peak() {
416        // peak = 4 Nm，target = 1 Nm → 250 ‰
417        let w = build_set_target_writes(
418            &MotorTarget::Torque { nm: 1.0 },
419            SetTargetContext {
420                current_mode: Some(MotorMode::Torque),
421                peak_torque_nm: Some(4.0),
422                ..Default::default()
423            },
424        )
425        .unwrap();
426        assert_eq!(w.len(), 1);
427        assert_eq!(w[0].index, OD_TARGET_TORQUE);
428        let v = i16::from_le_bytes([w[0].data[0], w[0].data[1]]);
429        assert_eq!(v, 250);
430    }
431
432    #[test]
433    fn target_torque_clamps_to_plus_minus_1000_permille() {
434        let w = build_set_target_writes(
435            &MotorTarget::Torque { nm: 99.0 },
436            SetTargetContext {
437                current_mode: Some(MotorMode::Torque),
438                peak_torque_nm: Some(4.0),
439                ..Default::default()
440            },
441        )
442        .unwrap();
443        let v = i16::from_le_bytes([w[0].data[0], w[0].data[1]]);
444        assert_eq!(v, 1000);
445
446        let w = build_set_target_writes(
447            &MotorTarget::Torque { nm: -99.0 },
448            SetTargetContext {
449                current_mode: Some(MotorMode::Torque),
450                peak_torque_nm: Some(4.0),
451                ..Default::default()
452            },
453        )
454        .unwrap();
455        let v = i16::from_le_bytes([w[0].data[0], w[0].data[1]]);
456        assert_eq!(v, -1000);
457    }
458
459    #[test]
460    fn target_torque_without_peak_cached_errs() {
461        let r = build_set_target_writes(
462            &MotorTarget::Torque { nm: 1.0 },
463            SetTargetContext {
464                current_mode: Some(MotorMode::Torque),
465                peak_torque_nm: None,
466                ..Default::default()
467            },
468        );
469        assert!(matches!(r, Err(Error::Internal(_))));
470    }
471
472    // ===== MIT =====
473
474    #[test]
475    fn target_mit_emits_five_writes_with_kp_kd_converted() {
476        // factor = 0.01 means kp_int = round(kp_phys / 0.01) = round(kp_phys * 100)
477        let w = build_set_target_writes(
478            &MotorTarget::Mit {
479                pos: 0.1,
480                vel: 0.2,
481                tor: 0.3,
482                kp: 5.0,   // → 500
483                kd: 0.5,   // → 50
484            },
485            SetTargetContext {
486                current_mode: Some(MotorMode::Mit),
487                mit_kp_kd_factor: Some(0.01),
488                ..Default::default()
489            },
490        )
491        .unwrap();
492        assert_eq!(w.len(), 5);
493        assert_eq!(w[0].index, 0x2003);
494        assert_eq!(w[0].subindex, 0x01);
495        let pos = f32::from_le_bytes([w[0].data[0], w[0].data[1], w[0].data[2], w[0].data[3]]);
496        assert!((pos - 0.1).abs() < 1e-6);
497        assert_eq!(w[3].subindex, 0x04);
498        let kp = u16::from_le_bytes([w[3].data[0], w[3].data[1]]);
499        assert_eq!(kp, 500);
500        assert_eq!(w[4].subindex, 0x05);
501        let kd = u16::from_le_bytes([w[4].data[0], w[4].data[1]]);
502        assert_eq!(kd, 50);
503    }
504
505    #[test]
506    fn target_mit_clamps_kp_to_10000() {
507        let w = build_set_target_writes(
508            &MotorTarget::Mit {
509                pos: 0.0,
510                vel: 0.0,
511                tor: 0.0,
512                kp: 1e6,
513                kd: 0.0,
514            },
515            SetTargetContext {
516                current_mode: Some(MotorMode::Mit),
517                mit_kp_kd_factor: Some(0.01),
518                ..Default::default()
519            },
520        )
521        .unwrap();
522        let kp = u16::from_le_bytes([w[3].data[0], w[3].data[1]]);
523        assert_eq!(kp, 10_000);
524    }
525
526    #[test]
527    fn target_mit_without_factor_cached_errs() {
528        let r = build_set_target_writes(
529            &MotorTarget::Mit {
530                pos: 0.0, vel: 0.0, tor: 0.0, kp: 1.0, kd: 0.0,
531            },
532            SetTargetContext {
533                current_mode: Some(MotorMode::Mit),
534                mit_kp_kd_factor: None,
535                ..Default::default()
536            },
537        );
538        assert!(matches!(r, Err(Error::Internal(_))));
539    }
540}
hex_motor/cia402/sequences.rs

hex_motor/cia402/
sequences.rs
