fix: swap open and close motor directions

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

View File

@ -11,7 +11,7 @@ MOTOR_CAN_BITRATE=1000000
MOTOR_IDS=1,2 MOTOR_IDS=1,2
# MS 的 0x280 是开环 raw,不是力矩或 N·m;协议范围为 -850..+850。 # 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_OPEN_CONTROL_RAW=400
MOTOR_CLOSE_CONTROL_RAW=400 MOTOR_CLOSE_CONTROL_RAW=400

View File

@ -10,8 +10,8 @@
- 电机 ID:`1,2` - 电机 ID:`1,2`
- 广播控制帧:标准帧 `0x280`,DLC 8 - 广播控制帧:标准帧 `0x280`,DLC 8
- 持续广播频率:`10 Hz` - 持续广播频率:`10 Hz`
- HTTP `open`: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 - HTTP `close`:ID 1 = `+400` raw,ID 2 = `-400` raw
- `open`/`close` 持续 `5 s` 后:保持方向,两台电机幅值降为 `100` raw - `open`/`close` 持续 `5 s` 后:保持方向,两台电机幅值降为 `100` raw
- 温度达到 `80 C`:两台电机控制量归零并发送 Stop - 温度达到 `80 C`:两台电机控制量归零并发送 Stop
- 服务启动:自动 Enable;正常退出:自动 Stop 后 Disable - 服务启动:自动 Enable;正常退出:自动 Stop 后 Disable
@ -19,8 +19,8 @@
同时控制 ID 1、2 时,固定控制帧分别为: 同时控制 ID 1、2 时,固定控制帧分别为:
- `open`:`0x280 90 01 70 FE 00 00 00 00` - `open`:`0x280 70 FE 90 01 00 00 00 00`
- `close`:`0x280 70 FE 90 01 00 00 00 00` - `close`:`0x280 90 01 70 FE 00 00 00 00`
> `400` 是 MS3008 的协议原始开环控制量,不是力矩,也不是 N·m。 > `400` 是 MS3008 的协议原始开环控制量,不是力矩,也不是 N·m。
> 正负号只表示协议方向;机械正反方向必须逐台、低值、短脉冲实测确认。 > 正负号只表示协议方向;机械正反方向必须逐台、低值、短脉冲实测确认。
@ -135,12 +135,12 @@ MOTOR_API_TOKEN=
``` ```
`MOTOR_OPEN_CONTROL_RAW` 和 `MOTOR_CLOSE_CONTROL_RAW` 是幅值;程序按电机 ID `MOTOR_OPEN_CONTROL_RAW` 和 `MOTOR_CLOSE_CONTROL_RAW` 是幅值;程序按电机 ID
固定方向:`open` 为 `{1: +400, 2: -400}`,`close` 为 固定方向:`open` 为 `{1: -400, 2: +400}`,`close` 为
`{1: -400, 2: +400}`。 `{1: +400, 2: -400}`。
每次调用 `open` 或 `close` 都会重新开始计时。持续 5 秒后,程序保持当前方向并把 每次调用 `open` 或 `close` 都会重新开始计时。持续 5 秒后,程序保持当前方向并把
两台电机的开环 raw 幅值降至 100,即 `open={1:+100,2:-100}`、 两台电机的开环 raw 幅值降至 100,即 `open={1:-100,2:+100}`、
`close={1:-100,2:+100}`。方向切换也会重新开始 5 秒计时。 `close={1:+100,2:-100}`。方向切换也会重新开始 5 秒计时。
运动期间服务每 0.5 秒发送一次 `0x9C` 状态2请求并读取温度。如果任一配置电机回报 运动期间服务每 0.5 秒发送一次 `0x9C` 状态2请求并读取温度。如果任一配置电机回报
温度达到或超过 `80 C`,程序会把两台电机控制量都设为 0、发送 Stop,并锁存温度保护; 温度达到或超过 `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` `uv run --locked python tests/test_can.py status`,观察 ID 1、2 的 `0x9C`
状态回包作为独立诊断。运行该脚本时不得同时启动 HTTP 服务。 状态回包作为独立诊断。运行该脚本时不得同时启动 HTTP 服务。
3. 临时设置 `MOTOR_IDS=1`,将两个方向的控制量都改为 `20`。服务启动并自动使能后只调用一次 3. 临时设置 `MOTOR_IDS=1`,将两个方向的控制量都改为 `20`。服务启动并自动使能后只调用一次
`open`(服务内部会以 10 Hz 持续广播 ID 1 = `+20`),约 `100 ms` 时必须显式调用 `stop`; `open`(服务内部会以 10 Hz 持续广播 ID 1 = `-20`),约 `100 ms` 时必须显式调用 `stop`;
确认完全停稳后,才以同样方式测试 `close` 的 ID 1 = `-20/100 ms`,再显式 `stop`、停稳并 确认完全停稳后,才以同样方式测试 `close` 的 ID 1 = `+20/100 ms`,再显式 `stop`、停稳并
最后用 `Ctrl-C` 正常退出,由服务自动 Stop 并 Disable。 最后用 `Ctrl-C` 正常退出,由服务自动 Stop 并 Disable。
4. 保持 raw `20`,把 `MOTOR_IDS` 改为 `2`,按 ID 2 `open=-20`、 4. 保持 raw `20`,把 `MOTOR_IDS` 改为 `2`,按 ID 2 `open=+20`、
`close=+20` 的预期验证第二台电机。 `close=-20` 的预期验证第二台电机。
5. 两台的状态、方向和停机都确认后,才逐级提高控制量。每一级、每个方向之间 5. 两台的状态、方向和停机都确认后,才逐级提高控制量。每一级、每个方向之间
都必须 Stop 并确认机械完全停稳,最终映射为 `open={1:+400,2:-400}`、 都必须 Stop 并确认机械完全停稳,最终映射为 `open={1:-400,2:+400}`、
`close={1:-400,2:+400}`。 `close={1:+400,2:-400}`。
6. 首次方向验证仍必须执行“当前方向 → Stop → 确认停稳 → 另一方向”。正常运行时 6. 首次方向验证仍必须执行“当前方向 → Stop → 确认停稳 → 另一方向”。正常运行时
`open → close` 和 `close → open` 都可直接调用;服务会自动停止当前广播,发送零输出 `open → close` 和 `close → open` 都可直接调用;服务会自动停止当前广播,发送零输出
与 Stop,等待 `20 ms` 后开始另一方向广播。该等待不代表机械轴已经停稳。 与 Stop,等待 `20 ms` 后开始另一方向广播。该等待不代表机械轴已经停稳。
@ -210,20 +210,20 @@ uv run --locked lingkong-motor-service
uv run --locked python tests/test_can.py status uv run --locked python tests/test_can.py status
``` ```
发送 `open` 映射(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): `Ctrl-C` 后自动零输出并 Stop):
```bash ```bash
uv run --locked python tests/test_can.py torque -400 400 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: 停止 ID 1、2:
```bash ```bash
@ -269,8 +269,8 @@ HTTP `409`,CAN/USB 通讯失败仍返回 HTTP `503`。
`MOTOR_HARDWARE_ENABLED=true` 时,启动过程会自动 Enable。HTTP 只保留 `MOTOR_HARDWARE_ENABLED=true` 时,启动过程会自动 Enable。HTTP 只保留
`open`、`close`、`stop` 和 `status`,不提供 `enable`/`disable`。 `open`、`close`、`stop` 和 `status`,不提供 `enable`/`disable`。
`open` 固定使用 `{1:+400,2:-400}`,`close` 固定使用 `open` 固定使用 `{1:-400,2:+400}`,`close` 固定使用
`{1:-400,2:+400}`;HTTP 请求不能临时提高 raw。状态响应中 `{1:+400,2:-400}`;HTTP 请求不能临时提高 raw。状态响应中
`control_raw_by_motor` 显示每台电机的实际有符号控制量;保留的 `control_raw` `control_raw_by_motor` 显示每台电机的实际有符号控制量;保留的 `control_raw`
字段表示 ID 1(单独配置 ID 2 时表示 ID 2)。调用任一运动接口后,服务内部线程都会以 字段表示 ID 1(单独配置 ID 2 时表示 ID 2)。调用任一运动接口后,服务内部线程都会以
`MOTOR_BROADCAST_FREQUENCY_HZ` `MOTOR_BROADCAST_FREQUENCY_HZ`

