SmartBench/aima_phone_catcher
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: swap open and close motor directions

Browse Source
This commit is contained in:
xtkuang 2026-08-13 11:31:35 +08:00
parent 83312c277c
commit d3a1e4c824
6 changed files with 58 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.env.example
View File

@ -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

48
README.md
View File

@ -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`

4
src/lingkong_motor_service/controller.py
View File

@ -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)

12
tests/test_api.py
View File

@ -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"

42
tests/test_controller.py
View File

@ -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()

8
tests/test_protocol.py
View File

@ -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])