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