View File

@ -156,14 +156,14 @@ class MotorController:
def open(self) -> MotorStatus: def open(self) -> MotorStatus:
values = { 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 for motor_id in self._settings.motor_ids
} }
return self._move(values, MotorState.OPENING) return self._move(values, MotorState.OPENING)
def close(self) -> MotorStatus: def close(self) -> MotorStatus:
values = { 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 for motor_id in self._settings.motor_ids
} }
return self._move(values, MotorState.CLOSING) return self._move(values, MotorState.CLOSING)

View File

@ -104,8 +104,8 @@ def test_http_open_and_close_can_switch_directions() -> None:
assert opening.json()["code"] == 200 assert opening.json()["code"] == 200
assert opening.json()["action"] == "open" assert opening.json()["action"] == "open"
assert opening.json()["state"] == "opening" assert opening.json()["state"] == "opening"
assert opening.json()["control_raw"] == 400 assert opening.json()["control_raw"] == -400
assert opening.json()["control_raw_by_motor"] == {"1": 400, "2": -400} assert opening.json()["control_raw_by_motor"] == {"1": -400, "2": 400}
assert opening.json()["control_mode"] == "open_loop" assert opening.json()["control_mode"] == "open_loop"
assert opening.json()["streaming"] is True assert opening.json()["streaming"] is True
assert opening.json()["broadcast_frequency_hz"] == 10 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()["code"] == 200
assert closing.json()["action"] == "close" assert closing.json()["action"] == "close"
assert closing.json()["state"] == "closing" assert closing.json()["state"] == "closing"
assert closing.json()["control_raw"] == -400 assert closing.json()["control_raw"] == 400
assert closing.json()["control_raw_by_motor"] == {"1": -400, "2": 400} assert closing.json()["control_raw_by_motor"] == {"1": 400, "2": -400}
assert "not verified" in closing.json()["transmission"] assert "not verified" in closing.json()["transmission"]
assert reopened.status_code == 200 assert reopened.status_code == 200
assert reopened.json()["code"] == 200 assert reopened.json()["code"] == 200
assert reopened.json()["action"] == "open" assert reopened.json()["action"] == "open"
assert reopened.json()["state"] == "opening" assert reopened.json()["state"] == "opening"
assert reopened.json()["control_raw"] == 400 assert reopened.json()["control_raw"] == -400
assert reopened.json()["control_raw_by_motor"] == {"1": 400, "2": -400} assert reopened.json()["control_raw_by_motor"] == {"1": -400, "2": 400}
assert stopped.status_code == 200 assert stopped.status_code == 200
assert stopped.json()["code"] == 200 assert stopped.json()["code"] == 200
assert stopped.json()["state"] == "stopped" assert stopped.json()["state"] == "stopped"

