245 lines
8.1 KiB
Python
245 lines
8.1 KiB
Python
|
#!/usr/bin/env python
|
|||
|
|
# -*- coding: utf-8 -*-
|
|||
|
|
'''
|
|||
|
|
# @Time : 2025/08/04
|
|||
|
|
# @Author : hx
|
|||
|
|
# @File : 5dof_motor_test.py (优化版：切割控制由外部模块接管)
|
|||
|
|
'''
|
|||
|
|
|
|||
|
|
import time
|
|||
|
|
import numpy as np
|
|||
|
|
import matplotlib.pyplot as plt
|
|||
|
|
from matplotlib.animation import FuncAnimation
|
|||
|
|
|
|||
|
|
# 设置中文字体和解决负号显示问题
|
|||
|
|
plt.rcParams['font.sans-serif'] = ['SimHei', 'WenQuanYi Zen Hei', 'FangSong']
|
|||
|
|
plt.rcParams['axes.unicode_minus'] = False
|
|||
|
|
|
|||
|
|
# ------------------------ 调试开关 ------------------------
|
|||
|
|
DEBUG_MODE = True # <<< True: 只播动画；False: 控制真实电机
|
|||
|
|
|
|||
|
|
# 导入运动学和轨迹函数
|
|||
|
|
from calculate.ik import inverseF
|
|||
|
|
from calculate.trajectory import (
|
|||
|
|
line_trajectory
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
# -------------------- 只在非 Debug 模式导入 DM_CAN 和 轮趣模块 --------------------
|
|||
|
|
if not DEBUG_MODE:
|
|||
|
|
from DM_CAN.DM_CAN import Motor, MotorControl, DM_Motor_Type, Control_Type
|
|||
|
|
import serial
|
|||
|
|
|
|||
|
|
# 注意：不再导入 can_test_motor 或轮趣控制模块
|
|||
|
|
else:
|
|||
|
|
print("【DEBUG MODE】: 已启用调试模式，不连接硬件。")
|
|||
|
|
|
|||
|
|
# ------------------------ 机械臂参数 ------------------------
|
|||
|
|
L1 = 250
|
|||
|
|
L2 = 300
|
|||
|
|
L3 = 300
|
|||
|
|
L4 = 250
|
|||
|
|
L0 = 250 # 右侧基座偏移
|
|||
|
|
|
|||
|
|
# ------------------------ DM4310 配置 ------------------------
|
|||
|
|
MOTOR1_ID = 0x01
|
|||
|
|
MOTOR2_ID = 0x04
|
|||
|
|
CAN_SERIAL_PORT = '/dev/ttyACM0'
|
|||
|
|
BAUD_RATE = 921600
|
|||
|
|
KP = 5.0
|
|||
|
|
KD = 1.0
|
|||
|
|
|
|||
|
|
# ------------------------ 切割控制模式常量 ------------------------
|
|||
|
|
CUTTING_MODE = False # <<< 控制标志：False=不切割，True=触发切割（调用 qiege_motor.control()）
|
|||
|
|
CUTTING_AREA_MIN_Y = 280 # Y 值大于此开启切割（用于直线等轨迹）
|
|||
|
|
|
|||
|
|
# ------------------------ 全局变量 ------------------------
|
|||
|
|
motor1 = motor2 = motor_control = None
|
|||
|
|
current_theta1 = 0.0
|
|||
|
|
current_theta4 = 0.0
|
|||
|
|
x_list = y_list = [] # 全局轨迹点
|
|||
|
|
line = None # 动画线对象
|
|||
|
|
ani = None # 动画对象
|
|||
|
|
|
|||
|
|
|
|||
|
|
# ------------------------ 初始化电机 ------------------------
|
|||
|
|
def init_motors():
|
|||
|
|
global motor1, motor2, motor_control
|
|||
|
|
|
|||
|
|
if DEBUG_MODE:
|
|||
|
|
print("【DEBUG】跳过 DM4310 电机初始化")
|
|||
|
|
motor1 = motor2 = type('Motor', (), {'id': 0})()
|
|||
|
|
motor_control = type('MotorControl', (), {
|
|||
|
|
'enable': lambda x: True,
|
|||
|
|
'disable': lambda x: None,
|
|||
|
|
'controlMIT': lambda m, kp, kd, pos, vel, torq: None
|
|||
|
|
})()
|
|||
|
|
return motor1, motor2, motor_control
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
can_serial = serial.Serial(CAN_SERIAL_PORT, BAUD_RATE, timeout=0.5)
|
|||
|
|
print(f"CAN 串口 {CAN_SERIAL_PORT} 打开成功")
|
|||
|
|
except Exception as e:
|
|||
|
|
print(f"无法打开串口: {e}")
|
|||
|
|
exit(1)
|
|||
|
|
|
|||
|
|
motor_control = MotorControl(can_serial)
|
|||
|
|
motor1 = Motor(DM_Motor_Type.DM4310, MOTOR1_ID, 0x11)
|
|||
|
|
motor2 = Motor(DM_Motor_Type.DM4310, MOTOR2_ID, 0x14)
|
|||
|
|
|
|||
|
|
motor_control.addMotor(motor1)
|
|||
|
|
motor_control.addMotor(motor2)
|
|||
|
|
motor_control.switchControlMode(motor1, Control_Type.MIT)
|
|||
|
|
motor_control.switchControlMode(motor2, Control_Type.MIT)
|
|||
|
|
time.sleep(0.1)
|
|||
|
|
motor_control.save_motor_param(motor1)
|
|||
|
|
motor_control.save_motor_param(motor2)
|
|||
|
|
motor_control.enable(motor1)
|
|||
|
|
motor_control.enable(motor2)
|
|||
|
|
print("DM4310 电机已使能。")
|
|||
|
|
return motor1, motor2, motor_control
|
|||
|
|
|
|||
|
|
|
|||
|
|
# ------------------------ MIT 控制函数（DM4310）------------------------
|
|||
|
|
def control_two_motors_mit(theta1, theta4):
|
|||
|
|
global current_theta1, current_theta4
|
|||
|
|
|
|||
|
|
pos1 = np.degrees(theta1)
|
|||
|
|
pos4 = np.degrees(theta4)
|
|||
|
|
vel = 0.5 # 可根据轨迹速度调整
|
|||
|
|
|
|||
|
|
if not DEBUG_MODE:
|
|||
|
|
motor_control.controlMIT(motor1, KP, KD, pos1, vel, 0.0)
|
|||
|
|
motor_control.controlMIT(motor2, KP, KD, pos4, vel, 0.0)
|
|||
|
|
else:
|
|||
|
|
print(f"[DEBUG] DM4310 -> M1: {pos1:.2f}°, M2: {pos4:.2f}°")
|
|||
|
|
|
|||
|
|
current_theta1 = theta1
|
|||
|
|
current_theta4 = theta4
|
|||
|
|
|
|||
|
|
|
|||
|
|
# ------------------------ 动画帧绘制函数 ------------------------
|
|||
|
|
def draw_frame(i):
|
|||
|
|
global line, x_list, y_list, CUTTING_MODE
|
|||
|
|
|
|||
|
|
x = x_list[i]
|
|||
|
|
y = y_list[i]
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
theta1, theta4 = inverseF(x, y, L1, L2, L3, L4, L0)
|
|||
|
|
|
|||
|
|
# 左臂第一杆末端
|
|||
|
|
x2 = L1 * np.cos(theta1)
|
|||
|
|
y2 = L1 * np.sin(theta1)
|
|||
|
|
# 右臂第一杆末端
|
|||
|
|
x4 = L0 + L4 * np.cos(theta4)
|
|||
|
|
y4 = L4 * np.sin(theta4)
|
|||
|
|
|
|||
|
|
# 五连杆结构点
|
|||
|
|
x_coords = [0, x2, x, x4, L0]
|
|||
|
|
y_coords = [0, y2, y, y4, 0]
|
|||
|
|
line.set_data(x_coords, y_coords)
|
|||
|
|
|
|||
|
|
# --- 控制两个 DM4310 电机 ---
|
|||
|
|
control_two_motors_mit(theta1, theta4)
|
|||
|
|
|
|||
|
|
# --- 切割控制逻辑：判断是否进入切割区域 ---
|
|||
|
|
in_cutting_zone = (y > CUTTING_AREA_MIN_Y) # 例如直线轨迹
|
|||
|
|
|
|||
|
|
# ✅ 仅当进入切割区且当前未开启切割时，触发外部控制
|
|||
|
|
if in_cutting_zone and not CUTTING_MODE:
|
|||
|
|
print(f"[切割控制] 进入切割区 (Y={y:.1f})，触发 qiege_motor.control(True)")
|
|||
|
|
# 调用你后续集成的切割控制函数
|
|||
|
|
try:
|
|||
|
|
from qiege_motor import control
|
|||
|
|
control(True) # 启动切割
|
|||
|
|
except Exception as e:
|
|||
|
|
print(f"[错误] 调用 qiege_motor.control(True) 失败: {e}")
|
|||
|
|
CUTTING_MODE = True # 标记已开启切割
|
|||
|
|
|
|||
|
|
# ✅ 当离开切割区且当前正在切割时，可选择关闭（按需）
|
|||
|
|
elif not in_cutting_zone and CUTTING_MODE:
|
|||
|
|
print(f"[切割控制] 离开切割区 (Y={y:.1f})，可调用 control(False) 关闭")
|
|||
|
|
# 可选：自动关闭切割
|
|||
|
|
# try:
|
|||
|
|
# from qiege_motor import control
|
|||
|
|
# control(False)
|
|||
|
|
# except Exception as e:
|
|||
|
|
# print(f"[错误] 调用 qiege_motor.control(False) 失败: {e}")
|
|||
|
|
# CUTTING_MODE = False
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
print(f"第 {i} 帧逆解失败: {e}")
|
|||
|
|
line.set_data([], [])
|
|||
|
|
|
|||
|
|
return line,
|
|||
|
|
|
|||
|
|
|
|||
|
|
# ------------------------ 运行轨迹 + 动画 ------------------------
|
|||
|
|
def run_trajectory_with_animation(trajectory_func, **params):
|
|||
|
|
"""
|
|||
|
|
生成轨迹并播放动画，同时发送控制指令
|
|||
|
|
"""
|
|||
|
|
global x_list, y_list, line, ani
|
|||
|
|
print(f"生成轨迹: {trajectory_func.__name__}")
|
|||
|
|
x_list, y_list = trajectory_func(**params)
|
|||
|
|
print(f"轨迹点数: {len(x_list)}")
|
|||
|
|
|
|||
|
|
fig, ax = plt.subplots(figsize=(10, 8))
|
|||
|
|
ax.set_xlim(-200, L0 + 200)
|
|||
|
|
ax.set_ylim(0, 500)
|
|||
|
|
ax.set_aspect('equal')
|
|||
|
|
ax.grid(True, linestyle='--', alpha=0.6)
|
|||
|
|
ax.set_title(f"五连杆机械臂运动仿真 - 轨迹: {trajectory_func.__name__}", fontsize=14)
|
|||
|
|
|
|||
|
|
# 绘制目标轨迹（虚线）
|
|||
|
|
ax.plot(x_list, y_list, 'b--', alpha=0.5, label='目标轨迹')
|
|||
|
|
line, = ax.plot([], [], 'r-o', linewidth=3, markersize=8, label='机械臂结构')
|
|||
|
|
ax.legend()
|
|||
|
|
|
|||
|
|
ani = FuncAnimation(
|
|||
|
|
fig, draw_frame,
|
|||
|
|
frames=len(x_list),
|
|||
|
|
interval=50,
|
|||
|
|
repeat=False,
|
|||
|
|
blit=True
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
plt.show()
|
|||
|
|
print("动画结束。")
|
|||
|
|
|
|||
|
|
|
|||
|
|
# ------------------------ 主函数 ------------------------
|
|||
|
|
if __name__ == "__main__":
|
|||
|
|
# === 设置轨迹类型和参数 ===
|
|||
|
|
trajectory_config = {
|
|||
|
|
'func': line_trajectory,
|
|||
|
|
'params': {'start': (125, 250), 'end': (125, 400)}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
motor1, motor2, motor_control = init_motors()
|
|||
|
|
# 启动轨迹动画与控制
|
|||
|
|
run_trajectory_with_animation(trajectory_config['func'], **trajectory_config['params'])
|
|||
|
|
|
|||
|
|
except KeyboardInterrupt:
|
|||
|
|
print("\n【用户中断】")
|
|||
|
|
except Exception as e:
|
|||
|
|
print(f"程序异常: {e}")
|
|||
|
|
finally:
|
|||
|
|
# ✅ 程序结束时，确保关闭切割（如果已开启）
|
|||
|
|
if CUTTING_MODE:
|
|||
|
|
print("[切割控制] 程序结束，尝试关闭切割...")
|
|||
|
|
try:
|
|||
|
|
from qiege_motor import control
|
|||
|
|
control(False)
|
|||
|
|
except Exception as e:
|
|||
|
|
print(f"[错误] 调用 qiege_motor.control(False) 失败: {e}")
|
|||
|
|
CUTTING_MODE = False
|
|||
|
|
|
|||
|
|
# 停用 DM4310 电机
|
|||
|
|
if not DEBUG_MODE and motor_control is not None:
|
|||
|
|
motor_control.disable(motor1)
|
|||
|
|
motor_control.disable(motor2)
|
|||
|
|
print("所有电机已停机。")
|
|||
|
|
else:
|
|||
|
|
print("【DEBUG】程序结束")
|