mian.py测试完成的多轨迹运动控制代码
This commit is contained in:
琉璃月光
245
else/5dof_motor_test.py
Normal file
245
else/5dof_motor_test.py
Normal file
@ -0,0 +1,245 @@
|
||||
#!/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】程序结束")
|
||||
Reference in New Issue
Block a user