81 lines
2.3 KiB
Python
81 lines
2.3 KiB
Python
import time
|
||
import struct
|
||
import can
|
||
|
||
|
||
def arb_id_single(motor_id: int) -> int:
|
||
# 单电机命令：0x140 + ID(1~32)
|
||
return 0x140 + motor_id
|
||
|
||
|
||
def send_cmd(bus, motor_id: int, data8: bytes):
|
||
msg = can.Message(
|
||
arbitration_id=arb_id_single(motor_id),
|
||
data=data8,
|
||
is_extended_id=False,
|
||
)
|
||
bus.send(msg)
|
||
|
||
|
||
def motor_run(bus, motor_id: int):
|
||
# 电机运行命令：DATA[0]=0x88，其余 0
|
||
send_cmd(bus, motor_id, bytes([0x88, 0, 0, 0, 0, 0, 0, 0]))
|
||
|
||
|
||
def motor_stop(bus, motor_id: int):
|
||
# 电机停止命令：DATA[0]=0x81，其余 0
|
||
send_cmd(bus, motor_id, bytes([0x81, 0, 0, 0, 0, 0, 0, 0]))
|
||
|
||
|
||
def torque_iq_cmd_a1(bus, motor_id: int, iq_control: int):
|
||
"""
|
||
转矩闭环控制命令 0xA1：
|
||
DATA[0] = 0xA1
|
||
DATA[4..5] = iqControl (int16, little-endian), 范围 -2048~2048
|
||
其余字节 0
|
||
【协议：0xA1，iqControl:int16，DATA[4]=低字节，DATA[5]=高字节】
|
||
"""
|
||
if not (-2048 <= iq_control <= 2048):
|
||
raise ValueError("iq_control must be in [-2048, 2048]")
|
||
|
||
data = bytearray(8)
|
||
data[0] = 0xA1
|
||
data[4:6] = struct.pack("<h", int(iq_control)) # little-endian int16
|
||
send_cmd(bus, motor_id, bytes(data))
|
||
|
||
|
||
if __name__ == "__main__":
|
||
# 前提：你已在系统里把 can0 配置并拉起，例如：
|
||
# sudo ip link set can0 type can bitrate 1000000 restart-ms 100
|
||
# sudo ip link set can0 up
|
||
bus = can.Bus(interface="canalystii", channel=0, bitrate=1000000)
|
||
|
||
motor_id = 2
|
||
iq = 40 # 示例：给一个正向转矩电流指令（注意范围 -2048~2048）
|
||
|
||
try:
|
||
motor_run(bus, motor_id)
|
||
time.sleep(0.05)
|
||
|
||
# 建议周期性刷新（例如 100~500 Hz），并在“零力矩”时及时归零
|
||
dt = 0.01 # 100 Hz
|
||
while True:
|
||
torque_iq_cmd_a1(bus, 1, iq)
|
||
torque_iq_cmd_a1(bus, 2, iq)
|
||
torque_iq_cmd_a1(bus, 3, iq)
|
||
# torque_iq_cmd_a1(bus, 4, iq)
|
||
time.sleep(dt)
|
||
|
||
except KeyboardInterrupt:
|
||
# 松手/退出时，建议先发 0 力矩，再 stop
|
||
try:
|
||
torque_iq_cmd_a1(bus, 1, 0)
|
||
torque_iq_cmd_a1(bus, 2, 0)
|
||
torque_iq_cmd_a1(bus, 3, 0)
|
||
# torque_iq_cmd_a1(bus, 4, 0)
|
||
time.sleep(0.05)
|
||
except Exception:
|
||
pass
|
||
motor_stop(bus, motor_id)
|
||
bus.shutdown()
|