305 lines
10 KiB
Python
305 lines
10 KiB
Python
|
#!/usr/bin/env python
|
|||
|
|
# -*- coding: utf-8 -*-
|
|||
|
|
'''
|
|||
|
|
# @Time : 2025/08/04
|
|||
|
|
# @Author : hx
|
|||
|
|
# @File : qt_main.py
|
|||
|
|
# 功能：PyQt5 GUI + 五连杆轨迹动画 + 真实电机控制（DM4310 + 轮趣切割）
|
|||
|
|
# 修改：使用 qiege_motor.control() 统一控制切割电机
|
|||
|
|
'''
|
|||
|
|
|
|||
|
|
import sys
|
|||
|
|
import time
|
|||
|
|
import numpy as np
|
|||
|
|
import matplotlib
|
|||
|
|
matplotlib.use('Qt5Agg')
|
|||
|
|
from PyQt5.QtWidgets import (
|
|||
|
|
QApplication, QDialog, QVBoxLayout, QMessageBox
|
|||
|
|
)
|
|||
|
|
from PyQt5.QtGui import QStandardItemModel, QStandardItem
|
|||
|
|
from PyQt5.QtCore import Qt
|
|||
|
|
|
|||
|
|
from matplotlib.figure import Figure
|
|||
|
|
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
|
|||
|
|
from matplotlib.animation import FuncAnimation
|
|||
|
|
|
|||
|
|
# 导入你的 UI 和算法模块
|
|||
|
|
from untitled import Ui_Dialog
|
|||
|
|
from calculate.ik import inverseF
|
|||
|
|
from calculate.trajectory import circle_trajectory, line_trajectory
|
|||
|
|
|
|||
|
|
import matplotlib.pyplot as plt
|
|||
|
|
# 设置中文字体和解决负号显示问题
|
|||
|
|
plt.rcParams['font.sans-serif'] = ['SimHei', 'WenQuanYi Zen Hei', 'FangSong'] # 按优先级选择字体
|
|||
|
|
plt.rcParams['axes.unicode_minus'] = False # 显示负号 -
|
|||
|
|
|
|||
|
|
# ------------------------ 调试与模式开关 ------------------------
|
|||
|
|
DEBUG_MODE = True # <<< 设为 False 启用真实电机控制
|
|||
|
|
USE_QIEGE_MOTOR = False # <<< 设为 True 才启用 qiege_motor.py 控制切割电机
|
|||
|
|
|
|||
|
|
# ------------------------ 机械臂参数 ------------------------
|
|||
|
|
L1 = 250
|
|||
|
|
L2 = 300
|
|||
|
|
L3 = 300
|
|||
|
|
L4 = 250
|
|||
|
|
L0 = 250 # 右侧基座偏移
|
|||
|
|
|
|||
|
|
# ------------------------ 电机配置 ------------------------
|
|||
|
|
CAN_PORT = '/dev/ttyACM0'
|
|||
|
|
CAN_BAUD = 921600
|
|||
|
|
|
|||
|
|
DM_MOTOR1_ID = 0x01 # 左臂 DM4310
|
|||
|
|
DM_MOTOR2_ID = 0x04 # 右臂 DM4310
|
|||
|
|
|
|||
|
|
KP = 5.0
|
|||
|
|
KD = 1.0
|
|||
|
|
|
|||
|
|
# ------------------------ 切割参数 ------------------------
|
|||
|
|
CUTTING_MIN_Y = 280 # Y > 此值才启动切割
|
|||
|
|
CUT_SPEED_RAD = 10.0 # 切割转速 (rad/s)，用于 qiege_motor 内部映射
|
|||
|
|
|
|||
|
|
# ------------------------ 全局变量 ------------------------
|
|||
|
|
motor1 = motor2 = motor_control = None
|
|||
|
|
current_cutting_state = False # 避免重复启停切割电机
|
|||
|
|
|
|||
|
|
|
|||
|
|
# -------------------- 导入硬件模块（仅非 DEBUG 模式） --------------------
|
|||
|
|
if not DEBUG_MODE:
|
|||
|
|
try:
|
|||
|
|
from DM_CAN.DM_CAN import Motor, MotorControl, DM_Motor_Type, Control_Type
|
|||
|
|
import serial
|
|||
|
|
print("【硬件模块导入成功】: DM4310 电机")
|
|||
|
|
except Exception as e:
|
|||
|
|
print(f"【硬件模块导入失败】: {e}")
|
|||
|
|
sys.exit(1)
|
|||
|
|
else:
|
|||
|
|
print("【DEBUG MODE】: 不连接真实电机")
|
|||
|
|
|
|||
|
|
|
|||
|
|
# -------------------- 导入切割电机模块（仅当启用时） --------------------
|
|||
|
|
if USE_QIEGE_MOTOR and not DEBUG_MODE:
|
|||
|
|
try:
|
|||
|
|
from qiege_motor import control as cutting_control
|
|||
|
|
print("【切割电机模块导入成功】: qiege_motor.control")
|
|||
|
|
except Exception as e:
|
|||
|
|
print(f"【qiege_motor 导入失败】: {e}")
|
|||
|
|
print("【警告】将使用模拟模式")
|
|||
|
|
USE_QIEGE_MOTOR = False
|
|||
|
|
else:
|
|||
|
|
def cutting_control(enable: bool):
|
|||
|
|
"""模拟切割电机控制"""
|
|||
|
|
state = "启动" if enable else "停止"
|
|||
|
|
if DEBUG_MODE:
|
|||
|
|
print(f"[DEBUG] 切割电机: {state}")
|
|||
|
|
else:
|
|||
|
|
if USE_QIEGE_MOTOR:
|
|||
|
|
print(f"[WARN] qiege_motor 未启用或导入失败，control({enable}) 被忽略")
|
|||
|
|
|
|||
|
|
|
|||
|
|
class MyDialog(QDialog, Ui_Dialog):
|
|||
|
|
def __init__(self):
|
|||
|
|
super().__init__()
|
|||
|
|
self.setupUi(self)
|
|||
|
|
|
|||
|
|
# 初始化 listView 模型
|
|||
|
|
self.angle_model = QStandardItemModel(self.listView)
|
|||
|
|
self.listView.setModel(self.angle_model)
|
|||
|
|
|
|||
|
|
# Matplotlib 图形初始化
|
|||
|
|
self.figure = Figure(figsize=(8, 6), dpi=100)
|
|||
|
|
self.ax = self.figure.add_subplot(111)
|
|||
|
|
self.ax.set_xlim(-300, 500)
|
|||
|
|
self.ax.set_ylim(0, 500)
|
|||
|
|
self.ax.set_aspect('equal')
|
|||
|
|
self.ax.grid(True, linestyle='--', alpha=0.6)
|
|||
|
|
self.ax.set_title("五连杆机械臂运动控制")
|
|||
|
|
|
|||
|
|
# 画布
|
|||
|
|
self.canvas = FigureCanvas(self.figure)
|
|||
|
|
self.widget.setLayout(QVBoxLayout())
|
|||
|
|
self.widget.layout().addWidget(self.canvas)
|
|||
|
|
|
|||
|
|
# 机械臂线
|
|||
|
|
self.line, = self.ax.plot([], [], 'r-o', linewidth=2, markersize=6)
|
|||
|
|
|
|||
|
|
# 轨迹数据
|
|||
|
|
self.x_list = []
|
|||
|
|
self.y_list = []
|
|||
|
|
self.ani = None
|
|||
|
|
|
|||
|
|
# 初始化电机（仅非 DEBUG）
|
|||
|
|
self.init_hardware()
|
|||
|
|
|
|||
|
|
# 连接按钮
|
|||
|
|
self.pushButton.clicked.connect(self.start_line_trajectory)
|
|||
|
|
self.pushButton_2.clicked.connect(self.start_circle_trajectory)
|
|||
|
|
|
|||
|
|
def init_hardware(self):
|
|||
|
|
global motor1, motor2, motor_control
|
|||
|
|
|
|||
|
|
if DEBUG_MODE:
|
|||
|
|
print("【DEBUG】跳过 DM4310 电机初始化")
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
ser = serial.Serial(CAN_PORT, CAN_BAUD, timeout=0.5)
|
|||
|
|
motor_control = MotorControl(ser)
|
|||
|
|
motor1 = Motor(DM_Motor_Type.DM4310, DM_MOTOR1_ID, 0x11)
|
|||
|
|
motor2 = Motor(DM_Motor_Type.DM4310, DM_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.enable(motor1)
|
|||
|
|
motor_control.enable(motor2)
|
|||
|
|
print("✅ DM4310 电机已使能")
|
|||
|
|
except Exception as e:
|
|||
|
|
QMessageBox.critical(self, "电机初始化失败", str(e))
|
|||
|
|
print(f"❌ 电机初始化失败: {e}")
|
|||
|
|
|
|||
|
|
def control_cutting_motor(self, enable=True):
|
|||
|
|
"""
|
|||
|
|
控制切割电机
|
|||
|
|
- 仅在状态变化时调用
|
|||
|
|
- 使用 qiege_motor.control() 接口
|
|||
|
|
"""
|
|||
|
|
global current_cutting_state
|
|||
|
|
|
|||
|
|
if enable == current_cutting_state:
|
|||
|
|
return # 状态未变，不重复操作
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
cutting_control(enable)
|
|||
|
|
print(f"✅ 切割电机: {'启动' if enable else '停止'}")
|
|||
|
|
except Exception as e:
|
|||
|
|
print(f"❌ 调用切割电机 control({enable}) 失败: {e}")
|
|||
|
|
|
|||
|
|
current_cutting_state = enable
|
|||
|
|
|
|||
|
|
def control_dm_motors(self, theta1, theta4):
|
|||
|
|
"""控制两个 DM4310 电机"""
|
|||
|
|
pos1 = np.degrees(theta1)
|
|||
|
|
pos4 = np.degrees(theta4)
|
|||
|
|
vel = 0.8 # 可根据需要调整
|
|||
|
|
|
|||
|
|
if DEBUG_MODE:
|
|||
|
|
print(f"[DEBUG] DM4310 -> M1:{pos1:.1f}°, M2:{pos4:.1f}°")
|
|||
|
|
else:
|
|||
|
|
try:
|
|||
|
|
motor_control.controlMIT(motor1, KP, KD, pos1, vel, 0.0)
|
|||
|
|
motor_control.controlMIT(motor2, KP, KD, pos4, vel, 0.0)
|
|||
|
|
except Exception as e:
|
|||
|
|
print(f"[DM电机] 控制失败: {e}")
|
|||
|
|
|
|||
|
|
def start_line_trajectory(self):
|
|||
|
|
self.angle_model.clear()
|
|||
|
|
self.ax.set_title("直线轨迹运动 + 电机控制")
|
|||
|
|
self.x_list, self.y_list = line_trajectory(start=(125, 300), end=(125, 400))
|
|||
|
|
self.start_animation()
|
|||
|
|
|
|||
|
|
def start_circle_trajectory(self):
|
|||
|
|
self.angle_model.clear()
|
|||
|
|
self.ax.set_title("圆形轨迹运动 + 电机控制")
|
|||
|
|
self.x_list, self.y_list = circle_trajectory(center=(100, 200), radius=40)
|
|||
|
|
self.start_animation()
|
|||
|
|
|
|||
|
|
def start_animation(self):
|
|||
|
|
# 安全停止旧动画
|
|||
|
|
try:
|
|||
|
|
if self.ani is not None and self.ani.event_source is not None:
|
|||
|
|
self.ani.event_source.stop()
|
|||
|
|
except:
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
# ✅ 开始前：重置切割状态
|
|||
|
|
global current_cutting_state
|
|||
|
|
current_cutting_state = False
|
|||
|
|
|
|||
|
|
def animate_frame(i):
|
|||
|
|
x = self.x_list[i]
|
|||
|
|
y = self.y_list[i]
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
theta1, theta4 = inverseF(x, y, L1, L2, L3, L4, L0)
|
|||
|
|
|
|||
|
|
# 显示角度（角度制）
|
|||
|
|
theta1_deg = np.degrees(theta1)
|
|||
|
|
theta4_deg = np.degrees(theta4)
|
|||
|
|
item_text = f"点 {i}: θ1={theta1_deg:.1f}°, θ4={theta4_deg:.1f}°"
|
|||
|
|
if self.angle_model.rowCount() >= 10:
|
|||
|
|
self.angle_model.removeRow(0)
|
|||
|
|
self.angle_model.appendRow(QStandardItem(item_text))
|
|||
|
|
|
|||
|
|
# 计算五连杆坐标
|
|||
|
|
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]
|
|||
|
|
self.line.set_data(x_coords, y_coords)
|
|||
|
|
|
|||
|
|
# --- 控制真实电机 ---
|
|||
|
|
self.control_dm_motors(theta1, theta4)
|
|||
|
|
|
|||
|
|
# --- 切割控制（进入区域才切）---
|
|||
|
|
in_cut_zone = y > CUTTING_MIN_Y
|
|||
|
|
self.control_cutting_motor(enable=in_cut_zone)
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
print(f"第 {i} 帧错误: {e}")
|
|||
|
|
return self.line,
|
|||
|
|
|
|||
|
|
# 创建动画
|
|||
|
|
self.ani = FuncAnimation(
|
|||
|
|
self.figure,
|
|||
|
|
animate_frame,
|
|||
|
|
frames=len(self.x_list),
|
|||
|
|
interval=100,
|
|||
|
|
repeat=False,
|
|||
|
|
blit=False
|
|||
|
|
)
|
|||
|
|
self.canvas.draw()
|
|||
|
|
|
|||
|
|
# ✅ 动画结束后自动停止所有电机
|
|||
|
|
from PyQt5.QtCore import QTimer
|
|||
|
|
timer = QTimer(self)
|
|||
|
|
timer.setSingleShot(True)
|
|||
|
|
timer.timeout.connect(self.on_animation_finish)
|
|||
|
|
timer.start(int(len(self.x_list) * 100 + 500)) # 动画总时长 + 0.5s 延迟
|
|||
|
|
|
|||
|
|
def on_animation_finish(self):
|
|||
|
|
"""动画结束后安全关闭所有电机"""
|
|||
|
|
if not DEBUG_MODE:
|
|||
|
|
try:
|
|||
|
|
if 'motor_control' in globals() and motor_control:
|
|||
|
|
motor_control.disable(motor1)
|
|||
|
|
motor_control.disable(motor2)
|
|||
|
|
print("✅ DM4310 电机已停用")
|
|||
|
|
except Exception as e:
|
|||
|
|
print(f"❌ DM 电机停用失败: {e}")
|
|||
|
|
|
|||
|
|
self.control_cutting_motor(False)
|
|||
|
|
print("✅ 所有电机已安全关闭")
|
|||
|
|
|
|||
|
|
def closeEvent(self, event):
|
|||
|
|
"""窗口关闭时确保电机停止"""
|
|||
|
|
self.control_cutting_motor(False)
|
|||
|
|
if not DEBUG_MODE and 'motor_control' in globals():
|
|||
|
|
try:
|
|||
|
|
if motor_control:
|
|||
|
|
motor_control.disable(motor1)
|
|||
|
|
motor_control.disable(motor2)
|
|||
|
|
print("【退出】所有电机已停用")
|
|||
|
|
except Exception as e:
|
|||
|
|
print(f"【退出】电机停用异常: {e}")
|
|||
|
|
event.accept()
|
|||
|
|
|
|||
|
|
|
|||
|
|
if __name__ == "__main__":
|
|||
|
|
app = QApplication(sys.argv)
|
|||
|
|
dialog = MyDialog()
|
|||
|
|
dialog.show()
|
|||
|
|
sys.exit(app.exec_())
|