mian.py测试完成的多轨迹运动控制代码
This commit is contained in:
琉璃月光
226
else/5dof_motor_main.py
Normal file
226
else/5dof_motor_main.py
Normal file
@ -0,0 +1,226 @@
|
||||
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 (
|
||||
circle_trajectory,
|
||||
line_trajectory,
|
||||
ellipse_trajectory,
|
||||
square_trajectory,
|
||||
triangle_trajectory
|
||||
)
|
||||
|
||||
# -------------------- 只在非 Debug 模式导入 DM_CAN --------------------
|
||||
if not DEBUG_MODE:
|
||||
from DM_CAN.DM_CAN import Motor, MotorControl, DM_Motor_Type, Control_Type, DM_variable
|
||||
import serial
|
||||
from can_test_motor import motor_control_can_all
|
||||
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
|
||||
|
||||
# ------------------------ 轮趣科技电机配置 ------------------------
|
||||
MOTOR3_ID = 0x05
|
||||
|
||||
# 全局变量
|
||||
motor1 = motor2 = motor3 = 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】跳过电机初始化")
|
||||
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, 0x15)
|
||||
|
||||
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("电机已使能。")
|
||||
return motor1, motor2, motor_control
|
||||
|
||||
|
||||
# ------------------------ MIT 控制函数 ------------------------
|
||||
def control_two_motors_mit(theta1, theta4):
|
||||
global current_theta1, current_theta4
|
||||
|
||||
pos1 = np.degrees(theta1)
|
||||
pos4 = np.degrees(theta4)
|
||||
vel = 0.0
|
||||
|
||||
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] 模拟控制 -> Motor1: {pos1:.2f}°, Motor2: {pos4:.2f}°")
|
||||
|
||||
current_theta1 = theta1
|
||||
current_theta4 = theta4
|
||||
|
||||
|
||||
# ------------------------ 动画帧绘制函数 ------------------------
|
||||
def draw_frame(i):
|
||||
global line, x_list, y_list
|
||||
|
||||
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)
|
||||
# 右臂第一杆末端（注意 L0 偏移）
|
||||
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)
|
||||
|
||||
# 发送控制指令（在循环中同步）
|
||||
control_two_motors_mit(theta1, theta4)
|
||||
|
||||
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) # 根据 L0 调整
|
||||
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, # 每帧间隔（ms）
|
||||
repeat=False, #循环播放开关
|
||||
blit=True
|
||||
)
|
||||
|
||||
plt.show() # <<< 阻塞显示，确保窗口不黑不闪退
|
||||
print("动画结束。")
|
||||
|
||||
|
||||
# ------------------------ 主函数 ------------------------
|
||||
if __name__ == "__main__":
|
||||
# === 在这里设置轨迹类型和参数 ===
|
||||
|
||||
'''trajectory_config = {
|
||||
'func': ellipse_trajectory,
|
||||
'params': {
|
||||
'center': (100, 200),
|
||||
'rx': 50,
|
||||
'ry': 25,
|
||||
'num_points': 100
|
||||
}
|
||||
}'''
|
||||
|
||||
|
||||
# 可选其他轨迹：
|
||||
trajectory_config = {
|
||||
'func': line_trajectory,
|
||||
'params': {'start': (125, 300), 'end': (275, 300)}
|
||||
}
|
||||
#trajectory_config = {
|
||||
# 'func': circle_trajectory,
|
||||
# 'params': {'center': (100, 300), 'radius': 40}
|
||||
# }
|
||||
|
||||
# 初始化电机
|
||||
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 not DEBUG_MODE:
|
||||
motor_control.disable(motor1)
|
||||
motor_control.disable(motor2)
|
||||
print("电机已停机。")
|
||||
else:
|
||||
print("【DEBUG】程序结束")
|
||||
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】程序结束")
|
||||
80
else/5dof_test_ctrl_motor.py
Normal file
80
else/5dof_test_ctrl_motor.py
Normal file
@ -0,0 +1,80 @@
|
||||
import numpy as np
|
||||
from calculate.fk import forwardF
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
# 设置中文字体和解决负号显示问题
|
||||
plt.rcParams['font.sans-serif'] = ['SimHei', 'WenQuanYi Zen Hei', 'FangSong'] # 按优先级选择字体
|
||||
plt.rcParams['axes.unicode_minus'] = False # 显示负号 -
|
||||
|
||||
# ========== 主程序开始 ==========
|
||||
# 输入参数
|
||||
l1 = 250
|
||||
l2 = 300
|
||||
l3 = 300
|
||||
l4 = 250
|
||||
l5 = 250
|
||||
|
||||
hd = np.pi / 180
|
||||
u1 = 120 * hd
|
||||
u4 = 60 * hd
|
||||
|
||||
omega1 = 500
|
||||
omega4 = 500
|
||||
alpha1 = 0
|
||||
alpha4 = 0
|
||||
|
||||
# 计算正向运动学
|
||||
xc, yc, u2, u3, omega2, omega3, alpha2, alpha3 = forwardF(u1, u4, omega1, omega4, l1, l2, l3, l4, l5, alpha1, alpha4)
|
||||
|
||||
# 存储中间变量
|
||||
theta2 = [u2]
|
||||
theta3 = [u3]
|
||||
omega2_list = [omega2]
|
||||
omega3_list = [omega3]
|
||||
alpha2_list = [alpha2]
|
||||
alpha3_list = [alpha3]
|
||||
xcd = [xc]
|
||||
ycd = [yc]
|
||||
|
||||
# 绘图部分
|
||||
fig = plt.figure(figsize=(8, 8))
|
||||
ax = fig.add_subplot(111)
|
||||
ax.set_title('并联SCARA')
|
||||
ax.set_xlabel('mm')
|
||||
ax.set_ylabel('mm')
|
||||
ax.set_xlim(-200, 600)
|
||||
ax.set_ylim(-200, 600)
|
||||
ax.grid(True)
|
||||
|
||||
x = [0] * 5
|
||||
y = [0] * 5
|
||||
|
||||
# 基础点
|
||||
x[0] = 0
|
||||
y[0] = 0
|
||||
|
||||
# 第一个连杆末端
|
||||
x[1] = l1 * np.cos(u1)
|
||||
y[1] = l1 * np.sin(u1)
|
||||
|
||||
# 末端执行器位置
|
||||
x[2] = xcd[0]
|
||||
y[2] = ycd[0]
|
||||
|
||||
# 第四个连杆末端
|
||||
x[3] = l4 * np.cos(u4) + l5
|
||||
y[3] = l4 * np.sin(u4)
|
||||
|
||||
# 第五个点（第二个电机位置）
|
||||
x[4] = l5
|
||||
y[4] = 0
|
||||
|
||||
# 绘制结构线和关键点
|
||||
ax.plot(x, y, 'k-', linewidth=2)
|
||||
ax.plot(x[0], y[0], 'or') # 基础点
|
||||
ax.plot(x[1], y[1], 'or') # 第一连杆末端
|
||||
ax.plot(x[2], y[2], 'og') # 末端执行器
|
||||
ax.plot(x[3], y[3], 'om') # 第四连杆末端
|
||||
ax.plot(x[4], y[4], 'oc') # 第二个电机
|
||||
|
||||
plt.show()
|
||||
187
else/main_limit.py
Normal file
187
else/main_limit.py
Normal file
@ -0,0 +1,187 @@
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from matplotlib.widgets import Slider
|
||||
import matplotlib.pyplot as plt
|
||||
import matplotlib
|
||||
|
||||
# 显式设置中文字体和正常显示负号
|
||||
matplotlib.rcParams['font.sans-serif'] = ['WenQuanYi Micro Hei']
|
||||
matplotlib.rcParams['axes.unicode_minus'] = False # 解决负号 '-' 显示为方块的问题
|
||||
|
||||
# 测试绘图
|
||||
plt.figure(figsize=(6, 4))
|
||||
plt.text(0.5, 0.5, '你好，世界！\nThis is 文泉驿微米黑', fontsize=16, ha='center')
|
||||
plt.axis('off')
|
||||
plt.title("中文测试")
|
||||
plt.show()
|
||||
|
||||
# ======================= 对称五自由度机器人模型 =======================
|
||||
class Symmetric5DoFRobot:
|
||||
def __init__(self):
|
||||
self.L1 = 0.5 # 臂段长度
|
||||
self.L2 = 0.4
|
||||
self.Y0 = 0.6 # 左右臂起始点在 Y 轴上的偏移
|
||||
self.tcp_x = 0.8 # 默认 TCP 目标位置
|
||||
self.tcp_y = 0.0
|
||||
self.tcp_theta = 0 # 夹具方向角度（弧度）
|
||||
|
||||
# 关节限位 [min, max]（单位：弧度）
|
||||
self.joint_limits = {
|
||||
'theta1': [-np.pi, np.pi], # 左肩
|
||||
'theta2': [-np.pi, np.pi], # 左肘
|
||||
'theta3': [-np.pi, np.pi], # 右肘
|
||||
'theta4': [-np.pi, np.pi], # 右肩
|
||||
'theta5': [-np.pi, np.pi] # 夹具方向
|
||||
}
|
||||
|
||||
# 初始角度
|
||||
self.joint_angles = [0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
|
||||
def inverse_kinematics(self, x, y, theta):
|
||||
"""给定末端位置和方向，返回左右臂各关节角度"""
|
||||
try:
|
||||
cos_q2_left = (x**2 + y**2 - self.L1**2 - self.L2**2) / (2 * self.L1 * self.L2)
|
||||
sin_q2_left = np.sqrt(1 - cos_q2_left**2)
|
||||
q2_left = np.arctan2(sin_q2_left, cos_q2_left)
|
||||
|
||||
k1 = self.L1 + self.L2 * np.cos(q2_left)
|
||||
k2 = self.L2 * np.sin(q2_left)
|
||||
q1_left = np.arctan2(y, x) - np.arctan2(k2, k1)
|
||||
|
||||
# 限制在合理范围内
|
||||
q1_left = np.clip(q1_left, *self.joint_limits['theta1'])
|
||||
q2_left = np.clip(q2_left, *self.joint_limits['theta2'])
|
||||
|
||||
except:
|
||||
q1_left = 0
|
||||
q2_left = 0
|
||||
|
||||
# 右臂镜像求解
|
||||
try:
|
||||
cos_q2_right = (x**2 + y**2 - self.L1**2 - self.L2**2) / (2 * self.L1 * self.L2)
|
||||
sin_q2_right = -np.sqrt(1 - cos_q2_right**2)
|
||||
q2_right = np.arctan2(sin_q2_right, cos_q2_right)
|
||||
|
||||
k1 = self.L1 + self.L2 * np.cos(q2_right)
|
||||
k2 = self.L2 * np.sin(q2_right)
|
||||
q1_right = np.arctan2(y, x) - np.arctan2(k2, k1)
|
||||
|
||||
q1_right = np.clip(q1_right, *self.joint_limits['theta4'])
|
||||
q2_right = np.clip(q2_right, *self.joint_limits['theta3'])
|
||||
|
||||
except:
|
||||
q1_right = 0
|
||||
q2_right = 0
|
||||
|
||||
self.joint_angles = [q1_left, q2_left, q2_right, q1_right, theta]
|
||||
return {
|
||||
'left': [q1_left, q2_left],
|
||||
'right': [q1_right, q2_right],
|
||||
'theta': theta
|
||||
}
|
||||
|
||||
def forward_kinematics(self):
|
||||
ik = self.inverse_kinematics(self.tcp_x, self.tcp_y, self.tcp_theta)
|
||||
θ1, θ2 = ik['left']
|
||||
θ4, θ3 = ik['right']
|
||||
|
||||
# 左臂坐标计算
|
||||
left_base = np.array([0, -self.Y0])
|
||||
j1_left = left_base + np.array([self.L1 * np.cos(θ1), self.L1 * np.sin(θ1)])
|
||||
tcp_left = j1_left + np.array([
|
||||
self.L2 * np.cos(θ1 + θ2),
|
||||
self.L2 * np.sin(θ1 + θ2)
|
||||
])
|
||||
|
||||
# 右臂坐标计算
|
||||
right_base = np.array([0, self.Y0])
|
||||
j1_right = right_base + np.array([self.L1 * np.cos(θ4), self.L1 * np.sin(θ4)])
|
||||
tcp_right = j1_right + np.array([
|
||||
self.L2 * np.cos(θ4 + θ3),
|
||||
self.L2 * np.sin(θ4 + θ3)
|
||||
])
|
||||
|
||||
# 确保末端相连
|
||||
tcp_point = (tcp_left + tcp_right) / 2
|
||||
gripper_dir = np.array([np.cos(self.tcp_theta), np.sin(self.tcp_theta)]) * 0.3
|
||||
|
||||
return {
|
||||
'left_arm': np.array([left_base, j1_left, tcp_point]),
|
||||
'right_arm': np.array([right_base, j1_right, tcp_point]),
|
||||
'gripper': [tcp_point, tcp_point + gripper_dir]
|
||||
}
|
||||
|
||||
# ======================= GUI界面 =======================
|
||||
class Symmetric5DoF_GUI:
|
||||
def __init__(self):
|
||||
self.robot = Symmetric5DoFRobot()
|
||||
|
||||
self.fig, self.ax = plt.subplots(figsize=(8, 6))
|
||||
plt.subplots_adjust(left=0.1, right=0.9, top=0.85, bottom=0.35)
|
||||
self.ax.set_title("非共基座对称五自由度机器人（带关节限位 + 自由度显示）")
|
||||
self.ax.set_xlim(-1.5, 1.5)
|
||||
self.ax.set_ylim(-1.5, 1.5)
|
||||
self.ax.set_aspect('equal')
|
||||
|
||||
# 初始化线条
|
||||
self.left_line, = self.ax.plot([], [], 'b-o', lw=2, markersize=6, label="左臂")
|
||||
self.right_line, = self.ax.plot([], [], 'r-o', lw=2, markersize=6, label="右臂")
|
||||
self.gripper_line, = self.ax.plot([], [], 'g-->', lw=2, markersize=4, label="夹具方向")
|
||||
|
||||
plt.legend()
|
||||
|
||||
# 创建滑动条
|
||||
self.sliders = []
|
||||
self.slider_labels = ['X', 'Y', 'Theta']
|
||||
self.slider_values = [self.robot.tcp_x, self.robot.tcp_y, self.robot.tcp_theta]
|
||||
|
||||
y_pos = 0.25
|
||||
for i in range(3):
|
||||
ax_slider = plt.axes([0.2, y_pos, 0.6, 0.03])
|
||||
slider = Slider(ax_slider, self.slider_labels[i], -2, 2, valinit=self.slider_values[i])
|
||||
slider.on_changed(lambda val, idx=i: self.update_tcp(idx, val))
|
||||
self.sliders.append(slider)
|
||||
y_pos -= 0.04
|
||||
|
||||
# 添加自由度标签
|
||||
joint_names = ['θ1 (Left Shoulder)', 'θ2 (Left Elbow)',
|
||||
'θ3 (Right Elbow)', 'θ4 (Right Shoulder)', 'θ5 (Gripper)']
|
||||
limits = self.robot.joint_limits
|
||||
ranges = [
|
||||
f"[{-round(np.degrees(v[0]), 1)}, {round(np.degrees(v[1]), 1)}]°"
|
||||
for v in limits.values()
|
||||
]
|
||||
|
||||
y_pos = 0.15
|
||||
for name, rng in zip(joint_names, ranges):
|
||||
plt.figtext(0.15, y_pos, f"{name}: {rng}", fontsize=10)
|
||||
y_pos -= 0.02
|
||||
|
||||
def update_tcp(self, idx, value):
|
||||
if idx == 0:
|
||||
self.robot.tcp_x = value
|
||||
elif idx == 1:
|
||||
self.robot.tcp_y = value
|
||||
elif idx == 2:
|
||||
self.robot.tcp_theta = value
|
||||
self.update_plot()
|
||||
|
||||
def update_plot(self):
|
||||
kinematics = self.robot.forward_kinematics()
|
||||
self.left_line.set_data(kinematics['left_arm'][:, 0], kinematics['left_arm'][:, 1])
|
||||
self.right_line.set_data(kinematics['right_arm'][:, 0], kinematics['right_arm'][:, 1])
|
||||
|
||||
gb = kinematics['gripper'][0]
|
||||
gt = kinematics['gripper'][1]
|
||||
self.gripper_line.set_data([gb[0], gt[0]], [gb[1], gt[1]])
|
||||
|
||||
self.fig.canvas.draw_idle()
|
||||
|
||||
def run(self):
|
||||
self.update_plot()
|
||||
plt.show()
|
||||
|
||||
# ======================= 主程序入口 =======================
|
||||
if __name__ == "__main__":
|
||||
gui = Symmetric5DoF_GUI()
|
||||
gui.run()
|
||||
301
else/new_main.py
Normal file
301
else/new_main.py
Normal file
@ -0,0 +1,301 @@
|
||||
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 (
|
||||
circle_trajectory,
|
||||
line_trajectory,
|
||||
line_trajectory_fix,
|
||||
ellipse_trajectory,
|
||||
square_trajectory,
|
||||
triangle_trajectory
|
||||
)
|
||||
|
||||
# -------------------- 只在非 Debug 模式导入 DM_CAN --------------------
|
||||
if not DEBUG_MODE:
|
||||
from DM_CAN.DM_CAN import Motor, MotorControl, DM_Motor_Type, Control_Type, DM_variable
|
||||
import serial
|
||||
from can_test_motor import motor_control_can_all
|
||||
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
|
||||
|
||||
# ------------------------ 轮趣科技电机配置 ------------------------
|
||||
MOTOR3_ID = 0x05
|
||||
|
||||
# 全局变量
|
||||
motor1 = motor2 = motor3 = 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】跳过电机初始化")
|
||||
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, 0x15)
|
||||
|
||||
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("电机已使能。")
|
||||
return motor1, motor2, motor_control
|
||||
|
||||
|
||||
# ------------------------ MIT 控制函数 ------------------------
|
||||
def control_two_motors_mit(theta1, theta4):
|
||||
global current_theta1, current_theta4
|
||||
|
||||
pos1 = np.degrees(theta1)
|
||||
pos4 = np.degrees(theta4)
|
||||
vel = 0.0
|
||||
|
||||
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] 模拟控制 -> Motor1: {pos1:.2f}°, Motor2: {pos4:.2f}°")
|
||||
|
||||
current_theta1 = theta1
|
||||
current_theta4 = theta4
|
||||
|
||||
|
||||
# ------------------------ 动画帧绘制函数 ------------------------
|
||||
def draw_frame(i):
|
||||
global line, x_list, y_list
|
||||
|
||||
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)
|
||||
# 右臂第一杆末端（注意 L0 偏移）
|
||||
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)
|
||||
|
||||
# 发送控制指令（在循环中同步）
|
||||
control_two_motors_mit(theta1, theta4)
|
||||
|
||||
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)}")
|
||||
|
||||
# --- 存储关节角度用于后续绘图 ---
|
||||
theta1_list = []
|
||||
theta4_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()
|
||||
|
||||
# --- 动画更新函数（修改：记录角度）---
|
||||
def draw_frame(i):
|
||||
x = x_list[i]
|
||||
y = y_list[i]
|
||||
|
||||
try:
|
||||
theta1, theta4 = inverseF(x, y, L1, L2, L3, L4, L0)
|
||||
|
||||
# 记录角度（弧度 → 角度）
|
||||
theta1_deg = np.degrees(theta1)
|
||||
theta4_deg = np.degrees(theta4)
|
||||
theta1_list.append(theta1_deg)
|
||||
theta4_list.append(theta4_deg)
|
||||
|
||||
# 左臂第一杆末端
|
||||
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)
|
||||
|
||||
# 发送控制指令
|
||||
control_two_motors_mit(theta1, theta4)
|
||||
|
||||
except Exception as e:
|
||||
print(f"第 {i} 帧逆解失败: {e}")
|
||||
line.set_data([], [])
|
||||
theta1_list.append(np.nan)
|
||||
theta4_list.append(np.nan)
|
||||
|
||||
return line,
|
||||
|
||||
# --- 创建动画 ---
|
||||
ani = FuncAnimation(
|
||||
fig, draw_frame,
|
||||
frames=len(x_list),
|
||||
interval=50,
|
||||
repeat=False,
|
||||
blit=True
|
||||
)
|
||||
|
||||
plt.show() # 显示机械臂动画
|
||||
print("动画结束，正在绘制关节角度轨迹...")
|
||||
|
||||
# --- 绘制关节角度曲线 ---
|
||||
if len(theta1_list) == 0:
|
||||
print("【警告】未记录到任何关节角度数据。")
|
||||
return
|
||||
|
||||
time_axis = np.linspace(0, len(theta1_list) * 0.05, len(theta1_list)) # 假设每帧 ~50ms
|
||||
|
||||
fig2, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 6), sharex=True)
|
||||
|
||||
# 电机1 关节角 θ1
|
||||
ax1.plot(time_axis, theta1_list, 'b-o', markersize=4, linewidth=2)
|
||||
ax1.set_ylabel('电机1 关节角 θ₁ (°)')
|
||||
ax1.set_title('电机关节角度轨迹')
|
||||
ax1.grid(True, alpha=0.5)
|
||||
ax1.axhline(y=0, color='k', linestyle='--', alpha=0.3)
|
||||
|
||||
# 电机2 关节角 θ4
|
||||
ax2.plot(time_axis, theta4_list, 'r-o', markersize=4, linewidth=2)
|
||||
ax2.set_xlabel('时间 (秒)')
|
||||
ax2.set_ylabel('电机2 关节角 θ₄ (°)')
|
||||
ax2.grid(True, alpha=0.5)
|
||||
ax2.axhline(y=0, color='k', linestyle='--', alpha=0.3)
|
||||
|
||||
plt.tight_layout()
|
||||
plt.show()
|
||||
|
||||
# --- 打印统计信息 ---
|
||||
print("\n=== 关节角度统计 ===")
|
||||
print(f"θ₁ 范围: {min(theta1_list):.2f}° ~ {max(theta1_list):.2f}°")
|
||||
print(f"θ₄ 范围: {min(theta4_list):.2f}° ~ {max(theta4_list):.2f}°")
|
||||
print(f"θ₁ 峰峰值: {max(theta1_list) - min(theta1_list):.2f}°")
|
||||
print(f"θ₄ 峰峰值: {max(theta4_list) - min(theta4_list):.2f}°")
|
||||
|
||||
# ------------------------ 主函数 ------------------------
|
||||
if __name__ == "__main__":
|
||||
# === 在这里设置轨迹类型和参数 ===
|
||||
|
||||
'''trajectory_config = {
|
||||
'func': ellipse_trajectory,
|
||||
'params': {
|
||||
'center': (100, 200),
|
||||
'rx': 50,
|
||||
'ry': 25,
|
||||
'num_points': 100
|
||||
}
|
||||
}'''
|
||||
|
||||
|
||||
# 可选其他轨迹：
|
||||
trajectory_config = {
|
||||
'func': line_trajectory_fix,
|
||||
'params': {'start': (25, 300), 'end': (200, 300), 'vx':3.0, 'vy':5.0, 'num_points':20}
|
||||
}
|
||||
#trajectory_config = {
|
||||
# 'func': circle_trajectory,
|
||||
# 'params': {'center': (100, 300), 'radius': 40}
|
||||
# }
|
||||
|
||||
# 初始化电机
|
||||
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 not DEBUG_MODE:
|
||||
motor_control.disable(motor1)
|
||||
motor_control.disable(motor2)
|
||||
print("电机已停机。")
|
||||
else:
|
||||
print("【DEBUG】程序结束")
|
||||
183
else/qiege_motor.py
Normal file
183
else/qiege_motor.py
Normal file
@ -0,0 +1,183 @@
|
||||
# qiege_motor.py
|
||||
# 切割电机控制模块（支持 CAN 和 USB 两种通信方式）
|
||||
# 使用 control(enable) 统一接口
|
||||
|
||||
import time
|
||||
import serial
|
||||
|
||||
# ------------------------ 配置区 ------------------------
|
||||
|
||||
# <<< 切换通信模式 >>>
|
||||
MODE = 'usb' # 'can' 或 'usb'
|
||||
|
||||
# CAN 模式配置
|
||||
CAN_SERIAL_PORT = '/dev/ttyACM0' # CAN 转串口设备
|
||||
CAN_BAUDRATE = 921600
|
||||
CAN_MOTOR_ID = 0x05 # 轮趣电机对应的 CAN ID
|
||||
|
||||
# USB 串口模式配置（轮趣 HEX 指令）
|
||||
USB_SERIAL_PORT = '/dev/ttyACM0' # 或 '/dev/ttyUSB0'
|
||||
USB_BAUDRATE = 115200
|
||||
|
||||
# 目标转速（rad/s）
|
||||
CUTTING_SPEED_RAD_PER_SEC = 8.0 # 对应 HEX 指令中的 speed=8 → 80 (0x50)
|
||||
|
||||
# 是否启用调试输出
|
||||
DEBUG = True
|
||||
|
||||
# ------------------------ 全局变量 ------------------------
|
||||
usb_serial = None
|
||||
can_serial = None
|
||||
|
||||
|
||||
# ------------------------ USB 模式：HEX 指令生成与发送 ------------------------
|
||||
def generate_usb_command(speed):
|
||||
"""
|
||||
生成轮趣电机 USB 串口 HEX 指令
|
||||
格式: 7B 01 01 00 20 00 00 00 <speed_byte> <checksum> 7D
|
||||
speed: float (rad/s), 映射为 0~255
|
||||
"""
|
||||
# 将 rad/s 映射为速度字节（示例：8 rad/s → 80）
|
||||
# 你可以根据实际响应调整映射关系
|
||||
speed_val = int(speed * 10) # 示例：8 → 80
|
||||
speed_byte = max(0, min(255, speed_val))
|
||||
|
||||
# 构造前9字节
|
||||
cmd = [
|
||||
0x7B, 0x01, 0x02, # 帧头、地址、模式
|
||||
0x01, 0x20, # 方向、细分
|
||||
0x0E, 0x10, # 位置高、低（持续旋转）
|
||||
0x00, 0x64, # 速度值
|
||||
]
|
||||
|
||||
# 计算校验和（前9字节异或）
|
||||
checksum = 0
|
||||
for b in cmd:
|
||||
checksum ^= b
|
||||
cmd.append(checksum)
|
||||
cmd.append(0x7D) # 帧尾
|
||||
|
||||
return bytes(cmd)
|
||||
|
||||
|
||||
def send_usb_command(port, baudrate, command):
|
||||
"""发送 USB 串口指令"""
|
||||
global usb_serial
|
||||
try:
|
||||
if usb_serial is None or not usb_serial.is_open:
|
||||
usb_serial = serial.Serial(port, baudrate, timeout=0.5)
|
||||
time.sleep(0.5) # 等待串口稳定
|
||||
|
||||
usb_serial.write(command)
|
||||
if DEBUG:
|
||||
print(f"[qiege_motor] USB 发送: {command.hex().upper()}")
|
||||
return True
|
||||
except Exception as e:
|
||||
if DEBUG:
|
||||
print(f"[qiege_motor] USB 发送失败: {e}")
|
||||
usb_serial = None
|
||||
return False
|
||||
|
||||
|
||||
# ------------------------ CAN 模式：CAN 指令发送 ------------------------
|
||||
def send_can_command(data, can_id=CAN_MOTOR_ID):
|
||||
"""通过 CAN 发送指令（假设使用串口转 CAN 模块）"""
|
||||
global can_serial
|
||||
try:
|
||||
if can_serial is None or not can_serial.is_open:
|
||||
can_serial = serial.Serial(CAN_SERIAL_PORT, CAN_BAUDRATE, timeout=0.5)
|
||||
time.sleep(0.5)
|
||||
|
||||
# 示例：简单封装 CAN 帧（ID + DLC + Data）
|
||||
# 格式根据你使用的 CAN 模块调整（如 CANable、TJA1050 等）
|
||||
can_frame = [
|
||||
(can_id >> 24) & 0xFF,
|
||||
(can_id >> 16) & 0xFF,
|
||||
(can_id >> 8) & 0xFF,
|
||||
can_id & 0xFF,
|
||||
len(data) # DLC
|
||||
] + list(data)
|
||||
can_serial.write(bytes(can_frame))
|
||||
|
||||
if DEBUG:
|
||||
print(f"[qiege_motor] CAN 发送 → ID:{can_id:05X}, Data:{[f'{b:02X}' for b in data]}")
|
||||
return True
|
||||
except Exception as e:
|
||||
if DEBUG:
|
||||
print(f"[qiege_motor] CAN 发送失败: {e}")
|
||||
can_serial = None
|
||||
return False
|
||||
|
||||
|
||||
def generate_can_command(speed_rpm=300, direction=1, microstep=32):
|
||||
"""生成轮趣 CAN 速度指令（示例）"""
|
||||
speed_01rpm = int(abs(speed_rpm) * 10)
|
||||
high_byte = (speed_01rpm >> 8) & 0xFF
|
||||
low_byte = speed_01rpm & 0xFF
|
||||
direction_bit = 1 if direction else 0
|
||||
|
||||
data = [
|
||||
0x88, # 控制字：速度模式
|
||||
((direction_bit & 0x01) << 7) | (microstep & 0x7F),
|
||||
0x00, 0x00,
|
||||
high_byte, low_byte,
|
||||
0x00, 0x00
|
||||
]
|
||||
return data
|
||||
|
||||
|
||||
# ------------------------ 统一控制接口 ------------------------
|
||||
def start_motor():
|
||||
"""启动切割电机"""
|
||||
if MODE == 'usb':
|
||||
cmd = generate_usb_command(CUTTING_SPEED_RAD_PER_SEC)
|
||||
return send_usb_command(USB_SERIAL_PORT, USB_BAUDRATE, cmd)
|
||||
elif MODE == 'can':
|
||||
# 示例：8 rad/s ≈ 76.4 RPM
|
||||
rpm = CUTTING_SPEED_RAD_PER_SEC * 60 / (2 * 3.1416)
|
||||
cmd = generate_can_command(speed_rpm=rpm, direction=1)
|
||||
return send_can_command(cmd, can_id=CAN_MOTOR_ID)
|
||||
else:
|
||||
print(f"[qiege_motor] 错误：不支持的模式 '{MODE}'")
|
||||
return False
|
||||
|
||||
|
||||
def stop_motor():
|
||||
"""停止切割电机"""
|
||||
if MODE == 'usb':
|
||||
cmd = generate_usb_command(0.0)
|
||||
return send_usb_command(USB_SERIAL_PORT, USB_BAUDRATE, cmd)
|
||||
elif MODE == 'can':
|
||||
cmd = generate_can_command(speed_rpm=0, direction=1)
|
||||
return send_can_command(cmd, can_id=CAN_MOTOR_ID)
|
||||
else:
|
||||
return False
|
||||
|
||||
|
||||
def control(enable: bool):
|
||||
"""
|
||||
统一控制接口
|
||||
:param enable: True 启动, False 停止
|
||||
:return: bool 成功与否
|
||||
"""
|
||||
try:
|
||||
if enable:
|
||||
return start_motor()
|
||||
else:
|
||||
return stop_motor()
|
||||
except Exception as e:
|
||||
if DEBUG:
|
||||
print(f"[qiege_motor] 控制异常: {e}")
|
||||
return False
|
||||
|
||||
|
||||
# ------------------------ 测试 ------------------------
|
||||
if __name__ == "__main__":
|
||||
print(f"【qiege_motor】测试开始 (模式: {MODE})")
|
||||
print("启动 → 1秒后停止")
|
||||
|
||||
control(True)
|
||||
time.sleep(1)
|
||||
control(False)
|
||||
|
||||
print("测试结束。")
|
||||
160
else/test1.py
Normal file
160
else/test1.py
Normal file
@ -0,0 +1,160 @@
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from matplotlib.widgets import Slider, Button
|
||||
import matplotlib.pyplot as plt
|
||||
import matplotlib
|
||||
|
||||
# 设置中文字体和解决负号显示问题
|
||||
plt.rcParams['font.sans-serif'] = ['SimHei', 'WenQuanYi Zen Hei', 'FangSong'] # 按优先级选择字体
|
||||
plt.rcParams['axes.unicode_minus'] = False # 显示负号 -
|
||||
|
||||
# ======================= 对称五自由度机器人模型 =======================
|
||||
class Symmetric5DoFRobot:
|
||||
def __init__(self):
|
||||
self.L1 = 0.25 # 臂段长度
|
||||
self.L2 = 0.3
|
||||
self.Y0 = 0.25 # 左右臂起始点在 Y 轴上的偏移
|
||||
self.tcp_x = 0.8 # 默认 TCP 目标位置
|
||||
self.tcp_y = 0.0
|
||||
self.tcp_theta = 0 # 夹具方向角度（弧度）
|
||||
|
||||
def inverse_kinematics(self, x, y, theta):
|
||||
"""给定末端位置和方向，返回左右臂各关节角度"""
|
||||
# 计算左臂角度
|
||||
try:
|
||||
cos_q2_left = (x**2 + y**2 - self.L1**2 - self.L2**2) / (2 * self.L1 * self.L2)
|
||||
sin_q2_left = np.sqrt(1 - cos_q2_left**2)
|
||||
q2_left = np.arctan2(sin_q2_left, cos_q2_left)
|
||||
|
||||
k1 = self.L1 + self.L2 * np.cos(q2_left)
|
||||
k2 = self.L2 * np.sin(q2_left)
|
||||
q1_left = np.arctan2(y, x) - np.arctan2(k2, k1)
|
||||
|
||||
except:
|
||||
q1_left = 0
|
||||
q2_left = 0
|
||||
|
||||
# 右臂镜像求解
|
||||
try:
|
||||
cos_q2_right = (x**2 + y**2 - self.L1**2 - self.L2**2) / (2 * self.L1 * self.L2)
|
||||
sin_q2_right = -np.sqrt(1 - cos_q2_right**2)
|
||||
q2_right = np.arctan2(sin_q2_right, cos_q2_right)
|
||||
|
||||
k1 = self.L1 + self.L2 * np.cos(q2_right)
|
||||
k2 = self.L2 * np.sin(q2_right)
|
||||
q1_right = np.arctan2(y, x) - np.arctan2(k2, k1)
|
||||
|
||||
except:
|
||||
q1_right = 0
|
||||
q2_right = 0
|
||||
|
||||
return {
|
||||
'left': [q1_left, q2_left],
|
||||
'right': [q1_right, q2_right],
|
||||
'theta': theta
|
||||
}
|
||||
|
||||
def forward_kinematics(self):
|
||||
ik = self.inverse_kinematics(self.tcp_x, self.tcp_y, self.tcp_theta)
|
||||
θ1, θ2 = ik['left']
|
||||
θ4, θ3 = ik['right']
|
||||
|
||||
# 左臂坐标计算
|
||||
left_base = np.array([0, -self.Y0])
|
||||
j1_left = left_base + np.array([self.L1 * np.cos(θ1), self.L1 * np.sin(θ1)])
|
||||
tcp_left = j1_left + np.array([
|
||||
self.L2 * np.cos(θ1 + θ2),
|
||||
self.L2 * np.sin(θ1 + θ2)
|
||||
])
|
||||
|
||||
# 右臂坐标计算
|
||||
right_base = np.array([0, self.Y0])
|
||||
j1_right = right_base + np.array([self.L1 * np.cos(θ4), self.L1 * np.sin(θ4)])
|
||||
tcp_right = j1_right + np.array([
|
||||
self.L2 * np.cos(θ4 + θ3),
|
||||
self.L2 * np.sin(θ4 + θ3)
|
||||
])
|
||||
|
||||
# 确保末端相连
|
||||
tcp_point = (tcp_left + tcp_right) / 2
|
||||
gripper_dir = np.array([np.cos(self.tcp_theta), np.sin(self.tcp_theta)]) * 0.3
|
||||
|
||||
return {
|
||||
'left_arm': np.array([left_base, j1_left, tcp_point]),
|
||||
'right_arm': np.array([right_base, j1_right, tcp_point]),
|
||||
'gripper': [tcp_point, tcp_point + gripper_dir]
|
||||
}
|
||||
|
||||
# ======================= GUI界面 =======================
|
||||
class Symmetric5DoF_GUI:
|
||||
def __init__(self):
|
||||
self.robot = Symmetric5DoFRobot()
|
||||
|
||||
self.fig, self.ax = plt.subplots(figsize=(8, 6))
|
||||
plt.subplots_adjust(left=0.1, right=0.9, top=0.85, bottom=0.3)
|
||||
self.ax.set_title("非共基座对称五自由度机器人（末端相连 + 夹具方向）")
|
||||
self.ax.set_xlim(-1.5, 1.5)
|
||||
self.ax.set_ylim(-1.5, 1.5)
|
||||
self.ax.set_aspect('equal')
|
||||
|
||||
# 初始化线条
|
||||
self.left_line, = self.ax.plot([], [], 'b-o', lw=2, markersize=6, label="左臂")
|
||||
self.right_line, = self.ax.plot([], [], 'r-o', lw=2, markersize=6, label="右臂")
|
||||
self.gripper_line, = self.ax.plot([], [], 'g-->', lw=2, markersize=4, label="夹具方向")
|
||||
|
||||
plt.legend()
|
||||
|
||||
# 创建滑动条
|
||||
self.sliders = []
|
||||
self.slider_labels = ['X', 'Y', 'Theta']
|
||||
self.slider_values = [self.robot.tcp_x, self.robot.tcp_y, self.robot.tcp_theta]
|
||||
|
||||
y_pos = 0.2
|
||||
for i in range(3):
|
||||
ax_slider = plt.axes([0.2, y_pos, 0.6, 0.03])
|
||||
slider = Slider(ax_slider, self.slider_labels[i], -2, 2, valinit=self.slider_values[i])
|
||||
slider.on_changed(lambda val, idx=i: self.update_tcp(idx, val))
|
||||
self.sliders.append(slider)
|
||||
y_pos -= 0.04
|
||||
|
||||
# 添加重置按钮
|
||||
reset_ax = plt.axes([0.8, 0.02, 0.1, 0.04])
|
||||
self.reset_button = Button(reset_ax, 'Reset', hovercolor='0.975')
|
||||
self.reset_button.on_clicked(self.reset)
|
||||
|
||||
def update_tcp(self, idx, value):
|
||||
if idx == 0:
|
||||
self.robot.tcp_x = value
|
||||
elif idx == 1:
|
||||
self.robot.tcp_y = value
|
||||
elif idx == 2:
|
||||
self.robot.tcp_theta = value
|
||||
self.update_plot()
|
||||
|
||||
def update_plot(self):
|
||||
kinematics = self.robot.forward_kinematics()
|
||||
self.left_line.set_data(kinematics['left_arm'][:, 0], kinematics['left_arm'][:, 1])
|
||||
self.right_line.set_data(kinematics['right_arm'][:, 0], kinematics['right_arm'][:, 1])
|
||||
|
||||
gb = kinematics['gripper'][0]
|
||||
gt = kinematics['gripper'][1]
|
||||
self.gripper_line.set_data([gb[0], gt[0]], [gb[1], gt[1]])
|
||||
|
||||
self.fig.canvas.draw_idle()
|
||||
|
||||
def reset(self, event):
|
||||
self.robot.tcp_x = 0.8
|
||||
self.robot.tcp_y = 0.0
|
||||
self.robot.tcp_theta = 0
|
||||
for i, s in enumerate(self.sliders):
|
||||
s.set_val([0.8, 0, 0][i])
|
||||
self.update_plot()
|
||||
|
||||
def run(self):
|
||||
self.update_plot()
|
||||
plt.show()
|
||||
|
||||
# ======================= 主程序入口 =======================
|
||||
if __name__ == "__main__":
|
||||
gui = Symmetric5DoF_GUI()
|
||||
gui.run()
|
||||
55
else/test_chuankou.py
Normal file
55
else/test_chuankou.py
Normal file
@ -0,0 +1,55 @@
|
||||
from calculate.calculate_angle import main_of_5dof
|
||||
|
||||
# control_with_serial.py
|
||||
|
||||
|
||||
# 📡 初始化串口连接
|
||||
def init_serial(port=SERIAL_PORT, baud_rate=BAUD_RATE):
|
||||
try:
|
||||
ser = serial.Serial(port, baud_rate, timeout=TIMEOUT)
|
||||
print(f"串口已连接: {port} @ {baud_rate}")
|
||||
return ser
|
||||
except Exception as e:
|
||||
print(f"无法打开串口 {port}: {e}")
|
||||
return None
|
||||
|
||||
# 📤 发送角度到串口
|
||||
def send_angles(ser, theta1_deg, theta4_deg):
|
||||
# 格式：theta1_deg,theta4_deg\n
|
||||
data = f"{int(theta1_deg)},{int(theta4_deg)}\n"
|
||||
try:
|
||||
ser.write(data.encode())
|
||||
print(f"已发送角度: theta1 = {theta1_deg}°, theta4 = {theta4_deg}°")
|
||||
except Exception as e:
|
||||
print(f"发送失败: {e}")
|
||||
|
||||
# 🚀 主程序入口
|
||||
if __name__ == "__main__":
|
||||
# 调用你的轨迹规划函数
|
||||
angles_list = main_of_5dof(
|
||||
trajectory_type='line',
|
||||
start=(0, 0),
|
||||
end=(200, 300),
|
||||
show_animation=False
|
||||
)
|
||||
|
||||
# 初始化串口
|
||||
ser = init_serial()
|
||||
if ser is None:
|
||||
exit(1)
|
||||
|
||||
# 遍历角度列表并发送
|
||||
for i, (theta1, theta4) in enumerate(angles_list):
|
||||
theta1_deg = np.degrees(theta1)
|
||||
theta4_deg = np.degrees(theta4)
|
||||
|
||||
print(f"第 {i} 帧角度: theta1 = {theta1_deg:.2f}°, theta4 = {theta4_deg:.2f}°")
|
||||
send_angles(ser, theta1_deg, theta4_deg)
|
||||
|
||||
# 可选：每帧之间加延迟，控制发送速度
|
||||
import time
|
||||
time.sleep(0.05) # 每帧间隔 50ms
|
||||
|
||||
# 关闭串口
|
||||
ser.close()
|
||||
print("串口已关闭")
|
||||
95
else/test_duoji.py
Normal file
95
else/test_duoji.py
Normal file
@ -0,0 +1,95 @@
|
||||
import time
|
||||
import numpy as np
|
||||
|
||||
# 导入运动学函数
|
||||
from calculate.ik import inverseF
|
||||
|
||||
# 导入串口控制函数
|
||||
from duoji_test.motor2 import send_hex_command, generate_position_command
|
||||
|
||||
# 机械臂参数（和 qt_main.py 保持一致）
|
||||
L1 = 250
|
||||
L2 = 300
|
||||
L3 = 300
|
||||
L4 = 250
|
||||
L0 = 250
|
||||
|
||||
# 串口配置
|
||||
MOTOR1_PORT = "/dev/ttyACM1" # 电机1使用的串口
|
||||
MOTOR2_PORT = "/dev/ttyACM0" # 电机2使用的串口
|
||||
BAUD_RATE = 115200
|
||||
|
||||
# 控制参数
|
||||
SPEED_MOTOR1 = 10 # Rad/s
|
||||
SPEED_MOTOR2 = 10 # Rad/s
|
||||
DIRECTION_MOTOR1 = 1 # 0: 逆时针, 1: 顺时针
|
||||
DIRECTION_MOTOR2 = 1
|
||||
|
||||
# 初始化电机角度为0
|
||||
current_angle_motor1 = 0
|
||||
current_angle_motor2 = 0
|
||||
|
||||
|
||||
def control_two_motors(theta1, theta4):
|
||||
"""
|
||||
根据 theta1 和 theta4 控制两个电机转动指定角度
|
||||
:param theta1: 左臂角度（弧度）
|
||||
:param theta4: 右臂角度（弧度）
|
||||
"""
|
||||
global current_angle_motor1, current_angle_motor2
|
||||
|
||||
# 弧度转角度
|
||||
angle1_target = np.degrees(theta1)
|
||||
angle4_target = np.degrees(theta4)
|
||||
|
||||
# 计算相对角度
|
||||
relative_angle1 = angle1_target - current_angle_motor1
|
||||
relative_angle4 = angle4_target - current_angle_motor2
|
||||
|
||||
print(f"发送相对角度: 电机1={relative_angle1:.2f}°, 电机2={relative_angle4:.2f}°")
|
||||
|
||||
# 更新当前角度
|
||||
current_angle_motor1 += relative_angle1
|
||||
current_angle_motor2 += relative_angle4
|
||||
|
||||
# 生成指令
|
||||
cmd1 = generate_position_command(angle=relative_angle1, speed=SPEED_MOTOR1, direction=DIRECTION_MOTOR1)
|
||||
cmd2 = generate_position_command(angle=relative_angle4, speed=SPEED_MOTOR2, direction=DIRECTION_MOTOR2)
|
||||
|
||||
# 发送指令到两个电机
|
||||
send_hex_command(MOTOR1_PORT, BAUD_RATE, cmd1)
|
||||
send_hex_command(MOTOR2_PORT, BAUD_RATE, cmd2)
|
||||
|
||||
|
||||
def run_trajectory():
|
||||
"""
|
||||
主函数：模拟轨迹点并控制电机
|
||||
"""
|
||||
|
||||
# 模拟一个圆形轨迹（你可以替换成从文件读取或用户输入）
|
||||
center = (150, 250)
|
||||
radius = 60
|
||||
num_points = 100
|
||||
angles = np.linspace(0, 2 * np.pi, num_points)
|
||||
x_list = center[0] + radius * np.cos(angles)
|
||||
y_list = center[1] + radius * np.sin(angles)
|
||||
|
||||
print("开始执行轨迹控制...")
|
||||
|
||||
for i in range(len(x_list)):
|
||||
x = x_list[i]
|
||||
y = y_list[i]
|
||||
|
||||
try:
|
||||
theta1, theta4 = inverseF(x, y, L1, L2, L3, L4, L0)
|
||||
control_two_motors(theta1, theta4)
|
||||
except Exception as e:
|
||||
print(f"第 {i} 点计算失败: {e}")
|
||||
|
||||
time.sleep(0.1) # 控制发送频率（可根据实际需求调整）
|
||||
|
||||
print("轨迹执行完成。")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
run_trajectory()
|
||||
Reference in New Issue
Block a user