From d3a1e4c824b86a37a1966451c70b024b56e9628c Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Thu, 13 Aug 2026 11:31:35 +0800 Subject: [PATCH] fix: swap open and close motor directions --- .env.example | 2 +- README.md | 48 ++++++++++++------------ src/lingkong_motor_service/controller.py | 4 +- tests/test_api.py | 12 +++--- tests/test_controller.py | 42 ++++++++++----------- tests/test_protocol.py | 8 ++-- 6 files changed, 58 insertions(+), 58 deletions(-) diff --git a/.env.example b/.env.example index 5f6a285..67ca7d6 100644 --- a/.env.example +++ b/.env.example @@ -11,7 +11,7 @@ MOTOR_CAN_BITRATE=1000000 MOTOR_IDS=1,2 # MS 的 0x280 是开环 raw，不是力矩或 N·m；协议范围为 -850..+850。 -# HTTP open：ID1=+400、ID2=-400；HTTP close：ID1=-400、ID2=+400。 +# HTTP open：ID1=-400、ID2=+400；HTTP close：ID1=+400、ID2=-400。 # 这两个配置值是绝对值，每台电机的符号由程序固定映射。 MOTOR_OPEN_CONTROL_RAW=400 MOTOR_CLOSE_CONTROL_RAW=400 diff --git a/README.md b/README.md index 977de7b..3de0849 100644 --- a/README.md +++ b/README.md @@ -10,8 +10,8 @@ - 电机 ID：`1,2` - 广播控制帧：标准帧 `0x280`，DLC 8 - 持续广播频率：`10 Hz` -- HTTP `open`：ID 1 = `+400` raw，ID 2 = `-400` raw -- HTTP `close`：ID 1 = `-400` raw，ID 2 = `+400` raw +- HTTP `open`：ID 1 = `-400` raw，ID 2 = `+400` raw +- HTTP `close`：ID 1 = `+400` raw，ID 2 = `-400` raw - `open`/`close` 持续 `5 s` 后：保持方向，两台电机幅值降为 `100` raw - 温度达到 `80 C`：两台电机控制量归零并发送 Stop - 服务启动：自动 Enable；正常退出：自动 Stop 后 Disable @@ -19,8 +19,8 @@ 同时控制 ID 1、2 时，固定控制帧分别为： -- `open`：`0x280 90 01 70 FE 00 00 00 00` -- `close`：`0x280 70 FE 90 01 00 00 00 00` +- `open`：`0x280 70 FE 90 01 00 00 00 00` +- `close`：`0x280 90 01 70 FE 00 00 00 00` > `400` 是 MS3008 的协议原始开环控制量，不是力矩，也不是 N·m。 > 正负号只表示协议方向；机械正反方向必须逐台、低值、短脉冲实测确认。 @@ -135,12 +135,12 @@ MOTOR_API_TOKEN= ``` `MOTOR_OPEN_CONTROL_RAW` 和 `MOTOR_CLOSE_CONTROL_RAW` 是幅值；程序按电机 ID -固定方向：`open` 为 `{1: +400, 2: -400}`，`close` 为 -`{1: -400, 2: +400}`。 +固定方向：`open` 为 `{1: -400, 2: +400}`，`close` 为 +`{1: +400, 2: -400}`。 每次调用 `open` 或 `close` 都会重新开始计时。持续 5 秒后，程序保持当前方向并把 -两台电机的开环 raw 幅值降至 100，即 `open={1:+100,2:-100}`、 -`close={1:-100,2:+100}`。方向切换也会重新开始 5 秒计时。 +两台电机的开环 raw 幅值降至 100，即 `open={1:-100,2:+100}`、 +`close={1:+100,2:-100}`。方向切换也会重新开始 5 秒计时。 运动期间服务每 0.5 秒发送一次 `0x9C` 状态2请求并读取温度。如果任一配置电机回报 温度达到或超过 `80 C`，程序会把两台电机控制量都设为 0、发送 Stop，并锁存温度保护； @@ -183,14 +183,14 @@ uv run --locked lingkong-motor-service `uv run --locked python tests/test_can.py status`，观察 ID 1、2 的 `0x9C` 状态回包作为独立诊断。运行该脚本时不得同时启动 HTTP 服务。 3. 临时设置 `MOTOR_IDS=1`，将两个方向的控制量都改为 `20`。服务启动并自动使能后只调用一次 - `open`（服务内部会以 10 Hz 持续广播 ID 1 = `+20`），约 `100 ms` 时必须显式调用 `stop`； - 确认完全停稳后，才以同样方式测试 `close` 的 ID 1 = `-20/100 ms`，再显式 `stop`、停稳并 + `open`（服务内部会以 10 Hz 持续广播 ID 1 = `-20`），约 `100 ms` 时必须显式调用 `stop`； + 确认完全停稳后，才以同样方式测试 `close` 的 ID 1 = `+20/100 ms`，再显式 `stop`、停稳并 最后用 `Ctrl-C` 正常退出，由服务自动 Stop 并 Disable。 -4. 保持 raw `20`，把 `MOTOR_IDS` 改为 `2`，按 ID 2 `open=-20`、 - `close=+20` 的预期验证第二台电机。 +4. 保持 raw `20`，把 `MOTOR_IDS` 改为 `2`，按 ID 2 `open=+20`、 + `close=-20` 的预期验证第二台电机。 5. 两台的状态、方向和停机都确认后，才逐级提高控制量。每一级、每个方向之间 - 都必须 Stop 并确认机械完全停稳，最终映射为 `open={1:+400,2:-400}`、 - `close={1:-400,2:+400}`。 + 都必须 Stop 并确认机械完全停稳，最终映射为 `open={1:-400,2:+400}`、 + `close={1:+400,2:-400}`。 6. 首次方向验证仍必须执行“当前方向 → Stop → 确认停稳 → 另一方向”。正常运行时 `open → close` 和 `close → open` 都可直接调用；服务会自动停止当前广播，发送零输出 与 Stop，等待 `20 ms` 后开始另一方向广播。该等待不代表机械轴已经停稳。 @@ -210,20 +210,20 @@ uv run --locked lingkong-motor-service uv run --locked python tests/test_can.py status ``` -发送 `open` 映射（ID 1 = `+400`、ID 2 = `-400`，持续 10 Hz， -`Ctrl-C` 后自动零输出并 Stop）： - -```bash -uv run --locked python tests/test_can.py torque 400 -400 -``` - -发送 `close` 映射（ID 1 = `-400`、ID 2 = `+400`，持续 10 Hz， +发送 `open` 映射（ID 1 = `-400`、ID 2 = `+400`，持续 10 Hz， `Ctrl-C` 后自动零输出并 Stop）： ```bash uv run --locked python tests/test_can.py torque -400 400 ``` +发送 `close` 映射（ID 1 = `+400`、ID 2 = `-400`，持续 10 Hz， +`Ctrl-C` 后自动零输出并 Stop）： + +```bash +uv run --locked python tests/test_can.py torque 400 -400 +``` + 停止 ID 1、2： ```bash @@ -269,8 +269,8 @@ HTTP `409`，CAN/USB 通讯失败仍返回 HTTP `503`。 `MOTOR_HARDWARE_ENABLED=true` 时，启动过程会自动 Enable。HTTP 只保留 `open`、`close`、`stop` 和 `status`，不提供 `enable`/`disable`。 -`open` 固定使用 `{1:+400,2:-400}`，`close` 固定使用 -`{1:-400,2:+400}`；HTTP 请求不能临时提高 raw。状态响应中 +`open` 固定使用 `{1:-400,2:+400}`，`close` 固定使用 +`{1:+400,2:-400}`；HTTP 请求不能临时提高 raw。状态响应中 `control_raw_by_motor` 显示每台电机的实际有符号控制量；保留的 `control_raw` 字段表示 ID 1（单独配置 ID 2 时表示 ID 2）。调用任一运动接口后，服务内部线程都会以 `MOTOR_BROADCAST_FREQUENCY_HZ` diff --git a/src/lingkong_motor_service/controller.py b/src/lingkong_motor_service/controller.py index 9883c03..d6472f5 100644 --- a/src/lingkong_motor_service/controller.py +++ b/src/lingkong_motor_service/controller.py @@ -156,14 +156,14 @@ class MotorController: def open(self) -> MotorStatus: values = { - motor_id: self._settings.open_control_raw * (1 if motor_id == 1 else -1) + motor_id: self._settings.open_control_raw * (-1 if motor_id == 1 else 1) for motor_id in self._settings.motor_ids } return self._move(values, MotorState.OPENING) def close(self) -> MotorStatus: values = { - motor_id: self._settings.close_control_raw * (-1 if motor_id == 1 else 1) + motor_id: self._settings.close_control_raw * (1 if motor_id == 1 else -1) for motor_id in self._settings.motor_ids } return self._move(values, MotorState.CLOSING) diff --git a/tests/test_api.py b/tests/test_api.py index c6b7240..b2899b8 100644 --- a/tests/test_api.py +++ b/tests/test_api.py @@ -104,8 +104,8 @@ def test_http_open_and_close_can_switch_directions() -> None: assert opening.json()["code"] == 200 assert opening.json()["action"] == "open" assert opening.json()["state"] == "opening" - assert opening.json()["control_raw"] == 400 - assert opening.json()["control_raw_by_motor"] == {"1": 400, "2": -400} + assert opening.json()["control_raw"] == -400 + assert opening.json()["control_raw_by_motor"] == {"1": -400, "2": 400} assert opening.json()["control_mode"] == "open_loop" assert opening.json()["streaming"] is True assert opening.json()["broadcast_frequency_hz"] == 10 @@ -115,15 +115,15 @@ def test_http_open_and_close_can_switch_directions() -> None: assert closing.json()["code"] == 200 assert closing.json()["action"] == "close" assert closing.json()["state"] == "closing" - assert closing.json()["control_raw"] == -400 - assert closing.json()["control_raw_by_motor"] == {"1": -400, "2": 400} + assert closing.json()["control_raw"] == 400 + assert closing.json()["control_raw_by_motor"] == {"1": 400, "2": -400} assert "not verified" in closing.json()["transmission"] assert reopened.status_code == 200 assert reopened.json()["code"] == 200 assert reopened.json()["action"] == "open" assert reopened.json()["state"] == "opening" - assert reopened.json()["control_raw"] == 400 - assert reopened.json()["control_raw_by_motor"] == {"1": 400, "2": -400} + assert reopened.json()["control_raw"] == -400 + assert reopened.json()["control_raw_by_motor"] == {"1": -400, "2": 400} assert stopped.status_code == 200 assert stopped.json()["code"] == 200 assert stopped.json()["state"] == "stopped" diff --git a/tests/test_controller.py b/tests/test_controller.py index e918222..1bcb46f 100644 --- a/tests/test_controller.py +++ b/tests/test_controller.py @@ -105,44 +105,44 @@ def test_open_and_close_can_switch_directions_without_manual_stop() -> None: ] opening = controller.open() - assert opening.control_raw == 400 - assert opening.control_raw_by_motor == {1: 400, 2: -400} + assert opening.control_raw == -400 + assert opening.control_raw_by_motor == {1: -400, 2: 400} assert opening.state is MotorState.OPENING assert opening.streaming is True - wait_for_payload_count(transport, bytes.fromhex("90 01 70 FE 00 00 00 00"), 3) + wait_for_payload_count(transport, bytes.fromhex("70 FE 90 01 00 00 00 00"), 3) transition_start = len(frame_dump(transport)) closing = controller.close() - assert closing.control_raw == -400 - assert closing.control_raw_by_motor == {1: -400, 2: 400} + assert closing.control_raw == 400 + assert closing.control_raw_by_motor == {1: 400, 2: -400} assert closing.state is MotorState.CLOSING assert closing.streaming is True - wait_for_payload_count(transport, bytes.fromhex("70 FE 90 01 00 00 00 00"), 3) + wait_for_payload_count(transport, bytes.fromhex("90 01 70 FE 00 00 00 00"), 3) transition_frames = frame_dump(transport)[transition_start:] - first_close = transition_frames.index((CONTROL_FRAME_ID, "70fe900100000000")) + first_close = transition_frames.index((CONTROL_FRAME_ID, "900170fe00000000")) assert (CONTROL_FRAME_ID, "0000000000000000") in transition_frames[:first_close] assert (MIXED_COMMAND_FRAME_ID, "8100810000000000") in transition_frames[:first_close] assert not any( - frame == (CONTROL_FRAME_ID, "900170fe00000000") for frame in transition_frames[first_close:] + frame == (CONTROL_FRAME_ID, "70fe900100000000") for frame in transition_frames[first_close:] ) - open_payload = bytes.fromhex("90 01 70 FE 00 00 00 00") + open_payload = bytes.fromhex("70 FE 90 01 00 00 00 00") open_count_before = sum(frame.data == open_payload for frame in transport.frames) transition_start = len(frame_dump(transport)) reopened = controller.open() - assert reopened.control_raw == 400 - assert reopened.control_raw_by_motor == {1: 400, 2: -400} + assert reopened.control_raw == -400 + assert reopened.control_raw_by_motor == {1: -400, 2: 400} assert reopened.state is MotorState.OPENING assert reopened.streaming is True wait_for_payload_count(transport, open_payload, open_count_before + 3) transition_frames = frame_dump(transport)[transition_start:] - first_open = transition_frames.index((CONTROL_FRAME_ID, "900170fe00000000")) + first_open = transition_frames.index((CONTROL_FRAME_ID, "70fe900100000000")) assert (CONTROL_FRAME_ID, "0000000000000000") in transition_frames[:first_open] assert (MIXED_COMMAND_FRAME_ID, "8100810000000000") in transition_frames[:first_open] assert not any( - frame == (CONTROL_FRAME_ID, "70fe900100000000") for frame in transition_frames[first_open:] + frame == (CONTROL_FRAME_ID, "900170fe00000000") for frame in transition_frames[first_open:] ) stopped = controller.stop() @@ -159,7 +159,7 @@ def test_open_to_close_stop_failure_never_sends_close_control() -> None: controller = MotorController(enabled_settings(), transport) controller.enable() controller.open() - wait_for_payload_count(transport, bytes.fromhex("90 01 70 FE 00 00 00 00"), 3) + wait_for_payload_count(transport, bytes.fromhex("70 FE 90 01 00 00 00 00"), 3) with pytest.raises(MotorCommunicationError, match="transition stop failure"): controller.close() @@ -168,7 +168,7 @@ def test_open_to_close_stop_failure_never_sends_close_control() -> None: assert status.state is MotorState.FAULT assert status.last_error == "injected transition stop failure" assert not any( - frame.data == bytes.fromhex("70 FE 90 01 00 00 00 00") for frame in transport.frames + frame.data == bytes.fromhex("90 01 70 FE 00 00 00 00") for frame in transport.frames ) controller.shutdown() @@ -176,8 +176,8 @@ def test_open_to_close_stop_failure_never_sends_close_control() -> None: @pytest.mark.parametrize( ("motor_ids", "opening_values", "closing_values", "opening_payload", "closing_payload"), [ - ((1,), {1: 400}, {1: -400}, "9001000000000000", "70fe000000000000"), - ((2,), {2: -400}, {2: 400}, "000070fe00000000", "0000900100000000"), + ((1,), {1: -400}, {1: 400}, "70fe000000000000", "9001000000000000"), + ((2,), {2: 400}, {2: -400}, "0000900100000000", "000070fe00000000"), ], ) def test_single_motor_keeps_its_configured_open_and_close_direction( @@ -204,7 +204,7 @@ def test_open_keeps_broadcasting_until_explicit_stop() -> None: controller = MotorController(enabled_settings(), transport) controller.enable() controller.open() - payload = bytes.fromhex("90 01 70 FE 00 00 00 00") + payload = bytes.fromhex("70 FE 90 01 00 00 00 00") wait_for_payload_count(transport, payload, 3) count_before_wait = sum(frame.data == payload for frame in transport.frames) @@ -228,8 +228,8 @@ def test_open_keeps_broadcasting_until_explicit_stop() -> None: @pytest.mark.parametrize( ("action", "initial_payload", "derated_payload", "expected_values"), [ - ("open", "900170fe00000000", "64009cff00000000", {1: 100, 2: -100}), - ("close", "70fe900100000000", "9cff640000000000", {1: -100, 2: 100}), + ("open", "70fe900100000000", "9cff640000000000", {1: -100, 2: 100}), + ("close", "900170fe00000000", "64009cff00000000", {1: 100, 2: -100}), ], ) def test_motion_derates_after_configured_delay_without_changing_direction( @@ -376,7 +376,7 @@ def test_shutdown_joins_sender_before_zero_stop_disable_and_transport_shutdown() controller = MotorController(enabled_settings(), transport) controller.enable() controller.open() - wait_for_payload_count(transport, bytes.fromhex("90 01 70 FE 00 00 00 00"), 3) + wait_for_payload_count(transport, bytes.fromhex("70 FE 90 01 00 00 00 00"), 3) controller.shutdown() diff --git a/tests/test_protocol.py b/tests/test_protocol.py index 243145c..b002cf4 100644 --- a/tests/test_protocol.py +++ b/tests/test_protocol.py @@ -25,11 +25,11 @@ def test_pdf_golden_vector_positive_100_motor_1_negative_100_motor_3() -> None: def test_requested_open_and_close_vectors_for_motors_1_and_2() -> None: - opening = build_open_loop_frame({1: 400, 2: -400}) - closing = build_open_loop_frame({1: -400, 2: 400}) + opening = build_open_loop_frame({1: -400, 2: 400}) + closing = build_open_loop_frame({1: 400, 2: -400}) - assert opening.data == bytes.fromhex("90 01 70 FE 00 00 00 00") - assert closing.data == bytes.fromhex("70 FE 90 01 00 00 00 00") + assert opening.data == bytes.fromhex("70 FE 90 01 00 00 00 00") + assert closing.data == bytes.fromhex("90 01 70 FE 00 00 00 00") @pytest.mark.parametrize("value", [-851, 851])