View File

@ -105,44 +105,44 @@ def test_open_and_close_can_switch_directions_without_manual_stop() -> None:
] ]
opening = controller.open() opening = controller.open()
assert opening.control_raw == 400 assert opening.control_raw == -400
assert opening.control_raw_by_motor == {1: 400, 2: -400} assert opening.control_raw_by_motor == {1: -400, 2: 400}
assert opening.state is MotorState.OPENING assert opening.state is MotorState.OPENING
assert opening.streaming is True 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)) transition_start = len(frame_dump(transport))
closing = controller.close() closing = controller.close()
assert closing.control_raw == -400 assert closing.control_raw == 400
assert closing.control_raw_by_motor == {1: -400, 2: 400} assert closing.control_raw_by_motor == {1: 400, 2: -400}
assert closing.state is MotorState.CLOSING assert closing.state is MotorState.CLOSING
assert closing.streaming is True 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:] 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 (CONTROL_FRAME_ID, "0000000000000000") in transition_frames[:first_close]
assert (MIXED_COMMAND_FRAME_ID, "8100810000000000") in transition_frames[:first_close] assert (MIXED_COMMAND_FRAME_ID, "8100810000000000") in transition_frames[:first_close]
assert not any( 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) open_count_before = sum(frame.data == open_payload for frame in transport.frames)
transition_start = len(frame_dump(transport)) transition_start = len(frame_dump(transport))
reopened = controller.open() reopened = controller.open()
assert reopened.control_raw == 400 assert reopened.control_raw == -400
assert reopened.control_raw_by_motor == {1: 400, 2: -400} assert reopened.control_raw_by_motor == {1: -400, 2: 400}
assert reopened.state is MotorState.OPENING assert reopened.state is MotorState.OPENING
assert reopened.streaming is True assert reopened.streaming is True
wait_for_payload_count(transport, open_payload, open_count_before + 3) wait_for_payload_count(transport, open_payload, open_count_before + 3)
transition_frames = frame_dump(transport)[transition_start:] 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 (CONTROL_FRAME_ID, "0000000000000000") in transition_frames[:first_open]
assert (MIXED_COMMAND_FRAME_ID, "8100810000000000") in transition_frames[:first_open] assert (MIXED_COMMAND_FRAME_ID, "8100810000000000") in transition_frames[:first_open]
assert not any( 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() 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 = MotorController(enabled_settings(), transport)
controller.enable() controller.enable()
controller.open() 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"): with pytest.raises(MotorCommunicationError, match="transition stop failure"):
controller.close() 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.state is MotorState.FAULT
assert status.last_error == "injected transition stop failure" assert status.last_error == "injected transition stop failure"
assert not any( 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() controller.shutdown()
@ -176,8 +176,8 @@ def test_open_to_close_stop_failure_never_sends_close_control() -> None:
@pytest.mark.parametrize( @pytest.mark.parametrize(
("motor_ids", "opening_values", "closing_values", "opening_payload", "closing_payload"), ("motor_ids", "opening_values", "closing_values", "opening_payload", "closing_payload"),
[ [
((1,), {1: 400}, {1: -400}, "9001000000000000", "70fe000000000000"), ((1,), {1: -400}, {1: 400}, "70fe000000000000", "9001000000000000"),
((2,), {2: -400}, {2: 400}, "000070fe00000000", "0000900100000000"), ((2,), {2: 400}, {2: -400}, "0000900100000000", "000070fe00000000"),
], ],
) )
def test_single_motor_keeps_its_configured_open_and_close_direction( 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 = MotorController(enabled_settings(), transport)
controller.enable() controller.enable()
controller.open() 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) wait_for_payload_count(transport, payload, 3)
count_before_wait = sum(frame.data == payload for frame in transport.frames) 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( @pytest.mark.parametrize(
("action", "initial_payload", "derated_payload", "expected_values"), ("action", "initial_payload", "derated_payload", "expected_values"),
[ [
("open", "900170fe00000000", "64009cff00000000", {1: 100, 2: -100}), ("open", "70fe900100000000", "9cff640000000000", {1: -100, 2: 100}),
("close", "70fe900100000000", "9cff640000000000", {1: -100, 2: 100}), ("close", "900170fe00000000", "64009cff00000000", {1: 100, 2: -100}),
], ],
) )
def test_motion_derates_after_configured_delay_without_changing_direction( 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 = MotorController(enabled_settings(), transport)
controller.enable() controller.enable()
controller.open() 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() controller.shutdown()

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: def test_requested_open_and_close_vectors_for_motors_1_and_2() -> None:
opening = 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}) closing = build_open_loop_frame({1: 400, 2: -400})
assert opening.data == bytes.fromhex("90 01 70 FE 00 00 00 00") assert opening.data == bytes.fromhex("70 FE 90 01 00 00 00 00")
assert closing.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]) @pytest.mark.parametrize("value", [-851, 851])