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】程序结束") |