#!/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_())