140 lines
4.1 KiB
Python
140 lines
4.1 KiB
Python
#!/usr/bin/env python3
|
||
# -*- coding: utf-8 -*-
|
||
"""
|
||
瓴控电机 CAN 广播控制（V2.35）：
|
||
- 力矩/开环控制：帧ID 0x280，data[0..7] 为 4 个电机的 int16 torqueValue（低字节在前，小端）
|
||
- 混合命令：帧ID 0x288，data[0,2,4,6] 为 4 个电机 motorCmd（如 0x88 开启 / 0x81 停止 / 0x80 关闭）
|
||
|
||
依赖：
|
||
pip install python-can
|
||
|
||
运行前准备（Linux 推荐 SocketCAN）：
|
||
sudo ip link set can0 up type can bitrate 500000
|
||
或者 1000000（看你驱动与设备设置）
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
import time
|
||
from typing import Iterable, Tuple
|
||
|
||
import can
|
||
|
||
|
||
TORQUE_CMD_ID = 0x280
|
||
MIXED_CMD_ID = 0x288
|
||
|
||
# 混合命令 motorCmd（来自文档）
|
||
CMD_READ_STATUS1 = 0x9A
|
||
CMD_CLR_ERROR = 0x9B
|
||
CMD_READ_STATUS2 = 0x9C
|
||
CMD_MOTOR_OFF = 0x80
|
||
CMD_MOTOR_ON = 0x88
|
||
CMD_MOTOR_STOP = 0x81
|
||
|
||
|
||
def _clamp_int16(x: int) -> int:
|
||
"""限制到 int16 范围（-32768..32767），随后按补码打包。"""
|
||
return max(-32768, min(32767, int(x)))
|
||
|
||
|
||
def _pack_i16_le(x: int) -> Tuple[int, int]:
|
||
"""int16 -> (low_byte, high_byte) 小端。"""
|
||
x = _clamp_int16(x)
|
||
u = x & 0xFFFF
|
||
return (u & 0xFF, (u >> 8) & 0xFF)
|
||
|
||
|
||
def build_torque_broadcast(t1: int, t2: int, t3: int, t4: int = 0) -> can.Message:
|
||
"""
|
||
帧ID 0x280:
|
||
data[0..1]=#1 torqueValue (low,high)
|
||
data[2..3]=#2
|
||
data[4..5]=#3
|
||
data[6..7]=#4
|
||
"""
|
||
d0, d1 = _pack_i16_le(t1)
|
||
d2, d3 = _pack_i16_le(t2)
|
||
d4, d5 = _pack_i16_le(t3)
|
||
d6, d7 = _pack_i16_le(t4)
|
||
data = [d0, d1, d2, d3, d4, d5, d6, d7]
|
||
return can.Message(arbitration_id=TORQUE_CMD_ID, is_extended_id=False, data=data)
|
||
|
||
|
||
def build_mixed_cmd(cmd1: int, cmd2: int, cmd3: int, cmd4: int = 0x00) -> can.Message:
|
||
"""
|
||
帧ID 0x288:
|
||
data[0]=#1 motorCmd, data[1]=0
|
||
data[2]=#2 motorCmd, data[3]=0
|
||
data[4]=#3 motorCmd, data[5]=0
|
||
data[6]=#4 motorCmd, data[7]=0
|
||
"""
|
||
data = [cmd1 & 0xFF, 0x00, cmd2 & 0xFF, 0x00, cmd3 & 0xFF, 0x00, cmd4 & 0xFF, 0x00]
|
||
return can.Message(arbitration_id=MIXED_CMD_ID, is_extended_id=False, data=data)
|
||
|
||
|
||
def send_many(bus: can.BusABC, msgs: Iterable[can.Message], gap_s: float = 0.005) -> None:
|
||
"""连续发几帧（给驱动一点处理间隔）。"""
|
||
for m in msgs:
|
||
bus.send(m)
|
||
print(m)
|
||
time.sleep(gap_s)
|
||
|
||
|
||
def main():
|
||
# 1) 选择 CAN 接口
|
||
# - Linux SocketCAN: channel="can0", bustype="socketcan"
|
||
# - 如果你用的是 USB-CAN 且已有 python-can 对应驱动，也可改 bustype/channel
|
||
bus = can.Bus(interface="canalystii", channel=0, bitrate=1000000)
|
||
|
||
# 2) 可选：先广播“电机开启”
|
||
send_many(bus, [build_mixed_cmd(CMD_MOTOR_ON, CMD_MOTOR_ON, CMD_MOTOR_ON, CMD_MOTOR_ON)], gap_s=0.02)
|
||
|
||
# 3) 力矩模式：控制 3 个电机一起转（示例：#1=+T, #2=-T, #3=+T，第4路=0）
|
||
# 注意 torqueValue 的具体量程与含义：文档写 MF/MG 为 -2000..2000（电流力矩），MS 为 -850..850（开环电压）
|
||
# 发送频率：文档提示 500kbps 下 4 电机广播最大约 600Hz；1Mbps 下约 1.2kHz。这里示例用 200Hz 更稳妥。
|
||
freq_hz = 500.0
|
||
dt = 1.0 / freq_hz
|
||
|
||
T = 200 # 示例力矩指令（根据你的电机系列调整范围）
|
||
|
||
count = 0
|
||
|
||
try:
|
||
while True:
|
||
# now = time.time()
|
||
# if now - t0 > duration_s:
|
||
# break
|
||
|
||
msg = build_torque_broadcast(
|
||
t1=50,
|
||
t2=50,
|
||
t3=40,
|
||
t4=50
|
||
)
|
||
bus.send(msg)
|
||
|
||
count += 1
|
||
if count % 100 == 0:
|
||
print(msg)
|
||
|
||
time.sleep(dt)
|
||
|
||
except KeyboardInterrupt:
|
||
pass
|
||
finally:
|
||
# 4) 停止（建议退出前先 stop，再按需 off）
|
||
msg = build_torque_broadcast(
|
||
t1=0,
|
||
t2=0,
|
||
t3=0,
|
||
t4=0
|
||
)
|
||
bus.send(msg)
|
||
send_many(bus, [build_mixed_cmd(CMD_MOTOR_STOP, CMD_MOTOR_STOP, CMD_MOTOR_STOP, CMD_MOTOR_STOP)], gap_s=0.02)
|
||
send_many(bus, [build_mixed_cmd(CMD_MOTOR_OFF, CMD_MOTOR_OFF, CMD_MOTOR_OFF, CMD_MOTOR_OFF)], gap_s=0.02)
|
||
bus.shutdown()
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|