183 lines
5.4 KiB
Python
183 lines
5.4 KiB
Python
# qiege_motor.py
|
||
# 切割电机控制模块（支持 CAN 和 USB 两种通信方式）
|
||
# 使用 control(enable) 统一接口
|
||
|
||
import time
|
||
import serial
|
||
|
||
# ------------------------ 配置区 ------------------------
|
||
|
||
# <<< 切换通信模式 >>>
|
||
MODE = 'usb' # 'can' 或 'usb'
|
||
|
||
# CAN 模式配置
|
||
CAN_SERIAL_PORT = '/dev/ttyACM0' # CAN 转串口设备
|
||
CAN_BAUDRATE = 921600
|
||
CAN_MOTOR_ID = 0x05 # 轮趣电机对应的 CAN ID
|
||
|
||
# USB 串口模式配置（轮趣 HEX 指令）
|
||
USB_SERIAL_PORT = '/dev/ttyACM0' # 或 '/dev/ttyUSB0'
|
||
USB_BAUDRATE = 115200
|
||
|
||
# 目标转速（rad/s）
|
||
CUTTING_SPEED_RAD_PER_SEC = 8.0 # 对应 HEX 指令中的 speed=8 → 80 (0x50)
|
||
|
||
# 是否启用调试输出
|
||
DEBUG = True
|
||
|
||
# ------------------------ 全局变量 ------------------------
|
||
usb_serial = None
|
||
can_serial = None
|
||
|
||
|
||
# ------------------------ USB 模式：HEX 指令生成与发送 ------------------------
|
||
def generate_usb_command(speed):
|
||
"""
|
||
生成轮趣电机 USB 串口 HEX 指令
|
||
格式: 7B 01 01 00 20 00 00 00 <speed_byte> <checksum> 7D
|
||
speed: float (rad/s), 映射为 0~255
|
||
"""
|
||
# 将 rad/s 映射为速度字节（示例：8 rad/s → 80）
|
||
# 你可以根据实际响应调整映射关系
|
||
speed_val = int(speed * 10) # 示例：8 → 80
|
||
speed_byte = max(0, min(255, speed_val))
|
||
|
||
# 构造前9字节
|
||
cmd = [
|
||
0x7B, 0x01, 0x02, # 帧头、地址、模式
|
||
0x01, 0x20, # 方向、细分
|
||
0x0E, 0x10, # 位置高、低（持续旋转）
|
||
0x00, 0x64, # 速度值
|
||
]
|
||
|
||
# 计算校验和（前9字节异或）
|
||
checksum = 0
|
||
for b in cmd:
|
||
checksum ^= b
|
||
cmd.append(checksum)
|
||
cmd.append(0x7D) # 帧尾
|
||
|
||
return bytes(cmd)
|
||
|
||
|
||
def send_usb_command(port, baudrate, command):
|
||
"""发送 USB 串口指令"""
|
||
global usb_serial
|
||
try:
|
||
if usb_serial is None or not usb_serial.is_open:
|
||
usb_serial = serial.Serial(port, baudrate, timeout=0.5)
|
||
time.sleep(0.5) # 等待串口稳定
|
||
|
||
usb_serial.write(command)
|
||
if DEBUG:
|
||
print(f"[qiege_motor] USB 发送: {command.hex().upper()}")
|
||
return True
|
||
except Exception as e:
|
||
if DEBUG:
|
||
print(f"[qiege_motor] USB 发送失败: {e}")
|
||
usb_serial = None
|
||
return False
|
||
|
||
|
||
# ------------------------ CAN 模式：CAN 指令发送 ------------------------
|
||
def send_can_command(data, can_id=CAN_MOTOR_ID):
|
||
"""通过 CAN 发送指令（假设使用串口转 CAN 模块）"""
|
||
global can_serial
|
||
try:
|
||
if can_serial is None or not can_serial.is_open:
|
||
can_serial = serial.Serial(CAN_SERIAL_PORT, CAN_BAUDRATE, timeout=0.5)
|
||
time.sleep(0.5)
|
||
|
||
# 示例：简单封装 CAN 帧（ID + DLC + Data）
|
||
# 格式根据你使用的 CAN 模块调整（如 CANable、TJA1050 等）
|
||
can_frame = [
|
||
(can_id >> 24) & 0xFF,
|
||
(can_id >> 16) & 0xFF,
|
||
(can_id >> 8) & 0xFF,
|
||
can_id & 0xFF,
|
||
len(data) # DLC
|
||
] + list(data)
|
||
can_serial.write(bytes(can_frame))
|
||
|
||
if DEBUG:
|
||
print(f"[qiege_motor] CAN 发送 → ID:{can_id:05X}, Data:{[f'{b:02X}' for b in data]}")
|
||
return True
|
||
except Exception as e:
|
||
if DEBUG:
|
||
print(f"[qiege_motor] CAN 发送失败: {e}")
|
||
can_serial = None
|
||
return False
|
||
|
||
|
||
def generate_can_command(speed_rpm=300, direction=1, microstep=32):
|
||
"""生成轮趣 CAN 速度指令（示例）"""
|
||
speed_01rpm = int(abs(speed_rpm) * 10)
|
||
high_byte = (speed_01rpm >> 8) & 0xFF
|
||
low_byte = speed_01rpm & 0xFF
|
||
direction_bit = 1 if direction else 0
|
||
|
||
data = [
|
||
0x88, # 控制字：速度模式
|
||
((direction_bit & 0x01) << 7) | (microstep & 0x7F),
|
||
0x00, 0x00,
|
||
high_byte, low_byte,
|
||
0x00, 0x00
|
||
]
|
||
return data
|
||
|
||
|
||
# ------------------------ 统一控制接口 ------------------------
|
||
def start_motor():
|
||
"""启动切割电机"""
|
||
if MODE == 'usb':
|
||
cmd = generate_usb_command(CUTTING_SPEED_RAD_PER_SEC)
|
||
return send_usb_command(USB_SERIAL_PORT, USB_BAUDRATE, cmd)
|
||
elif MODE == 'can':
|
||
# 示例：8 rad/s ≈ 76.4 RPM
|
||
rpm = CUTTING_SPEED_RAD_PER_SEC * 60 / (2 * 3.1416)
|
||
cmd = generate_can_command(speed_rpm=rpm, direction=1)
|
||
return send_can_command(cmd, can_id=CAN_MOTOR_ID)
|
||
else:
|
||
print(f"[qiege_motor] 错误：不支持的模式 '{MODE}'")
|
||
return False
|
||
|
||
|
||
def stop_motor():
|
||
"""停止切割电机"""
|
||
if MODE == 'usb':
|
||
cmd = generate_usb_command(0.0)
|
||
return send_usb_command(USB_SERIAL_PORT, USB_BAUDRATE, cmd)
|
||
elif MODE == 'can':
|
||
cmd = generate_can_command(speed_rpm=0, direction=1)
|
||
return send_can_command(cmd, can_id=CAN_MOTOR_ID)
|
||
else:
|
||
return False
|
||
|
||
|
||
def control(enable: bool):
|
||
"""
|
||
统一控制接口
|
||
:param enable: True 启动, False 停止
|
||
:return: bool 成功与否
|
||
"""
|
||
try:
|
||
if enable:
|
||
return start_motor()
|
||
else:
|
||
return stop_motor()
|
||
except Exception as e:
|
||
if DEBUG:
|
||
print(f"[qiege_motor] 控制异常: {e}")
|
||
return False
|
||
|
||
|
||
# ------------------------ 测试 ------------------------
|
||
if __name__ == "__main__":
|
||
print(f"【qiege_motor】测试开始 (模式: {MODE})")
|
||
print("启动 → 1秒后停止")
|
||
|
||
control(True)
|
||
time.sleep(1)
|
||
control(False)
|
||
|
||
print("测试结束。") |