From e1a00a9f066af45feb0c1982b46786b3154d6045 Mon Sep 17 00:00:00 2001 From: thetooler Date: Mon, 28 Sep 2026 17:14:24 +0800 Subject: [PATCH] feat: cover the enable window and expose stale state Three defects that all come from the same place: a DM motor in MIT mode keeps executing the last target frame it was given, and a disabled motor sends nothing at all. The enable window. The 0xFC enable carries no target of its own and does not clear the target registers, so the instant it takes effect the motor resumes whatever target a previous session left behind -- if you Ctrl+C'd mid-close(), that register still says q=closed, kp=100. Enable now streams zero-gain frames over that window, waits for a fresh status frame, and then leaves the motor holding the position it actually measured. initialize(), enable() and clear_fault() all go through that one path, and the bare enable() in initialize()'s retry loop is gone, since an uncovered enable is exactly the window being closed. The silent wait. initialize() used to wait quietly for feedback while the motor was already enabled. An enabled motor that hears nothing for ~900 ms latches the 0xD comm-loss fault, so that wait manufactured the failure it was looking for. It now keeps feeding the motor while it waits. Stale state. A disabled motor does not stream status frames, so get_state() silently returned the last decoded values -- or constructor defaults, i.e. position 0.0 with temperatures 0/0 -- while the jaw might be anywhere. MotorState and GripperState now carry data_age_s, and get_state() warns when the snapshot is older than STALE_AFTER_S. A new refresh_status() sends the 0xCC refresh command, which the motor answers regardless of enable state, so the position can be read before the first enable instead of guessed. Also adds the three error codes the SDK was rendering as "unknown error": 0x8 (over-voltage), 0xE (overload) and 0xD (comm loss) -- the last one being the fault a disabled motor sits in, which made a normal state look like a hardware failure. The old `err in (0, 1)` reading of `initialize()` is deliberately NOT adopted: this repository requires `err == 1` and tests/test_enable.py pins that. Only the freshness half of the change is taken here. Verified: 52 unittest cases, 0 failures -- the 40 that shipped plus 12 in tests/test_stale_state.py covering the freshness contract, the new error codes, and refresh_status on both paths. --- src/litegrip/can/motor.py | 23 ++++ src/litegrip/constants.py | 6 + src/litegrip/gripper.py | 35 +++++ src/litegrip/models.py | 21 +++ src/litegrip/protocols/can_bus.py | 211 +++++++++++++++++++++++------- tests/fake_can.py | 4 + tests/test_enable.py | 14 +- tests/test_stale_state.py | 132 +++++++++++++++++++ 8 files changed, 395 insertions(+), 51 deletions(-) create mode 100644 tests/test_stale_state.py diff --git a/src/litegrip/can/motor.py b/src/litegrip/can/motor.py index 5089565..fe6d022 100644 --- a/src/litegrip/can/motor.py +++ b/src/litegrip/can/motor.py @@ -7,6 +7,7 @@ from __future__ import annotations +import time from dataclasses import dataclass, field from enum import IntEnum @@ -133,6 +134,28 @@ def is_fault(self) -> bool: def last_update(self) -> float: return self._last_update + @property + def has_data(self) -> bool: + """True once at least one status frame has been decoded. + + A disabled DM motor does not stream status frames on its own, so + before the first enable (or after a disable, once the RX buffer + drains) there is nothing to read and every value here is still the + constructor default — position 0.0 with temperatures 0/0. + """ + return self.rx_count > 0 + + @property + def data_age_s(self) -> float: + """Seconds since the last decoded status frame. + + Returns ``inf`` when no frame has ever arrived, so comparisons like + ``data_age_s > threshold`` work without a separate has-data check. + """ + if self.rx_count == 0: + return float("inf") + return max(0.0, time.monotonic() - self._last_update) + # ── control mode management ───────────────────────────────────────── def set_mode(self, mode: ControlMode) -> None: diff --git a/src/litegrip/constants.py b/src/litegrip/constants.py index 641c91d..4e54796 100644 --- a/src/litegrip/constants.py +++ b/src/litegrip/constants.py @@ -72,19 +72,25 @@ class ErrorCode: """Damiao motor error codes (extracted from status frame data[0] >> 4).""" DISABLED: Final = 0 ENABLED: Final = 1 + OV_FAULT: Final = 0x8 UV_FAULT: Final = 0x9 OC_FAULT: Final = 0xA MOS_OT: Final = 0xB COIL_OT: Final = 0xC + COMM_LOSS: Final = 0xD + OVERLOAD: Final = 0xE ERROR_DESCRIPTIONS = { 0x0: "已失能", 0x1: "已使能", + 0x8: "过压故障 (OV)", 0x9: "欠压故障 (UV)", 0xA: "过流故障 (OC)", 0xB: "MOS 过温故障", 0xC: "线圈过温故障", + 0xD: "通讯丢失 (CAN 超时)", + 0xE: "过载故障", } diff --git a/src/litegrip/gripper.py b/src/litegrip/gripper.py index a52e516..b158bc1 100644 --- a/src/litegrip/gripper.py +++ b/src/litegrip/gripper.py @@ -54,6 +54,7 @@ _os.path.join(_os.path.expanduser("~"), ".litegrip", "litegrip_calibration.json"), ) from .models import ( + STALE_AFTER_S, GripperState, GripperConfig, GripperInfo, @@ -1294,6 +1295,12 @@ def _find_limit(direction: str) -> float: def get_state(self, wait: bool = True) -> GripperState: """Return the current gripper state. + A disabled motor does not stream status frames on its own, so when no + fresh frame arrives the returned snapshot is the last decoded one — or + constructor defaults (position 0.0, temperatures 0/0) if none ever + arrived. Check :attr:`GripperState.is_stale` / ``data_age_s`` before + trusting the readings, or send a refresh frame first. + Args: wait: If True (default), waits up to 50 ms for a fresh status frame. If False, returns immediately with the last cached @@ -1311,6 +1318,16 @@ def get_state(self, wait: bool = True) -> GripperState: else: self._can.poll(timeout_s=0.0) + motor = self._can.motor + data_age_s = motor.data_age_s if motor is not None else float("inf") + if wait and data_age_s > STALE_AFTER_S: + log.warning( + "get_state(): 未收到新状态帧（%s）——返回的是缓存/默认值，" + "不是当前测量。失能状态的电机不主动发状态帧。", + "从未收到" if data_age_s == float("inf") + else f"最近一帧 {data_age_s:.2f}s 前", + ) + position_rad = self._can.get_position() velocity_rad_s = self._can.get_velocity() torque_nm = self._can.get_torque() @@ -1330,10 +1347,28 @@ def get_state(self, wait: bool = True) -> GripperState: temperature_coil=t_coil, error_code=error_code, timestamp=time.time(), + data_age_s=data_age_s, position_mm=position_mm, force_n=force_n, ) + def refresh_status(self, timeout_s: float = 0.5) -> bool: + """Request a status frame from the motor, even while disabled. + + A disabled motor does not stream status frames, so :meth:`get_state` + keeps returning cached values (or zeros, before the first enable). + This sends the 0xCC refresh command — which the motor answers + regardless of enable state — and waits for the reply. Useful for + reading the position before enabling. No motion, no output change. + + Returns: + True if a fresh status frame arrived. + """ + self._check_connected() + if self._can is None: + return False + return self._can.refresh_status(timeout_s=timeout_s) + def get_position(self) -> float: """Current position in mm.""" return self.get_state().position_mm diff --git a/src/litegrip/models.py b/src/litegrip/models.py index d151397..0eee19e 100644 --- a/src/litegrip/models.py +++ b/src/litegrip/models.py @@ -7,6 +7,11 @@ import time from typing import Optional +# A status frame older than this is treated as no longer representing the +# present. DM motors emit status at ~10 Hz while enabled, so 0.5 s is ~5 +# missed frames; the motor's own CAN-timeout fault trips at ~0.9 s. +STALE_AFTER_S = 0.5 + class GripperMode(IntEnum): """Gripper control mode.""" @@ -41,10 +46,26 @@ class GripperState: error_code: int = 0 timestamp: float = field(default_factory=time.time) + # Age of the status frame these values came from; inf = never received. + # A disabled motor does not stream status frames, so a state read before + # the first enable (or after a disable) holds constructor defaults, not + # measurements — check this before trusting position/force/temperature. + data_age_s: float = float("inf") + # Convenience — computed from raw values with unit conversion position_mm: float = 0.0 force_n: float = 0.0 + @property + def has_data(self) -> bool: + """True if at least one status frame has been decoded.""" + return self.data_age_s != float("inf") + + @property + def is_stale(self) -> bool: + """True if the snapshot is not backed by a recent status frame.""" + return self.data_age_s > STALE_AFTER_S + @property def is_enabled(self) -> bool: """True if the motor is enabled (error_code == 1).""" diff --git a/src/litegrip/protocols/can_bus.py b/src/litegrip/protocols/can_bus.py index 5c36a60..6f77bb2 100644 --- a/src/litegrip/protocols/can_bus.py +++ b/src/litegrip/protocols/can_bus.py @@ -138,13 +138,110 @@ def register_gripper( except Exception as e: raise CommError(f"注册夹爪电机失败: {e}") + # ═══════════════════════════════════════════════════════════════════ + # Enable window + # ═══════════════════════════════════════════════════════════════════ + # + # In MIT mode the motor continuously executes the last target frame it + # received: τ = kp·(q_target − q) + kd·(dq_target − dq) + tau_ff, with + # every one of those five values coming from that frame. The 0xFC enable + # command carries no target of its own and does not clear the target + # registers — it only re-engages the control loop. So the instant enable + # takes effect, a motor left over from a previous session would resume + # driving toward *that* session's target (e.g. you Ctrl+C'd mid-close() + # and the register still holds q=closed, kp=100). + # + # These two helpers close that window: stream zero-gain frames over it, + # then leave the motor holding wherever it actually is. + + def _hold_at_current(self, kp: float, kd: float, + duration_s: float = 0.05) -> bool: + """Stream hold-position frames at the motor's current position. + + Call this only after a fresh status frame has been decoded — the + MotorState default position is 0.0, and holding at 0.0 with a real + gain would drive the gripper to a bogus target. + + Leaving "hold where you are" as the last target also makes the *next* + enable inherently safe, since that is what the motor resumes from. + + Returns: + True if the hold frames were streamed. + """ + if self._controller is None or self._motor is None: + return False + if self._motor.rx_count == 0: + log.warning("Refusing to hold position: no status frame decoded yet") + return False + return self.control_mit_stream( + q_target=self._motor.position, kp=kp, kd=kd, + duration_s=duration_s) + + def _enable_and_hold(self, kp: float, kd: float, + timeout_s: float = DefaultParams.INIT_TIMEOUT_S + ) -> Optional[int]: + """Enable the motor and leave it holding its current position. + + Sequence: 0xFC enable → streamed zero-gain frames → wait for a fresh + status frame → hold at the freshly-read position with *kp*/*kd*. + + Returns: + The error code from the first fresh status frame, or None if no + feedback arrived within *timeout_s*. + """ + if self._controller is None or self._motor is None: + return None + + ctrl = self._controller + m = self._motor + prev_rx = m.rx_count + + ctrl.enable(m) + + # Zero-gain cover. Streamed rather than a single frame: everything + # else in this SDK streams for the same reason, and here one lost + # frame means the motor keeps running the stale target indefinitely + # rather than for ~10 ms. + self.control_mit_stream(q_target=0.0, kp=0.0, kd=0.0, + duration_s=0.05, interval_s=0.005) + + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline: + # Keep feeding the motor while we wait. It is ENABLED from the + # 0xFC above, and an enabled motor that hears nothing for ~900 ms + # latches the 0xD comm-loss fault (measured on hardware) — which + # is exactly the fault this wait is supposed to detect, so silence + # here manufactures the failure it is looking for. The 0.05 s + # burst before the loop is not long enough to cover a 2 s wait. + ctrl.control_mit(m, kp=0, kd=0, q=0, dq=0, tau=0) + ctrl.poll(timeout_s=0.01) + if m.rx_count > prev_rx: + break + time.sleep(0.005) + else: + return None + + # rx_count only advances on a decoded status frame, so m.position is a + # real reading from here on. + self._hold_at_current(kp, kd) + return m.error + # ═══════════════════════════════════════════════════════════════════ # Initialization / enable / disable # ═══════════════════════════════════════════════════════════════════ - def initialize(self) -> bool: + def initialize(self, kp: Optional[float] = None, + kd: Optional[float] = None) -> bool: """Full initialization: disable → switch to MIT → enable → verify feedback. + Once enabled, the motor is left holding its current position (see + :meth:`_enable_and_hold`) rather than outputting zero torque, so it + cannot jump toward whatever target the previous session left behind. + + Args: + kp: Stiffness used for the post-enable hold (default DEFAULT_KP). + kd: Damping used for the post-enable hold (default DEFAULT_KD). + Returns: True if the motor reports ``err == 1`` (enabled). ``err == 0`` means the enable command did not take effect — the attempt is @@ -159,6 +256,8 @@ def initialize(self) -> bool: ctrl = self._controller m = self._motor + kp = kp if kp is not None else GripperParams.DEFAULT_KP + kd = kd if kd is not None else GripperParams.DEFAULT_KD last_err = -1 for attempt in range(GripperParams.FAULT_CLEAR_RETRIES): @@ -172,37 +271,28 @@ def initialize(self) -> bool: log.warning("switchControlMode verification failed; proceeding anyway") time.sleep(0.05) - # 3. Enable - ctrl.enable(m) - - # 4. Send zero-torque MIT frame and poll for feedback - prev_rx = m.rx_count - ctrl.control_mit(m, kp=0, kd=0, q=0, dq=0, tau=0) - - deadline = time.monotonic() + DefaultParams.INIT_TIMEOUT_S - while time.monotonic() < deadline: - ctrl.poll(timeout_s=0.01) - if m.rx_count > prev_rx: - err = m.error - if err == 1: - self._initialized = True - return True - # err == 0 → the enable frame was lost; retry. - # anything else → a real fault; retry after clearing. - last_err = err - break - time.sleep(0.005) - else: + # 3+4. Enable, cover the window, verify feedback, then hold + # at the freshly-read position. + err = self._enable_and_hold(kp, kd) + + if err is None: last_err = -2 # timeout, no feedback + elif err == 1: + self._initialized = True + return True + else: + # err == 0 → the enable frame was lost; retry. + # anything else → a real fault; retry after clearing. + last_err = err except Exception: last_err = -3 - # Retry: clear fault before next attempt + # Retry: clear the fault before the next attempt. No bare + # enable() here — the next iteration disables immediately, and an + # uncovered enable is exactly the window this method avoids. if attempt < GripperParams.FAULT_CLEAR_RETRIES - 1: ctrl.clear_fault(m) - time.sleep(0.005) - ctrl.enable(m) time.sleep(0.01) # All retries exhausted → raise @@ -222,15 +312,21 @@ def initialize(self) -> bool: else: raise HardwareError("初始化失败：所有重试耗尽") - def enable(self) -> bool: - """Send enable command only (no full init). Prefer initialize().""" + def enable(self, kp: Optional[float] = None, + kd: Optional[float] = None) -> bool: + """Send enable command only (no full init). Prefer initialize(). + + The motor is left holding its current position once enabled — see + :meth:`_enable_and_hold`. + """ if self._controller is None or self._motor is None: return False + kp = kp if kp is not None else GripperParams.DEFAULT_KP + kd = kd if kd is not None else GripperParams.DEFAULT_KD try: - self._controller.enable(self._motor) - self._controller.control_mit(self._motor, kp=0, kd=0, q=0) - self._controller.poll(timeout_s=0.05) - return self._motor.error in (0, 1) + # None (no feedback) fails the membership test, so a silent motor + # is no longer reported as a successful enable. + return self._enable_and_hold(kp, kd) in (0, 1) except Exception: return False @@ -253,33 +349,32 @@ def _clear_fault(self) -> bool: """[Deprecated] Use :meth:`clear_fault` instead.""" return self.clear_fault() - def clear_fault(self) -> bool: + def clear_fault(self, kp: Optional[float] = None, + kd: Optional[float] = None) -> bool: """Clear latched fault and re-enable the motor. Tries two strategies: 1. Direct clear (0xFB) + enable (0xFC) — works for UV/OC/OT faults where the motor is already effectively disabled. 2. Full disable → clear → enable cycle. + + Both re-enable through :meth:`_enable_and_hold`, so the motor ends up + holding its current position instead of resuming a stale target, and a + motor that never answers counts as a failure rather than a success. """ if self._controller is None or self._motor is None: return False ctrl = self._controller m = self._motor + kp = kp if kp is not None else GripperParams.DEFAULT_KP + kd = kd if kd is not None else GripperParams.DEFAULT_KD for _ in range(GripperParams.FAULT_CLEAR_RETRIES): # Strategy 1: Direct clear + enable (proven for UV_FAULT) ctrl.clear_fault(m) time.sleep(0.005) - ctrl.enable(m) - time.sleep(0.01) - - # Verify - ctrl.control_mit(m, kp=0, kd=0, q=0) - for _ in range(20): - ctrl.poll(timeout_s=0.01) - time.sleep(0.003) - if m.error in (0, 1): + if self._enable_and_hold(kp, kd) in (0, 1): return True # Strategy 2: Full disable → clear → enable @@ -287,14 +382,7 @@ def clear_fault(self) -> bool: time.sleep(0.01) ctrl.clear_fault(m) time.sleep(0.01) - ctrl.enable(m) - time.sleep(0.01) - - ctrl.control_mit(m, kp=0, kd=0, q=0) - for _ in range(20): - ctrl.poll(timeout_s=0.01) - time.sleep(0.003) - if m.error in (0, 1): + if self._enable_and_hold(kp, kd) in (0, 1): return True return False @@ -371,6 +459,31 @@ def update_state(self, timeout_s: float = 0.05) -> bool: return False return self._controller.poll_until(self._motor, timeout_s=timeout_s) + def refresh_status(self, timeout_s: float = 0.5) -> bool: + """Request a status frame and wait for it (0xCC refresh command). + + A disabled motor does not stream status frames on its own, so + :meth:`update_state` finds nothing and callers read stale/default + values. The refresh command is answered regardless of enable state, + which makes the position readable before the first enable. Sends no + motion command and changes no motor output. + + Returns: + True if a fresh status frame arrived within *timeout_s*. + """ + if self._controller is None or self._motor is None: + return False + m = self._motor + prev_rx = m.rx_count + self._controller.refresh_status(m) + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline: + self._controller.poll(timeout_s=0.01) + if m.rx_count > prev_rx: + return True + time.sleep(0.005) + return False + def get_position(self) -> float: """Current position in rad.""" if self._motor is None: diff --git a/tests/fake_can.py b/tests/fake_can.py index a726771..4e05462 100644 --- a/tests/fake_can.py +++ b/tests/fake_can.py @@ -76,6 +76,10 @@ def __init__(self, pos: float = 0.0, block_rad: Optional[float] = None, self._block_dir = 0.0 if block_rad is not None: self._block_dir = 1.0 if block_rad > pos else -1.0 + # 状态帧的年龄。假电机一直在线上，所以取 0（= 新鲜）；要模拟 + # 「失能电机不发状态帧」的用例，把这个值调大即可（见 + # ``GripperState.is_stale`` / ``STALE_AFTER_S``）。 + self.data_age_s = 0.0 def reported_pos(self) -> float: """上报位置（可量化 —— 模拟闭合侧约 0.0103 rad 的粘滑死区）。""" diff --git a/tests/test_enable.py b/tests/test_enable.py index 500cb59..a078673 100644 --- a/tests/test_enable.py +++ b/tests/test_enable.py @@ -15,12 +15,18 @@ class FakeMotorState: - """``MotorState`` 的最小替身（initialize 只碰这三个字段）。""" + """``MotorState`` 的最小替身。 + + ``initialize`` 使能后会把电机保持在**实测位置**上（见 + ``can_bus._enable_and_hold``），所以除了判定的三个字段，还要给出 + ``position`` —— 真 ``MotorState`` 一直都有。 + """ def __init__(self): self.rx_count = 0 self.error = 0 self.mst_id = 0x18 + self.position = 0.0 class FakeController: @@ -45,6 +51,10 @@ def disable(self, motor): def enable(self, motor): self.calls.append("enable") + # 错误码按 **enable 次数** 推进，而不是按 poll 次数：脚本描述的正是 + # 「第几次使能有没有生效」。_enable_and_hold 使能后会先流一小段零增益 + # 帧、期间 poll 多次，按 poll 推进会让脚本在那段覆盖里就被吃光。 + self.motor.error = self._next_err() def clear_fault(self, motor): self.calls.append("clear_fault") @@ -57,8 +67,8 @@ def control_mit(self, motor, kp, kd, q, dq=0.0, tau=0.0): self.calls.append("control_mit") def poll(self, timeout_s=0.0): + # 一次 poll 解出一帧状态；错误码由 enable 决定（见上）。 self.motor.rx_count += 1 - self.motor.error = self._next_err() return self.motor def close(self): diff --git a/tests/test_stale_state.py b/tests/test_stale_state.py new file mode 100644 index 0000000..2dc3aa2 --- /dev/null +++ b/tests/test_stale_state.py @@ -0,0 +1,132 @@ +"""陈旧状态与刷新帧：失能电机不主动发状态帧，读到的可能是缓存/默认值。 + +``MotorState`` / ``GripperState`` 现在带 ``data_age_s`` 与 ``is_stale``， +``refresh_status()`` 给出「主动要一帧」的路径 —— 失能状态下也能读到位置， +不必为了读位置先使能（使能本身可能让电机朝上一会话遗留的目标运动）。 +""" + +from __future__ import annotations + +import time +import unittest + +import _sdkpath # noqa: F401 +from litegrip.can.motor import MotorParams, MotorState +from litegrip.constants import describe_error +from litegrip.models import STALE_AFTER_S, GripperState +from litegrip.protocols.can_bus import LiteGripCAN + + +def _motor_with_frame(age_s: float = 0.0) -> MotorState: + """一个「刚收到一帧（或 age_s 秒前收到过一帧）」的 MotorState。""" + m = MotorState(MotorParams()) + m.update_from_status(position=0.5, velocity=0.0, torque=0.0, + error=1, t_mos=30, t_coil=31, + timestamp=time.monotonic() - age_s) + return m + + +class TestMotorStateFreshness(unittest.TestCase): + + def test_never_received_has_infinite_age_and_no_data(self): + m = MotorState(MotorParams()) + self.assertFalse(m.has_data) + self.assertEqual(m.data_age_s, float("inf")) + + def test_a_status_frame_makes_it_fresh_and_have_data(self): + m = _motor_with_frame() + self.assertTrue(m.has_data) + self.assertLess(m.data_age_s, STALE_AFTER_S) + + def test_age_is_measured_from_the_frame(self): + # 有数据，但已经旧了 —— 这两件事必须能分开判断：失能电机的典型 + # 状态就是「读到过，但此刻不新鲜」。 + m = _motor_with_frame(age_s=5.0) + self.assertTrue(m.has_data) + self.assertGreater(m.data_age_s, 4.0) + + +class TestGripperStateFreshness(unittest.TestCase): + + def test_default_state_has_no_data_and_is_stale(self): + s = GripperState() + self.assertFalse(s.has_data) + self.assertTrue(s.is_stale) + + def test_fresh_state_is_not_stale(self): + s = GripperState(data_age_s=0.0) + self.assertTrue(s.has_data) + self.assertFalse(s.is_stale) + + def test_threshold_is_exclusive(self): + self.assertFalse(GripperState(data_age_s=STALE_AFTER_S).is_stale) + self.assertTrue(GripperState(data_age_s=STALE_AFTER_S + 0.001).is_stale) + + +class TestCommLossIsNamed(unittest.TestCase): + """0xD 是 DM4310 在 CAN 静默约 900 ms 后 latch 的状态。 + + 未使能、或刚失能的那段时间就会看到它，所以它不能报成「未知错误」—— + 那会把一个正常状态当成硬件故障来排查。 + """ + + def test_comm_loss_has_a_description(self): + self.assertIn("通讯", describe_error(0xD)) + + def test_overvoltage_and_overload_have_descriptions(self): + self.assertIn("过压", describe_error(0x8)) + self.assertIn("过载", describe_error(0xE)) + + def test_unknown_codes_still_say_unknown(self): + self.assertIn("未知", describe_error(0x7F)) + + +class FakeRefreshController: + """只实现 ``refresh_status`` / ``poll`` 的最小控制器。""" + + def __init__(self, motor: MotorState, answer: bool): + self.motor = motor + self.answer = answer + self.refresh_calls = 0 + + def refresh_status(self, motor: MotorState) -> None: + self.refresh_calls += 1 + + def poll(self, timeout_s: float = 0.0): + if self.answer: + self.motor.update_from_status( + position=0.25, velocity=0.0, torque=0.0, + error=0, t_mos=30, t_coil=31, + timestamp=time.monotonic()) + return self.motor + + +def make_can(answer: bool = True): + can = LiteGripCAN(channel="vcan0") + motor = MotorState(MotorParams()) + can._motor = motor + can._controller = FakeRefreshController(motor, answer) + can._connected = True + return can, motor + + +class TestRefreshStatus(unittest.TestCase): + + def test_reads_a_position_while_disabled(self): + can, motor = make_can() + self.assertFalse(motor.has_data) # 还没使能，一帧都没有 + self.assertTrue(can.refresh_status(timeout_s=0.2)) + self.assertTrue(motor.has_data) + + def test_reports_failure_when_the_motor_is_silent(self): + can, _ = make_can(answer=False) + self.assertFalse(can.refresh_status(timeout_s=0.1)) + + def test_sends_the_refresh_command_and_no_motion(self): + can, _ = make_can() + can.refresh_status(timeout_s=0.2) + self.assertEqual(can._controller.refresh_calls, 1) + + +if __name__ == "__main__": + unittest.main()