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