From 9477f25a5117e66f7673fc1a3424d5a2c6cc0f3e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E7=90=89=E7=92=83=E6=9C=88=E5=85=89?= <15630071+llyg777@user.noreply.gitee.com> Date: Wed, 13 Aug 2025 12:36:04 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E5=88=9D=E5=A7=8B=E5=8C=96=E9=A1=B9?= =?UTF-8?q?=E7=9B=AE=EF=BC=8C=E6=B7=BB=E5=8A=A0=E7=94=B5=E6=9C=BA=E6=8E=A7?= =?UTF-8?q?=E5=88=B6=E3=80=81CAN=E9=80=9A=E4=BF=A1=E3=80=81QT=E7=95=8C?= =?UTF-8?q?=E9=9D=A2=E7=AD=89=E6=A8=A1=E5=9D=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .idea/.gitignore | 3 + .idea/5dof.iml | 8 + .../inspectionProfiles/profiles_settings.xml | 6 + .idea/misc.xml | 7 + .idea/modules.xml | 8 + 5dof_motor_main.py | 226 +++++++ 5dof_motor_test.py | 245 +++++++ 5dof_test_ctrl_motor.py | 80 +++ DM_CAN/DM_CAN.py | 624 ++++++++++++++++++ DM_CAN/DM_Motor_Test.py | 114 ++++ DM_CAN/__pycache__/DM_CAN.cpython-39.pyc | Bin 0 -> 20252 bytes calculate/__pycache__/ik.cpython-39.pyc | Bin 0 -> 1816 bytes .../__pycache__/trajectory.cpython-39.pyc | Bin 0 -> 2209 bytes calculate/angle_A.txt | 1 + calculate/angle_B.txt | 1 + calculate/calculate_angle.py | 179 +++++ calculate/fk.py | 40 ++ calculate/ik.py | 69 ++ calculate/test_ik.py | 64 ++ calculate/traj_fk.py | 62 ++ calculate/traj_main.py | 71 ++ calculate/trajectory.py | 62 ++ .../__pycache__/can_main.cpython-38.pyc | Bin 0 -> 2930 bytes .../motor_control_can_all.cpython-38.pyc | Bin 0 -> 4798 bytes can_test_motor/can_main.py | 93 +++ can_test_motor/can_run_demo.py | 15 + can_test_motor/motor_control_can_all.py | 174 +++++ main_limit.py | 187 ++++++ .../__pycache__/can_main.cpython-38.pyc | Bin 0 -> 3368 bytes motor_control/can_main.py | 93 +++ motor_control/can_run_demo.py | 13 + motor_control/can_test.py | 105 +++ motor_control/contral.py | 134 ++++ motor_control/motor_control_can_all.py | 170 +++++ motor_control/motor_control_usb_all.py | 212 ++++++ motor_control/motor_test_mostimprotant.py | 134 ++++ motor_control/test.py | 9 + qiege_motor.py | 183 +++++ qt/DM_CAN/DM_CAN.py | 624 ++++++++++++++++++ qt/DM_CAN/DM_Motor_Test.py | 114 ++++ qt/__pycache__/untitled.cpython-39.pyc | Bin 0 -> 1604 bytes qt/calculate/__pycache__/ik.cpython-39.pyc | Bin 0 -> 1819 bytes .../__pycache__/trajectory.cpython-39.pyc | Bin 0 -> 2207 bytes qt/calculate/calculate_angle.py | 138 ++++ qt/calculate/fk.py | 40 ++ qt/calculate/ik.py | 69 ++ qt/calculate/test_ik.py | 64 ++ qt/calculate/traj_fk.py | 62 ++ qt/calculate/traj_main.py | 71 ++ qt/calculate/trajectory.py | 62 ++ qt/can_test_motor/can_main.py | 93 +++ qt/can_test_motor/can_run_demo.py | 15 + qt/can_test_motor/motor_control_can_all.py | 174 +++++ qt/qt_main.py | 146 ++++ qt/qt_motor_main.py | 305 +++++++++ qt/untitled.py | 46 ++ qt/untitled.ui | 91 +++ test1.py | 160 +++++ test_chuankou.py | 55 ++ test_duoji.py | 95 +++ 60 files changed, 5816 insertions(+) create mode 100644 .idea/.gitignore create mode 100644 .idea/5dof.iml create mode 100644 .idea/inspectionProfiles/profiles_settings.xml create mode 100644 .idea/misc.xml create mode 100644 .idea/modules.xml create mode 100644 5dof_motor_main.py create mode 100644 5dof_motor_test.py create mode 100644 5dof_test_ctrl_motor.py create mode 100644 DM_CAN/DM_CAN.py create mode 100644 DM_CAN/DM_Motor_Test.py create mode 100644 DM_CAN/__pycache__/DM_CAN.cpython-39.pyc create mode 100644 calculate/__pycache__/ik.cpython-39.pyc create mode 100644 calculate/__pycache__/trajectory.cpython-39.pyc create mode 100644 calculate/angle_A.txt create mode 100644 calculate/angle_B.txt create mode 100644 calculate/calculate_angle.py create mode 100644 calculate/fk.py create mode 100644 calculate/ik.py create mode 100644 calculate/test_ik.py create mode 100644 calculate/traj_fk.py create mode 100644 calculate/traj_main.py create mode 100644 calculate/trajectory.py create mode 100644 can_test_motor/__pycache__/can_main.cpython-38.pyc create mode 100644 can_test_motor/__pycache__/motor_control_can_all.cpython-38.pyc create mode 100644 can_test_motor/can_main.py create mode 100644 can_test_motor/can_run_demo.py create mode 100644 can_test_motor/motor_control_can_all.py create mode 100644 main_limit.py create mode 100644 motor_control/__pycache__/can_main.cpython-38.pyc create mode 100644 motor_control/can_main.py create mode 100644 motor_control/can_run_demo.py create mode 100644 motor_control/can_test.py create mode 100644 motor_control/contral.py create mode 100644 motor_control/motor_control_can_all.py create mode 100644 motor_control/motor_control_usb_all.py create mode 100644 motor_control/motor_test_mostimprotant.py create mode 100644 motor_control/test.py create mode 100644 qiege_motor.py create mode 100644 qt/DM_CAN/DM_CAN.py create mode 100644 qt/DM_CAN/DM_Motor_Test.py create mode 100644 qt/__pycache__/untitled.cpython-39.pyc create mode 100644 qt/calculate/__pycache__/ik.cpython-39.pyc create mode 100644 qt/calculate/__pycache__/trajectory.cpython-39.pyc create mode 100644 qt/calculate/calculate_angle.py create mode 100644 qt/calculate/fk.py create mode 100644 qt/calculate/ik.py create mode 100644 qt/calculate/test_ik.py create mode 100644 qt/calculate/traj_fk.py create mode 100644 qt/calculate/traj_main.py create mode 100644 qt/calculate/trajectory.py create mode 100644 qt/can_test_motor/can_main.py create mode 100644 qt/can_test_motor/can_run_demo.py create mode 100644 qt/can_test_motor/motor_control_can_all.py create mode 100644 qt/qt_main.py create mode 100644 qt/qt_motor_main.py create mode 100644 qt/untitled.py create mode 100644 qt/untitled.ui create mode 100644 test1.py create mode 100644 test_chuankou.py create mode 100644 test_duoji.py diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000..359bb53 --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,3 @@ +# 默认忽略的文件 +/shelf/ +/workspace.xml diff --git a/.idea/5dof.iml b/.idea/5dof.iml new file mode 100644 index 0000000..ef778ac --- /dev/null +++ b/.idea/5dof.iml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000..ed483f3 --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,7 @@ + + + + + + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml new file mode 100644 index 0000000..398f280 --- /dev/null +++ b/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/5dof_motor_main.py b/5dof_motor_main.py new file mode 100644 index 0000000..f618612 --- /dev/null +++ b/5dof_motor_main.py @@ -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': (125, 400)} + } + #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】程序结束") \ No newline at end of file diff --git a/5dof_motor_test.py b/5dof_motor_test.py new file mode 100644 index 0000000..0f585e8 --- /dev/null +++ b/5dof_motor_test.py @@ -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】程序结束") \ No newline at end of file diff --git a/5dof_test_ctrl_motor.py b/5dof_test_ctrl_motor.py new file mode 100644 index 0000000..73fbb12 --- /dev/null +++ b/5dof_test_ctrl_motor.py @@ -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() \ No newline at end of file diff --git a/DM_CAN/DM_CAN.py b/DM_CAN/DM_CAN.py new file mode 100644 index 0000000..d548f7f --- /dev/null +++ b/DM_CAN/DM_CAN.py @@ -0,0 +1,624 @@ +from time import sleep +import numpy as np +from enum import IntEnum +from struct import unpack +from struct import pack + + +class Motor: + def __init__(self, MotorType, SlaveID, MasterID): + """ + define Motor object 定义电机对象 + :param MotorType: Motor type 电机类型 + :param SlaveID: CANID 电机ID + :param MasterID: MasterID 主机ID 建议不要设为0 + """ + self.Pd = float(0) + self.Vd = float(0) + self.state_q = float(0) + self.state_dq = float(0) + self.state_tau = float(0) + self.SlaveID = SlaveID + self.MasterID = MasterID + self.MotorType = MotorType + self.isEnable = False + self.NowControlMode = Control_Type.MIT + self.temp_param_dict = {} + + def recv_data(self, q: float, dq: float, tau: float): + self.state_q = q + self.state_dq = dq + self.state_tau = tau + + def getPosition(self): + """ + get the position of the motor 获取电机位置 + :return: the position of the motor 电机位置 + """ + return self.state_q + + def getVelocity(self): + """ + get the velocity of the motor 获取电机速度 + :return: the velocity of the motor 电机速度 + """ + return self.state_dq + + def getTorque(self): + """ + get the torque of the motor 获取电机力矩 + :return: the torque of the motor 电机力矩 + """ + return self.state_tau + + def getParam(self, RID): + """ + get the parameter of the motor 获取电机内部的参数，需要提前读取 + :param RID: DM_variable 电机参数 + :return: the parameter of the motor 电机参数 + """ + if RID in self.temp_param_dict: + return self.temp_param_dict[RID] + else: + return None + + +class MotorControl: + send_data_frame = np.array( + [0x55, 0xAA, 0x1e, 0x03, 0x01, 0x00, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0, 0, 0, 0, 0x00, 0x08, 0x00, + 0x00, 0, 0, 0, 0, 0, 0, 0, 0, 0x00], np.uint8) + # 4310 4310_48 4340 4340_48 + Limit_Param = [[12.5, 30, 10], [12.5, 50, 10], [12.5, 8, 28], [12.5, 10, 28], + # 6006 8006 8009 10010L 10010 + [12.5, 45, 20], [12.5, 45, 40], [12.5, 45, 54], [12.5, 25, 200], [12.5, 20, 200], + # H3510 DMG62150 DMH6220 + [12.5 , 280 , 1],[12.5 , 45 , 10],[12.5 , 45 , 10]] + + def __init__(self, serial_device): + """ + define MotorControl object 定义电机控制对象 + :param serial_device: serial object 串口对象 + """ + self.serial_ = serial_device + self.motors_map = dict() + self.data_save = bytes() # save data + if self.serial_.is_open: # open the serial port + print("Serial port is open") + serial_device.close() + self.serial_.open() + + def controlMIT(self, DM_Motor, kp: float, kd: float, q: float, dq: float, tau: float): + """ + MIT Control Mode Function 达妙电机MIT控制模式函数 + :param DM_Motor: Motor object 电机对象 + :param kp: kp + :param kd: kd + :param q: position 期望位置 + :param dq: velocity 期望速度 + :param tau: torque 期望力矩 + :return: None + """ + if DM_Motor.SlaveID not in self.motors_map: + print("controlMIT ERROR : Motor ID not found") + return + kp_uint = float_to_uint(kp, 0, 500, 12) + kd_uint = float_to_uint(kd, 0, 5, 12) + MotorType = DM_Motor.MotorType + Q_MAX = self.Limit_Param[MotorType][0] + DQ_MAX = self.Limit_Param[MotorType][1] + TAU_MAX = self.Limit_Param[MotorType][2] + q_uint = float_to_uint(q, -Q_MAX, Q_MAX, 16) + dq_uint = float_to_uint(dq, -DQ_MAX, DQ_MAX, 12) + tau_uint = float_to_uint(tau, -TAU_MAX, TAU_MAX, 12) + data_buf = np.array([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8) + data_buf[0] = (q_uint >> 8) & 0xff + data_buf[1] = q_uint & 0xff + data_buf[2] = dq_uint >> 4 + data_buf[3] = ((dq_uint & 0xf) << 4) | ((kp_uint >> 8) & 0xf) + data_buf[4] = kp_uint & 0xff + data_buf[5] = kd_uint >> 4 + data_buf[6] = ((kd_uint & 0xf) << 4) | ((tau_uint >> 8) & 0xf) + data_buf[7] = tau_uint & 0xff + self.__send_data(DM_Motor.SlaveID, data_buf) + self.recv() # receive the data from serial port + + def control_delay(self, DM_Motor, kp: float, kd: float, q: float, dq: float, tau: float, delay: float): + """ + MIT Control Mode Function with delay 达妙电机MIT控制模式函数带延迟 + :param DM_Motor: Motor object 电机对象 + :param kp: kp + :param kd: kd + :param q: position 期望位置 + :param dq: velocity 期望速度 + :param tau: torque 期望力矩 + :param delay: delay time 延迟时间 单位秒 + """ + self.controlMIT(DM_Motor, kp, kd, q, dq, tau) + sleep(delay) + + def control_Pos_Vel(self, Motor, P_desired: float, V_desired: float): + """ + control the motor in position and velocity control mode 电机位置速度控制模式 + :param Motor: Motor object 电机对象 + :param P_desired: desired position 期望位置 + :param V_desired: desired velocity 期望速度 + :return: None + """ + if Motor.SlaveID not in self.motors_map: + print("Control Pos_Vel Error : Motor ID not found") + return + motorid = 0x100 + Motor.SlaveID + data_buf = np.array([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8) + P_desired_uint8s = float_to_uint8s(P_desired) + V_desired_uint8s = float_to_uint8s(V_desired) + data_buf[0:4] = P_desired_uint8s + data_buf[4:8] = V_desired_uint8s + self.__send_data(motorid, data_buf) + # time.sleep(0.001) + self.recv() # receive the data from serial port + + def control_Vel(self, Motor, Vel_desired): + """ + control the motor in velocity control mode 电机速度控制模式 + :param Motor: Motor object 电机对象 + :param Vel_desired: desired velocity 期望速度 + """ + if Motor.SlaveID not in self.motors_map: + print("control_VEL ERROR : Motor ID not found") + return + motorid = 0x200 + Motor.SlaveID + data_buf = np.array([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8) + Vel_desired_uint8s = float_to_uint8s(Vel_desired) + data_buf[0:4] = Vel_desired_uint8s + self.__send_data(motorid, data_buf) + self.recv() # receive the data from serial port + + def control_pos_force(self, Motor, Pos_des: float, Vel_des, i_des): + """ + control the motor in EMIT control mode 电机力位混合模式 + :param Pos_des: desired position rad 期望位置 单位为rad + :param Vel_des: desired velocity rad/s 期望速度 为放大100倍 + :param i_des: desired current rang 0-10000 期望电流标幺值放大10000倍 + 电流标幺值：实际电流值除以最大电流值，最大电流见上电打印 + """ + if Motor.SlaveID not in self.motors_map: + print("control_pos_vel ERROR : Motor ID not found") + return + motorid = 0x300 + Motor.SlaveID + data_buf = np.array([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8) + Pos_desired_uint8s = float_to_uint8s(Pos_des) + data_buf[0:4] = Pos_desired_uint8s + Vel_uint = np.uint16(Vel_des) + ides_uint = np.uint16(i_des) + data_buf[4] = Vel_uint & 0xff + data_buf[5] = Vel_uint >> 8 + data_buf[6] = ides_uint & 0xff + data_buf[7] = ides_uint >> 8 + self.__send_data(motorid, data_buf) + self.recv() # receive the data from serial port + + def enable(self, Motor): + """ + enable motor 使能电机 + 最好在上电后几秒后再使能电机 + :param Motor: Motor object 电机对象 + """ + self.__control_cmd(Motor, np.uint8(0xFC)) + sleep(0.1) + self.recv() # receive the data from serial port + + def enable_old(self, Motor ,ControlMode): + """ + enable motor old firmware 使能电机旧版本固件，这个是为了旧版本电机固件的兼容性 + 可恶的旧版本固件使能需要加上偏移量 + 最好在上电后几秒后再使能电机 + :param Motor: Motor object 电机对象 + """ + data_buf = np.array([0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc], np.uint8) + enable_id = ((int(ControlMode)-1) << 2) + Motor.SlaveID + self.__send_data(enable_id, data_buf) + sleep(0.1) + self.recv() # receive the data from serial port + + def disable(self, Motor): + """ + disable motor 失能电机 + :param Motor: Motor object 电机对象 + """ + self.__control_cmd(Motor, np.uint8(0xFD)) + sleep(0.1) + self.recv() # receive the data from serial port + + def set_zero_position(self, Motor): + """ + set the zero position of the motor 设置电机0位 + :param Motor: Motor object 电机对象 + """ + self.__control_cmd(Motor, np.uint8(0xFE)) + sleep(0.1) + self.recv() # receive the data from serial port + + def recv(self): + # 把上次没有解析完的剩下的也放进来 + data_recv = b''.join([self.data_save, self.serial_.read_all()]) + packets = self.__extract_packets(data_recv) + for packet in packets: + data = packet[7:15] + CANID = (packet[6] << 24) | (packet[5] << 16) | (packet[4] << 8) | packet[3] + CMD = packet[1] + self.__process_packet(data, CANID, CMD) + + def recv_set_param_data(self): + data_recv = self.serial_.read_all() + packets = self.__extract_packets(data_recv) + for packet in packets: + data = packet[7:15] + CANID = (packet[6] << 24) | (packet[5] << 16) | (packet[4] << 8) | packet[3] + CMD = packet[1] + self.__process_set_param_packet(data, CANID, CMD) + + def __process_packet(self, data, CANID, CMD): + if CMD == 0x11: + if CANID != 0x00: + if CANID in self.motors_map: + q_uint = np.uint16((np.uint16(data[1]) << 8) | data[2]) + dq_uint = np.uint16((np.uint16(data[3]) << 4) | (data[4] >> 4)) + tau_uint = np.uint16(((data[4] & 0xf) << 8) | data[5]) + MotorType_recv = self.motors_map[CANID].MotorType + Q_MAX = self.Limit_Param[MotorType_recv][0] + DQ_MAX = self.Limit_Param[MotorType_recv][1] + TAU_MAX = self.Limit_Param[MotorType_recv][2] + recv_q = uint_to_float(q_uint, -Q_MAX, Q_MAX, 16) + recv_dq = uint_to_float(dq_uint, -DQ_MAX, DQ_MAX, 12) + recv_tau = uint_to_float(tau_uint, -TAU_MAX, TAU_MAX, 12) + self.motors_map[CANID].recv_data(recv_q, recv_dq, recv_tau) + else: + MasterID=data[0] & 0x0f + if MasterID in self.motors_map: + q_uint = np.uint16((np.uint16(data[1]) << 8) | data[2]) + dq_uint = np.uint16((np.uint16(data[3]) << 4) | (data[4] >> 4)) + tau_uint = np.uint16(((data[4] & 0xf) << 8) | data[5]) + MotorType_recv = self.motors_map[MasterID].MotorType + Q_MAX = self.Limit_Param[MotorType_recv][0] + DQ_MAX = self.Limit_Param[MotorType_recv][1] + TAU_MAX = self.Limit_Param[MotorType_recv][2] + recv_q = uint_to_float(q_uint, -Q_MAX, Q_MAX, 16) + recv_dq = uint_to_float(dq_uint, -DQ_MAX, DQ_MAX, 12) + recv_tau = uint_to_float(tau_uint, -TAU_MAX, TAU_MAX, 12) + self.motors_map[MasterID].recv_data(recv_q, recv_dq, recv_tau) + + + def __process_set_param_packet(self, data, CANID, CMD): + if CMD == 0x11 and (data[2] == 0x33 or data[2] == 0x55): + masterid=CANID + slaveId = ((data[1] << 8) | data[0]) + if CANID==0x00: #防止有人把MasterID设为0稳一手 + masterid=slaveId + + if masterid not in self.motors_map: + if slaveId not in self.motors_map: + return + else: + masterid=slaveId + + RID = data[3] + # 读取参数得到的数据 + if is_in_ranges(RID): + #uint32类型 + num = uint8s_to_uint32(data[4], data[5], data[6], data[7]) + self.motors_map[masterid].temp_param_dict[RID] = num + + else: + #float类型 + num = uint8s_to_float(data[4], data[5], data[6], data[7]) + self.motors_map[masterid].temp_param_dict[RID] = num + + + def addMotor(self, Motor): + """ + add motor to the motor control object 添加电机到电机控制对象 + :param Motor: Motor object 电机对象 + """ + self.motors_map[Motor.SlaveID] = Motor + if Motor.MasterID != 0: + self.motors_map[Motor.MasterID] = Motor + return True + + def __control_cmd(self, Motor, cmd: np.uint8): + data_buf = np.array([0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, cmd], np.uint8) + self.__send_data(Motor.SlaveID, data_buf) + + def __send_data(self, motor_id, data): + """ + send data to the motor 发送数据到电机 + :param motor_id: + :param data: + :return: + """ + self.send_data_frame[13] = motor_id & 0xff + self.send_data_frame[14] = (motor_id >> 8)& 0xff #id high 8 bits + self.send_data_frame[21:29] = data + self.serial_.write(bytes(self.send_data_frame.T)) + + def __read_RID_param(self, Motor, RID): + can_id_l = Motor.SlaveID & 0xff #id low 8 bits + can_id_h = (Motor.SlaveID >> 8)& 0xff #id high 8 bits + data_buf = np.array([np.uint8(can_id_l), np.uint8(can_id_h), 0x33, np.uint8(RID), 0x00, 0x00, 0x00, 0x00], np.uint8) + self.__send_data(0x7FF, data_buf) + + def __write_motor_param(self, Motor, RID, data): + can_id_l = Motor.SlaveID & 0xff #id low 8 bits + can_id_h = (Motor.SlaveID >> 8)& 0xff #id high 8 bits + data_buf = np.array([np.uint8(can_id_l), np.uint8(can_id_h), 0x55, np.uint8(RID), 0x00, 0x00, 0x00, 0x00], np.uint8) + if not is_in_ranges(RID): + # data is float + data_buf[4:8] = float_to_uint8s(data) + else: + # data is int + data_buf[4:8] = data_to_uint8s(int(data)) + self.__send_data(0x7FF, data_buf) + + def switchControlMode(self, Motor, ControlMode): + """ + switch the control mode of the motor 切换电机控制模式 + :param Motor: Motor object 电机对象 + :param ControlMode: Control_Type 电机控制模式 example:MIT:Control_Type.MIT MIT模式 + """ + max_retries = 20 + retry_interval = 0.1 #retry times + RID = 10 + self.__write_motor_param(Motor, RID, np.uint8(ControlMode)) + for _ in range(max_retries): + sleep(retry_interval) + self.recv_set_param_data() + if Motor.SlaveID in self.motors_map: + if RID in self.motors_map[Motor.SlaveID].temp_param_dict: + if abs(self.motors_map[Motor.SlaveID].temp_param_dict[RID] - ControlMode) < 0.1: + return True + else: + return False + return False + + def save_motor_param(self, Motor): + """ + save the all parameter to flash 保存所有电机参数 + :param Motor: Motor object 电机对象 + :return: + """ + can_id_l = Motor.SlaveID & 0xff #id low 8 bits + can_id_h = (Motor.SlaveID >> 8)& 0xff #id high 8 bits + data_buf = np.array([np.uint8(can_id_l), np.uint8(can_id_h), 0xAA, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8) + self.disable(Motor) # before save disable the motor + self.__send_data(0x7FF, data_buf) + sleep(0.001) + + def change_limit_param(self, Motor_Type, PMAX, VMAX, TMAX): + """ + change the PMAX VMAX TMAX of the motor 改变电机的PMAX VMAX TMAX + :param Motor_Type: + :param PMAX: 电机的PMAX + :param VMAX: 电机的VMAX + :param TMAX: 电机的TMAX + :return: + """ + self.Limit_Param[Motor_Type][0] = PMAX + self.Limit_Param[Motor_Type][1] = VMAX + self.Limit_Param[Motor_Type][2] = TMAX + + def refresh_motor_status(self,Motor): + """ + get the motor status 获得电机状态 + """ + can_id_l = Motor.SlaveID & 0xff #id low 8 bits + can_id_h = (Motor.SlaveID >> 8) & 0xff #id high 8 bits + data_buf = np.array([np.uint8(can_id_l), np.uint8(can_id_h), 0xCC, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8) + self.__send_data(0x7FF, data_buf) + self.recv() # receive the data from serial port + + def change_motor_param(self, Motor, RID, data): + """ + change the RID of the motor 改变电机的参数 + :param Motor: Motor object 电机对象 + :param RID: DM_variable 电机参数 + :param data: 电机参数的值 + :return: True or False ,True means success, False means fail + """ + max_retries = 20 + retry_interval = 0.05 #retry times + + self.__write_motor_param(Motor, RID, data) + for _ in range(max_retries): + self.recv_set_param_data() + if Motor.SlaveID in self.motors_map and RID in self.motors_map[Motor.SlaveID].temp_param_dict: + if abs(self.motors_map[Motor.SlaveID].temp_param_dict[RID] - data) < 0.1: + return True + else: + return False + sleep(retry_interval) + return False + + def read_motor_param(self, Motor, RID): + """ + read only the RID of the motor 读取电机的内部信息例如 版本号等 + :param Motor: Motor object 电机对象 + :param RID: DM_variable 电机参数 + :return: 电机参数的值 + """ + max_retries = 20 + retry_interval = 0.05 #retry times + self.__read_RID_param(Motor, RID) + for _ in range(max_retries): + sleep(retry_interval) + self.recv_set_param_data() + if Motor.SlaveID in self.motors_map: + if RID in self.motors_map[Motor.SlaveID].temp_param_dict: + return self.motors_map[Motor.SlaveID].temp_param_dict[RID] + return None + + # ------------------------------------------------- + # Extract packets from the serial data + def __extract_packets(self, data): + frames = [] + header = 0xAA + tail = 0x55 + frame_length = 16 + i = 0 + remainder_pos = 0 + + while i <= len(data) - frame_length: + if data[i] == header and data[i + frame_length - 1] == tail: + frame = data[i:i + frame_length] + frames.append(frame) + i += frame_length + remainder_pos = i + else: + i += 1 + self.data_save = data[remainder_pos:] + return frames + + +def LIMIT_MIN_MAX(x, min, max): + if x <= min: + x = min + elif x > max: + x = max + + +def float_to_uint(x: float, x_min: float, x_max: float, bits): + LIMIT_MIN_MAX(x, x_min, x_max) + span = x_max - x_min + data_norm = (x - x_min) / span + return np.uint16(data_norm * ((1 << bits) - 1)) + + +def uint_to_float(x: np.uint16, min: float, max: float, bits): + span = max - min + data_norm = float(x) / ((1 << bits) - 1) + temp = data_norm * span + min + return np.float32(temp) + + +def float_to_uint8s(value): + # Pack the float into 4 bytes + packed = pack('f', value) + # Unpack the bytes into four uint8 values + return unpack('4B', packed) + + +def data_to_uint8s(value): + # Check if the value is within the range of uint32 + if isinstance(value, int) and (0 <= value <= 0xFFFFFFFF): + # Pack the uint32 into 4 bytes + packed = pack('I', value) + else: + raise ValueError("Value must be an integer within the range of uint32") + + # Unpack the bytes into four uint8 values + return unpack('4B', packed) + + +def is_in_ranges(number): + """ + check if the number is in the range of uint32 + :param number: + :return: + """ + if (7 <= number <= 10) or (13 <= number <= 16) or (35 <= number <= 36): + return True + return False + + +def uint8s_to_uint32(byte1, byte2, byte3, byte4): + # Pack the four uint8 values into a single uint32 value in little-endian order + packed = pack('<4B', byte1, byte2, byte3, byte4) + # Unpack the packed bytes into a uint32 value + return unpack('iZ+GA6dGZhp2%$zuNR1^JAtcKzmOu~?XkgF*2^+-Vn;7*4~in0Bt~`;>j8RXw{~k&TiI;Nf8KvKwM!3lZIx5mO{%uM z-gtk%?>_o51ClmQD&5uJdE9f(J?A^$>wM>&&UJUE41AJzeDl!J-!P2-&P4OaMq&$o z<(~s6Lz$z-nCY4(Od-`QmXUohTB=_;#YN@iK%YY^L5+pF2qK9B(E2FeQE*ndeuU;2&MbfVs#zD z1!{>}ig2OoSIZDCQp?r#2p6jr>IQ_@sg>$RgiG+wv|5FCt}4V&#oVPx-=tO}y;{=! zNZ+h(LHZU+FH_cDBeSNCF&i1viS4?WM^vL=%&xiCq3LVm8A(%Qk-^ z%pU9|Ka5-WZNsnpMSu#X>6BSDvSt*sqF5}7*-Dd$C=) za;2bIE{vrwy>k)$K%Lsf3s=sczx2*?S6)AP<@^Vi-nlRkRo5m4wPPsL3 zJwBRcz8BlIbD!HYH2!#yyK7t(yxvM-Y$7Y@$*N+$lCeFzTo@hJ-RPO_Bko~O71kd*wtn{PH|yVfb-kJ#A1SWCTa6E|AKaA%qJm)E#BtBbW{ag_C7bO) z#qtsWBbhS8Z^HDyjycsM{a+Ur&%`|Qh-YEgF%0Cv(Wzd)lhN}$r!V|4 z&Y$oDD^-mV^dnC%D3LGo&H^)A}_uF z+~xPrM-?~eLS<5yHeI9ky!A4sVBwtUkG*ZMo#@Z*pew7;&oWush)*r>huZY?A2Zma zh0*bRv2y(CgRP(VLG8lp^AC3Z+WuhM>S3@l+siSC_Ic${_Xi!hZQh}VPe0~RkD`&N z$KkwU7LQC8u0GJ(Gp}6!!MUhf%dE~<*B@qEEew{5UE>frkGT3jSQMiRU zDD3COMH>xdaE5fvTKy5>~N)4**2yaq5AoCyexYV-7 z8?~q2n&0*;7a+ezv#M~kn1}u8C&NZAz4OPl)4$hNaca?CsRL6%SJK6DdVHc#8p?FQ zIP}}{Y+7tyN=%1xb}TpH#Sa{>6v|!)t(R;WwyT>gmb0wr#V2&JRPo~Z(eZM@vzZqN zJ-551b4|iWT4^6Q+eqt~U zVwS5Wbd51$9yN7;1#1@04`-tit6EA{ET`=0BvLV?5-N_AgF5zf2WrKu9m^oZmKoEj zD3wGC3yI*FgH#u4+tpO3QB7eTXis+|9j|sG-6`$$Af2doA>AeE-b!L73E>ElS4oUG zXRYbJYPagV-%wp!jB1~XZ8nhh<4MGORQLVNcO>4k#ZcY%8P%lJK&3HVP^TMl zC#aKmrA{B7^#*koAa2Zb2)6ppTBO~XR7AN8C$|EX5#_?>PlpssgFn4vvXmFQ z^U4Pw)LwtdSBfa*)9&mK&(_{MU3>Cr(oF?sMUEFT!$w%r4LWE zH*{ESN~aI2_Ut3bjjZDI?3o|To_VF+2KLu+ibW%H!wBZ<7TD+ia=G z0vj4H6{0>(-IDj$PDJ4LJ$oMAlMcs?HC<`Ek{%wPEU6(8rp3Pl%Ij3ibrQ5lt2zy; zI!%jnPDA2;L`+2NuUL_*>v}JMzLhcR{yLc_)9EkDC`0vU0K9HlIc6*4*-7xXtlPZK zM~Y)ub&1j9StZDYoYuMHUYtel_4@sZ4Co@8Ef-2k7CfHKwM}M$Zwoo$jBCnM{aJ$6 zrIVXHJRz?|L^ZuX{`u^#t-s+V2K_L(Z|fKQj3ZLWOR6B^V1E28M?}_v$>BEfJ+213lhjSd+<#N>@oY~yAZGo-xU$;F{hTcNU279nZ#8`=EYTEG@MzIh{ZlLzsmAiYxf_ zp0J=Nl8D0xDsf{bNiq#IM#I)|CRa8SX)xXc>{wd;*=CE2PD)^79FK^o4IEc^uN?OMZM zGp|ml(+{K{z&T6HSI1v1P~H16@jK4)c+d5 zOODZPR8)FDQf^;_C6TH3mfgN+{6S8;bnOkvI~doqXvg={IlhQZnG40(qfqU#rdG^L z@wu_9$dI^m2st_VAzl+vT1cJrwh5Zj zAmnKB!>3KO`|GD#5K74SAD8~7)`T;%fBPfXvOJ1(*|byHmw*6hHSuzC0^H7~F6b3QUu!Pz6A*b{F$i>;W->yCXly6~2L>h?008b4fnF!s(I;YYt z#&DqOu#bR8?d=59+Gyu!=y^rq_2sZ7pVF~Mabfm5AJkrZB-rdCu{FssGWE(YP7Z4wo-rd-pjS~-><*)grARk z^_O0|bn*9R&zwM)qD*)Rn{%$baq`kT&+zHfzf*gzu}GO(8`3LKm&LHJ%{!1(?Yu)R zEMi*U&)5!v2LUpvYfEkYTPTEmIhfXW-qR$pQw=uM2T;UK2D9VF1+&fU7|cJ7g-~=m zEX_vUjw0qkqV3XqLGzf0u|Ze(F(% zSnxVPHB#Ows~+z@I#&fP+C*t&7@5*Q0MvX`_Y-5}ABX{=uB&5Y0eX~WZ?d6VrPwm7K|vQX6z~?{j^oJ28}Q6nWhUCF4-q6xKts2lnvKw9AXJ>S;Ug}(a{-G zaJZ_DAf8%=yy-Y{J5eVQ2rl|eVy1EAX71Q$x`zH2y5K8ay`8Zg0QG*xL?v%!Y>;3r z!L0=3%S^(vN5+dKeJ^u10(g!tBK^S_}=^b;EK}|Q|N8pCXGcPVjM7-FxUF{o)A_o2zz4ATi))FXV2TXiB z+1TG`55&SczToOJG=VlSGf)m^4Ve{d&a6O1M6&{CxUywd;)o@JnSfJVIYAsQ^I%pm zKQb$!CyTSvFSF7)Bvw#kLUfr}zV?LJG7Fl#q>lo4w_XLyq)&111o+m+Sfx!2ERqu! zjiSW&|3Nt)pJLiX`1DWqG~*NQ&F~$p*y4eqhc#k={|QwI0yPg?&zt(^*mJNB$Mi3F zutfI4|2GXD{;DST8P1x5TvGwL4}D-P5TGZ2$**7LH0`PoooQ?1muSp*oT7 zLOK!bc`)4_zGWs+?W}fHyUBv3ze7LpCgp4~rhBSAEOXQ#9-2n=PdVzIf0j|>EzP42 z#7UH>2y4{V9F(YyKN^8|bNI3cWX#7#=5j<^tXIIRRXx}4K$ z;~cjyn{Ab6Hbz-yuMH7GDKl-}Y_5bbqANXgqA#(yQkd__)D10~x4rci-g5Agh_`6L z_`Ie6GN82BVXK@D;`U(+`4*Ik@DqgTVC9qwR+E+SCU~c+X;3g0`#^ZH^v4!P@{LTqoje zZ_~GG9dyDqBaAHW8wBe^p;v227Cf1F+v>$%vv3IE8A)=k{szH*f>w>}c4xBznzQKS zgB%k(XF!y+?TWM%Y#9wHfR$u4Y5PYWbrMmm$+rHQD7o11AWery9bQAzFex9cZPVw-#Rx%0Jw1He!w?)64{uGwLpR`ada_~W{W6Lg zu>a4RFbwF0$23U{D+L@XmoG@r9D!2;_C8h)3g!qM6heRrp8$OW{si76OjQ3BRw5uu z;i1nl_hAC@yl^$$?9%qdnUl|zFfrLtFN__Umr$VqWhH=PeZHOfScD>QUynIp&QWQt z*?s0Twv8v39Ow$tR|bWDjEo*43z@Ontlp~k_!(_ual%=_yf z-`z!h;M3q~#-1mbPdY5hW`&S(0FwM8_WhTLck%%Z58~ai7RIoL+G?-QQ?Z3mIRX5M z%yLl&op3{ysnJBf3yw}~}@ zO9&&d+(#VdRA^;KIaOHNRe0a%gF9^KuYjEGy*w=jFWZsZ2;pn{Z|G1Y1IpOb&mR(r z6KW*cYie;G)Sh~B_PJM^b<+I%1EM-Uj!p^$SFrx!i67iuXu1Uu9G`98noSpu<;EsP z3!AWev8lQ2I&Np+*VZNfh~6;9DyQ)F?ZC0ubldW7&h$52sa{;{9N(i#N!{dK??cjy z74p=y zk+5Ob(6FI1jjP9n&|L`&EAlo6icDiu6D^huJ-3nIR7gYu0k&t8K%j`^+7VvdKyX5U z%9*xykx8GfI9tS-j-7meqZ8g;*tC&``zGI;4n92RCv*mPM_QOo4nI@h4Ugu^htiim z{9f%xFU~%FV)jgUKc@8*^e1&yg=eOsF8FTLwdj?;g+Rm)CC|ZMp8MOM?K-gg{vk~1 zeByna#VKioAkH;8OZ7E{jMv9uH%l1sq<=Qr!0GHBbFFV(wJuoN>$Gf4ax!E1iEvqm zh;MN4_`9NEDyD{{Xr9PIL{c~eNMVqPPyP>7ig1!YM0;K)W;aelrS}u;BWMu;v)_5Q zcKStM1i(?=S~O~`d4}XVPm}fGACO_iO#yoc?X=fs51NWF*|MHQ|?s+y}>Dg?xomUt7=*o`D$u$|$vz$;$0HcFz)2SP! zy?_+WLa+ZeDFdr&Ck3w zd*WnNzQH{8zd$<;<{5(BGat_gy9}}DO9ZV3p|@BUhIOHQDBunM{Vx)ilvIZ8GY1wP zt6K0mU#&UD4_QRm;{+nmy*6|&jXdgI9D@^$Yf{Vl8W&HjK+}?WZ0J=jOht1E-lUN@ zbFi0Be^!t~|5r68nl=VaJKC&iVG6}51&^!@G;TMB72&ao;b87vlgrw#kN7Mk`1k}` z7H|1DcT@bvWy#QTdxbk@C*F&?9}3mS1PB=lX?24Zx0^% znZvo_Xw--(=U{*H-2Jkf00r&n-^P6D-yxVs8Zhcs1ouw=8ZT_17rP4S@zUsV z5$kiP%)i1E$?}@qce?c9+1ZnCUi#qK+Us9S2OF!W-@g2#r=wPzh5N6c`~s-Zb;?ka8BICq|O?86AU$VRk$bh1mo5CQkePu z2J+~p^z$TFc3PaOHDgvCx*u5f)7(c%Oec@@bG^nCVmK4543_3qlqxTIV;wApk}TG? zqIbU3r!K|a*Vrg7Y$S3MxTB$DQws|?v4`mZa1-J&D0_)RpvHps>^fAZ=#y zoMwhHt96>=T19XZ!RH7bCU}J4SpwP%O&8+i!0vA{@5=-^0-DvDY_G=%#9%zam}o$u z!N-~8{-u6`;1Gc*$=_p4WZ#R7y-e_Zf*FEW30@;OO3*-!#1=7cHNXS-l?MTM8 z|6tqw)=F#PFYScWy<~NNPv3P;Ur)c2a=M)kd{g+{=qz_qeYVr(B=EZqPr8x6#91sY zwurYFS|&S*UM1qIAV&~lPt}|@N?k}$%?z`3xq3Gqtm?$N>Ov%-Mh(|4BpvQn%77TAdB* zAHd-x&Xu+S&RJd@Yvaj+=kHUupDz)|&)>5PENxgROLDIM`Yk9$gyBk^rPr|=Hu?ra zT?v|hDUFnJ{DYi5N$K)x|PB~+$HA42prqx8D4Q4(5mA`2zEu@MHBF(ri$fa2}W)yU+~>l z#DJHQ7Uht(U_)O-Zu=yB8+pHsCc(PN>av`eW^M}>*x_grD7u=)zL;Y#50>bO!?**C ztW|cD3-dV0-J%k}(?fzMo33i*VDdVq9uq?#f2fc@oGuQ_Sg~1i0HepHzxf9*H3GB2 zFYB)dLmN__`}_78bXY%0uoa-bn&4)DeLDml{7VtPg?8+mL?H(R7#w9Agz(eMYhf)S zn<8Qxexfye0`->zr?6osWNM!RRqzsd#vb}bZ1ctkrmW37eT{bk70=;sTzGNuPCwY- z2Y30wM*TjDw0G`n$XmqD`NZ32Y3BSRyQcp=+CbNa8(q`?VV;+tLf-P0mouk@X7IFO zeWa1_|AM7z3_{?5c|k3(8n9_0P8}aJ^ER*-b;TY!2-f>j7mj`4A6;Itzil0%%fWPG z0~`FcG;W+&>joe_fyg#&d8y5##d0M-J~naRP3W1{XmsiZxe%H?R5-S7Gya|di;epb z3CsP$OydI0XHZaA01y}3BZcC)iw|xsx4)z#YHwA98yE`uKcRYyWk%dk6A8_b6sJ<4 z7vSYF;K7EBIgSe{{Qjmvg*PBj)E*oRe}?iHYMh46F%M_tCdaaQc)8|uX91SsS$TYb zo;sKf14DO_Ifba5XP*VVf*J3U@^c!f^9APpN&^94<(3tIPuWp|`b16Dk&b@~8? zaC{)moK*xj0pNPwu8nuyIpC)JFuU>I{jTI~4DvP(gn9Q24BW$K_cp@MxVYbkL)>=` zJmMyU7$XnebvN2%*z1hjXQ`{+9b4&=KE@vx;mEs3{U_6 zv5Y?i=)s=>^x}^I`tT5ib5O#-;!3+@)#>F8MFT1^<3r?q7zB{mXHw z|9V{LUxCZ~H{c@wN;j=;#0CB|F7K~$SF4-co7HOf7Im|`M&07xs@Ax-saxHQy3Jjy zGVZ6;TK9JKDfbR_ySq-^;jUNfFw^V3&ZhsU_(b!p{|gY{-=+U6#_l5!dHob)+X=Q3 zd?Zg;z))tOjTl|$3;VO8b6w}*ePQg;Mr>P<7u&jRn;RS4zRgX- zD$VZQJ?JKO?cJB%iE#VgZ48t9cJA8#=oj|69ozQpc_h2*(ZTJmJ%~LD7ypA*(3N7& zO&oe0XZ*FBDEpzccgT$m?cVKLd&;i$$X?gpF*>Icj5<)HYUlKDrz@2ksWjGW8;&Rd70{G=;mfJm5a#Lm})*o z`($~V@3}f#uE5i)vn5DAp?gq7jQTcdMpu8D&mvK!dy#aLyC2<~#jPecMv&Skx3aUi z`Y|WSLCtwhi@)4X8h)=<_hzRtoLxiKU9ueM^sZs?KE&z4_r9@In#0HcqBV`IV;4v+qP@HzhW5@XQ>Iy;0{y%Sm>z$@N}dC4@75a7*c<&R|8U%%%-UPaxkvMR_p^n$&HbH^ z?5&mD<5g#FDZhH>$;hZqYm?8wVNXps>!0MdvxQp^_jhI>!`Yb4uVwbu=G+>W0@Ng* zh4e_m=ud>TzP6C$_L1H_2NQ(egW9Kh{2x}IZ5(Hn3_^z<$-X%w^-A`_WP=N>zbh@;>Bd+ecgman&VT{E4r#0 zmQN2WS~zaBp0V(dg@-LJ7J}Uly90J7>@JzR6p^{h5!oM44yt9R_$3SXSh!dAGDc=G(%B}9?#tZ=Pot=}Q{D2qe%Y1n_mSQR5C2d4 zDh8v8K{Xg13ObpY!ouyKl1yCHf?Z0YKNtzeBFR|TRD;^UbJvC~L5p8kbwhpC19ip) zI8d1fdw2<#V}Yx}0uJykxP=qI5-_zzUchcNK%?OG;~EZhjst%S_F_LTVm~Khk<*(2 iua(p3UlY?A$74qm1T5kC!Gw~GsV~rP$)I1P!2b=Q6nZ}Z literal 0 HcmV?d00001 diff --git a/calculate/__pycache__/trajectory.cpython-39.pyc b/calculate/__pycache__/trajectory.cpython-39.pyc new file mode 100644 index 0000000000000000000000000000000000000000..2d3166a8e88b69df23623b36f15bef5a2be70e12 GIT binary patch literal 2209 zcmaJ?-D@0G6u?*0AFIa3@T z&oEqfjl+Q!dF# zIR(v<=VVsSK(pnnoV&(WQ;R)yC#pz2;VZFSLu*a>c>iT&uXAAKEQF?=zLftN(chqOe}ubPDUu!tw4_^zB|j zHfkGg;aOR`TqqSQrADRbI|W}AH=L4RQ>~}#t#GR3s#3*S8(0Y)FAc#&md|3SS?K#h z5pnuJfVtVzBl`G*5xxX(58)Ho8UauTni}bO)<;;tMn0Mu$;3Cn*@$tz|Mf5Tf4w)1 zvqHA9yig;wH_AZ(YI%N9`Jv%drJ95(Auduhaaac3pORQ}LmgMg;wJe1+0xQd;soA!r)lOC4gx7}mZJBDrLJ^V^=Sftm z`T{IzM7$?c+H3c`^9)ZIc*ndHq-k$@$eRkp4iD^I&XH5lQlwp(4n$yRU`HB+PVV^j zcgJ5I4vMxMpYx9uMU6wzNuz%5C(b`tqAl2yRd*#XLQA1Q9h7Okj%>@PL(_Am6SL4K zAmpw(j|W5E40-Ex0#12X8bvkC#v)?9MVqp(u4=&pAFC1DIHSw>3k@wNr#gl6uGw(SG8KrEsDvg|5vr)VX`BAZ1F&0sZ+ z^h_EO2X7GTx$Z}|IzMh79bS*y!{5Jcb$|c7^TGE=2RDumZgoEX zs=Ir$^U2ShPk+kk?d8R4y43Le+U8obul+Y2laRr`fqthR?Su*SJY?R`u*bg!Dij#z zFsXSGl)MQs>R~eENa!%3r&u@CQ)KTK8%oE*f3Mz(J30M*bmLVQNTc&ALP&352y6|z b?&Im@&6;dfoEPbp-WWt$WKw#}WXAsmgOb}{ literal 0 HcmV?d00001 diff --git a/calculate/angle_A.txt b/calculate/angle_A.txt new file mode 100644 index 0000000..6c53205 --- /dev/null +++ b/calculate/angle_A.txt @@ -0,0 +1 @@ +2.25,2.24,2.23,2.22,2.21,2.20,2.19,2.19,2.18,2.17,2.16,2.15,2.14,2.12,2.11,2.10,2.09,2.08,2.07,2.05 \ No newline at end of file diff --git a/calculate/angle_B.txt b/calculate/angle_B.txt new file mode 100644 index 0000000..84d3363 --- /dev/null +++ b/calculate/angle_B.txt @@ -0,0 +1 @@ +0.90,0.90,0.91,0.92,0.93,0.94,0.95,0.96,0.97,0.98,0.99,1.00,1.01,1.02,1.03,1.04,1.05,1.06,1.08,1.09 \ No newline at end of file diff --git a/calculate/calculate_angle.py b/calculate/calculate_angle.py new file mode 100644 index 0000000..48acfb2 --- /dev/null +++ b/calculate/calculate_angle.py @@ -0,0 +1,179 @@ +# qt_main.py + +import numpy as np +import matplotlib.pyplot as plt +from ik import inverseF # 假设这是你自己的逆运动学函数 +from matplotlib.animation import FuncAnimation + +# 设置中文字体和解决负号显示问题 +plt.rcParams['font.sans-serif'] = ['SimHei', 'WenQuanYi Zen Hei', 'FangSong'] # 按优先级选择字体 +plt.rcParams['axes.unicode_minus'] = False # 显示负号 - + + +# 杆长参数 +L1 = 250 +L2 = 300 +L3 = 300 +L4 = 250 +L0 = 250 + + +# 1. 轨迹生成函数：每个轨迹类型独立封装，支持外部传参 +# -------------------------------------------------- + +def generate_circle(center=(100, 300), radius=40): + from trajectory import circle_trajectory + return circle_trajectory(center=center, radius=radius) + +def generate_line(start=(125, 300), end=(125, 400)): + from trajectory import line_trajectory + return line_trajectory(start=start, end=end) + +def generate_ellipse(center=(100, 200), rx=50, ry=25): + from trajectory import ellipse_trajectory + return ellipse_trajectory(center=center, rx=rx, ry=ry) + +def generate_square(side=60, start_point=(100, 200)): + from trajectory import square_trajectory + return square_trajectory(side=side, start_point=start_point) + +def generate_triangle(base_length=100, height=80, base_center=(100, 200)): + from trajectory import triangle_trajectory + return triangle_trajectory(base_length=base_length, height=height, base_center=base_center) + + +# 4. 主函数：根据轨迹类型调用对应函数并执行 +# -------------------------------------------------- + + +def main_of_5dof(trajectory_type='line', show_animation=True, save_angle_a='angle_A.txt', + save_angle_b='angle_B.txt', **kwargs): + """ + 主函数：生成轨迹、计算逆解，并将 theta1 和 theta4 分别保存为两个 txt 文件，逗号分隔 + 参数: + - trajectory_type: 轨迹类型 ('circle', 'line', 'ellipse', 'square', 'triangle') + - show_animation: 是否显示动画 + - save_angle_a: 保存 theta1（A角）的文件名，设为 None 不保存 + - save_angle_b: 保存 theta4（B角）的文件名，设为 None 不保存 + - **kwargs: 传递给轨迹生成函数的参数 + """ + # 生成轨迹 + if trajectory_type == 'circle': + x_list, y_list = generate_circle( + center=kwargs.get('center', (100, 300)), + radius=kwargs.get('radius', 40) + ) + elif trajectory_type == 'line': + x_list, y_list = generate_line( + start=kwargs.get('start', (125, 300)), + end=kwargs.get('end', (125, 400)) + ) + elif trajectory_type == 'ellipse': + x_list, y_list = generate_ellipse( + center=kwargs.get('center', (100, 200)), + rx=kwargs.get('rx', 50), + ry=kwargs.get('ry', 25) + ) + elif trajectory_type == 'square': + x_list, y_list = generate_square( + side=kwargs.get('side', 60), + start_point=kwargs.get('start_point', (100, 200)) + ) + elif trajectory_type == 'triangle': + x_list, y_list = generate_triangle( + base_length=kwargs.get('base_length', 100), + height=kwargs.get('height', 80), + base_center=kwargs.get('base_center', (100, 200)) + ) + else: + raise ValueError(f"不支持的轨迹类型: {trajectory_type}") + + # 存储角度值的列表 + angle_A_list = [] # theta1 (弧度或角度) + angle_B_list = [] # theta4 (弧度或角度) + + # 计算每个点的逆运动学并存储角度（以角度制保存） + 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) + angle_A_list.append(theta1) + angle_B_list.append(theta4) + print(f"第 {i} 个点: A角 = {theta1:.2f}°, B角 = {theta4:.2f}°") + except Exception as e: + print(f"第 {i} 点逆解失败: {e}") + # 可选择插入 NaN 或上一个有效值 + angle_A_list.append(np.nan) + angle_B_list.append(np.nan) + + # ==================== 保存为两个独立的 txt 文件 ==================== + if save_angle_a: + with open(save_angle_a, 'w') as f: + # 将所有 A 角度转为字符串，保留2位小数，用逗号连接 + formatted = ",".join([f"{angle:.2f}" for angle in angle_A_list]) + f.write(formatted) + print(f"\n✅ A角度（theta1）已保存至: {save_angle_a}") + + if save_angle_b: + with open(save_angle_b, 'w') as f: + formatted = ",".join([f"{angle:.2f}" for angle in angle_B_list]) + f.write(formatted) + print(f"✅ B角度（theta4）已保存至: {save_angle_b}") + + # ==================== 可选：显示动画 ==================== + if show_animation: + fig, ax = plt.subplots() + ax.set_xlim(-300, 500) + ax.set_ylim(0, 500) + ax.set_aspect('equal') + ax.grid(True) + ax.set_title("五连杆末端沿轨迹运动") + ax.plot(x_list, y_list, 'b--', label='理想轨迹') + line, = ax.plot([], [], 'r-o', linewidth=2, markersize=6, label='五连杆结构') + + def draw_frame(i): + x = x_list[i] + y = y_list[i] + + try: + theta1, theta4 = inverseF(x, y, L1, L2, L3, L4, L0) + except Exception as e: + print(f"第 {i} 帧: 计算失败 -> {e}") + theta1 = theta4 = None + + if theta1 is None or theta4 is None: + line.set_data([], []) + return line, + + # 计算连杆坐标 + x2 = L1 * np.cos(theta1) + y2 = L1 * np.sin(theta1) + x4 = L4 * np.cos(theta4) + L0 + 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) + return line, + + ani = FuncAnimation(fig, draw_frame, frames=len(x_list), interval=50, blit=True) + plt.legend() + plt.show() + +# 📌 运行主函数 +if __name__ == "__main__": + main_of_5dof(trajectory_type='line',start=(125, 300), end=(125, 400), show_animation=False) + #main_of_5dof( + # trajectory_type='circle', + # center=(150, 250), + # radius=60, + # show_animation=False # 设置为 False 则不显示动画 + #) + + # 示例：其他轨迹使用方式 + # main_of_5dof(trajectory_type='line', start=(0, 0), end=(200, 300), show_animation=False) + # main_of_5dof(trajectory_type='ellipse', center=(100, 200), rx=80, ry=40) + # main_of_5dof(trajectory_type='square', side=100, start_point=(100, 200)) + # main_of_5dof(trajectory_type='triangle', base_length=120, height=100, base_center=(100, 200)) \ No newline at end of file diff --git a/calculate/fk.py b/calculate/fk.py new file mode 100644 index 0000000..07200bf --- /dev/null +++ b/calculate/fk.py @@ -0,0 +1,40 @@ +import numpy as np + + +def forwardF(u1, u4, omega1, omega4, l1, l2, l3, l4, l5, alpha1, alpha4): + # 位置分析 + xb = l1 * np.cos(u1) + yb = l1 * np.sin(u1) + xd = l5 + l4 * np.cos(u4) + yd = l4 * np.sin(u4) + lbd = np.sqrt((xd - xb) ** 2 + (yd - yb) ** 2) + A0 = 2 * l2 * (xd - xb) + B0 = 2 * l2 * (yd - yb) + C0 = l2 ** 2 + lbd ** 2 - l3 ** 2 + u2 = 2 * np.arctan((B0 + np.sqrt(A0 ** 2 + B0 ** 2 - C0 ** 2)) / (A0 + C0)) + xc = xb + l2 * np.cos(u2) + yc = yb + l2 * np.sin(u2) + u3 = np.arctan2((yc - yd), (xc - xd)) + np.pi + + # 速度分析 + A = np.array([[l2 * np.sin(u2), -l3 * np.sin(u3)], + [l2 * np.cos(u2), -l3 * np.cos(u3)]]) + B = np.array([-l1 * omega1 * np.sin(u1) + l4 * omega4 * np.sin(u4), + -l1 * omega1 * np.cos(u1) + l4 * omega4 * np.cos(u4)]) + omega = np.linalg.solve(A, B) + omega2, omega3 = omega[0], omega[1] + + # 加速度分析 + Aa = np.array([[l2 * np.sin(u2), -l3 * np.sin(u3)], + [l2 * np.cos(u2), -l3 * np.cos(u3)]]) + Ba = np.array([l3 * np.cos(u3) * omega3 ** 2 - l2 * np.cos(u2) * omega2 ** 2 + l4 * np.cos( + u4) * omega4 ** 2 + l4 * np.sin(u4) * alpha4 - l1 * np.cos(u1) * omega1 ** 2 - l1 * np.sin(u1) * alpha1, + -l3 * np.sin(u3) * omega3 ** 2 + l2 * np.sin(u2) * omega2 ** 2 - l4 * np.sin( + u4) * omega4 ** 2 + l4 * np.cos(u4) * alpha4 + l1 * np.sin(u1) * omega1 ** 2 - l1 * np.cos( + u1) * alpha1]) + alpha = np.linalg.solve(Aa, Ba) + + return xc, yc, u2, u3, omega2, omega3, alpha[0], alpha[1] + +# 示例调用 +# xc, yc, u2, u3, omega2, omega3, alpha2, alpha3 = five(np.pi/6, np.pi/4, 1, 2, 1, 2, 3, 4, 5, 0.1, 0.2) \ No newline at end of file diff --git a/calculate/ik.py b/calculate/ik.py new file mode 100644 index 0000000..6046976 --- /dev/null +++ b/calculate/ik.py @@ -0,0 +1,69 @@ +import math + + +def inverseF(x, y, l1, l2, l3, l4, l5): + """ + 五连杆机构逆向运动学函数（Python 实现） + + 输入： + x, y: 末端执行器坐标 + l1~l5: 各杆长度 + + 输出： + theta1, theta2: 两个主动关节角度（弧度） + """ + Xc = x + Yc = y + + # ===== 左侧链路（l1, l2）===== + numerator_left = Xc ** 2 + Yc ** 2 - l1 ** 2 - l2 ** 2 + denominator_left = 2 * l1 * l2 + cosfoai_12 = numerator_left / denominator_left + + if cosfoai_12 > 1 or cosfoai_12 < -1: + raise ValueError("目标点超出工作空间！左侧无解。") + foai_12 = 2 * math.pi - math.acos(cosfoai_12) + + # 求第一个电机角度 foai_01 + numerator_foai01 = l2 * Yc * math.sin(foai_12) + Xc * (l2 * math.cos(foai_12) + l1) + denominator_foai01 = (l2 * math.cos(foai_12) + l1) ** 2 + (l2 * math.sin(foai_12)) ** 2 + + if denominator_foai01 == 0: + raise ZeroDivisionError("分母为零，无法计算左侧角度。") + + cosfoai_01 = numerator_foai01 / denominator_foai01 + if cosfoai_01 > 1 or cosfoai_01 < -1: + raise ValueError("cosfoai_01 超出 [-1, 1]，左侧无解。") + foai_01 = math.acos(cosfoai_01) + + # ===== 右侧链路（l3, l4）===== + Xc_shifted = Xc - l5 + numerator_right = Xc_shifted ** 2 + Yc ** 2 - l3 ** 2 - l4 ** 2 + denominator_right = 2 * l3 * l4 + cosfoai_34 = numerator_right / denominator_right + + if cosfoai_34 > 1 or cosfoai_34 < -1: + raise ValueError("目标点超出工作空间！右侧无解。") + foai_34 = 2 * math.pi - math.acos(cosfoai_34) + + A = l5 - Xc + B = l3 * math.sin(foai_34) + C = l4 + l3 * math.cos(foai_34) + + if B == 0 and C == 0: + raise ZeroDivisionError("B 和 C 均为零，无法计算右侧角度。") + + try: + foai_t = math.acos(B / math.sqrt(B ** 2 + C ** 2)) + foai_40 = foai_t - math.asin(A / math.sqrt(B ** 2 + C ** 2)) + except: + raise ValueError("右侧三角函数计算失败，请检查输入是否合法。") + + # 转换为角度再转回弧度 + theta1_deg = math.degrees(foai_01) + theta2_deg = 180 - math.degrees(foai_40) + + theta1 = math.radians(theta1_deg) + theta2 = math.radians(theta2_deg) + + return theta1, theta2 \ No newline at end of file diff --git a/calculate/test_ik.py b/calculate/test_ik.py new file mode 100644 index 0000000..5e3ee00 --- /dev/null +++ b/calculate/test_ik.py @@ -0,0 +1,64 @@ +import math +from ik import inverseF +from 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 +x = 125 +#y = 382.338 +y = 300 +omega1 = omega4 = 500 +alpha1 = alpha4 = 0 + +# 逆解 +u1, u4 = inverseF(x, y, l1, l2, l3, l4, l5) + +# 正解验证 +xc, yc, u2, u3, omega, alpha, _, _ = forwardF(u1, u4, omega1, omega4, l1, l2, l3, l4, l5, alpha1, alpha4) + +# 左侧链路 +x1, y1 = 0, 0 +x2 = l1 * math.cos(u1) +y2 = l1 * math.sin(u1) + +# 右侧链路 +x5, y5 = l5, 0 +x4 = l4 * math.cos(u4)+l5 # 注意方向 +y4 = l4 * math.sin(u4) + +# 绘图 +plt.figure(figsize=(8, 8)) + +# 左侧链路 +plt.plot([x1, x2, xc], [y1, y2, yc], 'b-o', label='左侧链路') + +# 右侧链路 +plt.plot([x5, x4, xc], [y5, y4, yc], 'r-o', label='右侧链路') + +# 标记关键点 +plt.plot(x1, y1, 'ro') # O1 +plt.plot(x5, y5, 'go') # O2 +plt.plot(x2, y2, 'yo') # B +plt.plot(x4, y4, 'mo') # D +plt.plot(xc, yc, 'ko', markersize=10) # C（末端） + +# 设置图形 +plt.grid(True) +plt.axis('equal') +plt.xlim([-200, l5 + 200]) +plt.ylim([-200, 600]) +plt.title('SCARA 五连杆逆解结构图') +plt.xlabel('X (mm)') +plt.ylabel('Y (mm)') +plt.legend() +plt.show() \ No newline at end of file diff --git a/calculate/traj_fk.py b/calculate/traj_fk.py new file mode 100644 index 0000000..d12693f --- /dev/null +++ b/calculate/traj_fk.py @@ -0,0 +1,62 @@ +import numpy as np + +from 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 = 50 +l2 = 50 +l3 = 50 +l4 = 50 +l5 = 50 + +# 初始角度（弧度） +u1_base = np.deg2rad(120) # 左侧电机初始角 +u4_base = np.deg2rad(120) # 右侧电机初始角 + +# 设置绘图区域 +plt.figure(figsize=(8, 8)) +ax = plt.gca() +ax.set_xlim(-100, l5 + 100) +ax.set_ylim(-100, 100) +ax.set_aspect('equal') +ax.grid(True) +ax.set_title("五连杆机构运动仿真（调用FK函数）") + +for i in range(1, 61): + # 更新两个驱动臂的角度 + angle1 = u1_base - np.deg2rad(1.75 * i) # 左侧角度变化 + angle4 = u4_base + np.deg2rad(1.75 * i) # 右侧角度变化 + + #正向运动学函数获取末端位置和中间角度 + result = forwardF(angle1, angle4, 0, 0, l1, l2, l3, l4, l5, 0, 0) + xc, yc, u2, u3 = result[:4] + + # 构建各点坐标 + x = [0, l1*np.cos(angle1), xc, l4*np.cos(angle4)+l5, l5] + y = [0, l1*np.sin(angle1), yc, l4*np.sin(angle4), 0] + + # 清除上一帧并绘制新图形 + ax.cla() + ax.set_xlim(-100, l5 + 100) + ax.set_ylim(-100, 100) + ax.set_aspect('equal') + ax.grid(True) + ax.set_title("五连杆机构运动仿真（调用FK函数）") + + # 绘制结构线和关键点 + ax.plot(x, y, 'r-o', linewidth=2, markersize=6, markerfacecolor='red') + ax.plot(x[0], y[0], 'go') # 原点 + ax.plot(x[1], y[1], 'bo') # 第二个点 + ax.plot(x[2], y[2], 'mo') # 中间点 + ax.plot(x[3], y[3], 'co') # 第四个点 + ax.plot(x[4], y[4], 'yo') # 最后一个点 + + plt.pause(0.1) + +plt.close() \ No newline at end of file diff --git a/calculate/traj_main.py b/calculate/traj_main.py new file mode 100644 index 0000000..5df2cfb --- /dev/null +++ b/calculate/traj_main.py @@ -0,0 +1,71 @@ +# main_animation.py +import numpy as np +import matplotlib.pyplot as plt +from ik import inverseF +from trajectory import circle_trajectory, line_trajectory, ellipse_trajectory, square_trajectory, triangle_trajectory +from matplotlib.animation import FuncAnimation + +# 设置中文字体和解决负号显示问题 +plt.rcParams['font.sans-serif'] = ['SimHei', 'WenQuanYi Zen Hei', 'FangSong'] # 按优先级选择字体 +plt.rcParams['axes.unicode_minus'] = False # 显示负号 - + +# 杆长参数 +L1 = 250 +L2 = 300 +L3 = 300 +L4 = 250 +L0 = 250 + +# 设置绘图区域 +fig, ax = plt.subplots() +ax.set_xlim(-300, 500) +ax.set_ylim(0, 500) +ax.set_aspect('equal') +ax.grid(True) +ax.set_title("五连杆末端沿轨迹运动") + +line, = ax.plot([], [], 'r-o', linewidth=2, markersize=6) +# 选择轨迹类型： +TRAJECTORY_TYPE = 'ellipse' # 可选: circle, line, ellipse, square, triangle +if TRAJECTORY_TYPE == 'line': + x_list, y_list = circle_trajectory(center=(100, 300), radius=40) +elif TRAJECTORY_TYPE == 'line': + x_list, y_list = line_trajectory(start=(125, 300), end=(125, 400)) +elif TRAJECTORY_TYPE == 'ellipse': + x_list, y_list = ellipse_trajectory(center=(100, 200), rx=50, ry=25) +elif TRAJECTORY_TYPE == 'square': + x_list, y_list = square_trajectory(side=60) +elif TRAJECTORY_TYPE == 'triangle': + x_list, y_list = triangle_trajectory(base_length=100, height=80) +else: + raise ValueError("未知的轨迹类型，请选择 circle / line / ellipse / square / triangle") + +# 动画函数 +def draw_frame(i): + x = x_list[i] + y = y_list[i] + + try: + theta1, theta4 = inverseF(x, y, L1, L2, L3, L4, L0) + print(theta1) + print(theta4) + # 左侧电机臂末端 + x2 = L1 * np.cos(theta1) + y2 = L1 * np.sin(theta1) + # 右侧电机臂末端 + x4 = L4 * np.cos(theta4)+L0 + 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) + except Exception as e: + print(f"第 {i} 帧跳过，错误: {e}") + line.set_data([], []) + + return line, + +# 创建动画 +ani = FuncAnimation(fig, draw_frame, frames=len(x_list), interval=100, repeat=True) + +plt.show() \ No newline at end of file diff --git a/calculate/trajectory.py b/calculate/trajectory.py new file mode 100644 index 0000000..160c21c --- /dev/null +++ b/calculate/trajectory.py @@ -0,0 +1,62 @@ +# trajectory.py + +import numpy as np + +def circle_trajectory(center=(80, 0), radius=40, num_points=60): + """ 圆形轨迹 """ + 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) + return x_list, y_list + +def line_trajectory(start=(40, 0), end=(120, 0), num_points=20): + """ 直线轨迹 """ + t = np.linspace(0, 1, num_points) + x_list = start[0] + t * (end[0] - start[0]) + y_list = start[1] + t * (end[1] - start[1]) + return x_list, y_list + +def ellipse_trajectory(center=(80, 0), rx=50, ry=25, num_points=60): + """ 椭圆轨迹 """ + angles = np.linspace(0, 2 * np.pi, num_points) + x_list = center[0] + rx * np.cos(angles) + y_list = center[1] + ry * np.sin(angles) + return x_list, y_list + +def square_trajectory(side=60, num_points=60): + """ 正方形轨迹 """ + x_list, y_list = [], [] + for i in range(num_points): + t = i / num_points + if t < 0.25: + x = 80 + 60 * t * 4 + y = 0 + elif t < 0.5: + x = 140 + y = 0 + 60 * (t - 0.25) * 4 + elif t < 0.75: + x = 140 - 60 * (t - 0.5) * 4 + y = 60 + else: + x = 80 + y = 60 - 60 * (t - 0.75) * 4 + x_list.append(x) + y_list.append(y) + return x_list, y_list + +def triangle_trajectory(base_length=100, height=80, num_points=60): + """ 三角形轨迹 """ + x_list, y_list = [], [] + points = [(80, 0), (130, 80), (30, 80), (80, 0)] + for i in range(num_points): + idx = int(i / num_points * 3) + t = (i % (num_points // 3)) / (num_points // 3) + x = points[idx][0] + t * (points[idx+1][0] - points[idx][0]) + y = points[idx][1] + t * (points[idx+1][1] - points[idx][1]) + x_list.append(x) + y_list.append(y) + return x_list, y_list + +def custom_trajectory(custom_x, custom_y): + """ 自定义轨迹，输入两个列表即可 """ + return custom_x, custom_y \ No newline at end of file diff --git a/can_test_motor/__pycache__/can_main.cpython-38.pyc b/can_test_motor/__pycache__/can_main.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..d83a18456b88b0f4e3824996d2d7fb5e01c60833 GIT binary patch literal 2930 zcmcJRTW=Fb6o6;;=C#QM!kylhLMd*mB%mUs2qP#oQXiU^(n1{t%Xl`#!QM@0cc~j~ zP`HN@b8A{ELR!)S0aPeR5Va6msXt?1%i8f1PrUHdb9NmkQ2K(@P4?_u&di)SbG|wL zw7xz@;Hirp=yB@^`5P;99~CNF;I(#xU?fQxWkQZ7Y48=20(`}sn3s|gC2T2+F!cyc z%4`{nv3XEZ*mBm$=0ho>t8BqOfhY9{lUNg5$QCh~EoMuK3XBv!A@LRcTVt!VEjzM# zU4y4hTi?2VL(9gN_3N;Ct7G?=9GiOzSZ~kfs%n|Ek;`Ip;S01O7*fNt`%H&arDJ*hHVJeFrAuicY4%2Zu zN?2@6J`8#=A~H@W*-4C6iP-AC7(YeH0Jx*S3FesRDx+jfK+TK!yK2ENk#(`PG#2e)nQ@PC^K8ru5OJe}8A+D^A=8~vN-r@uZ_zHr1JIzK&r zZLS7-FU#6A7^{7>{h6Mgv|;GEu&Xk7p>pt>x!qaA)_He2qqk|L!3X90f0pk&@P9ZN zs9YDkNG2TaMQe)fanWl?^F3J`4hByo%RC7!Og3b#lwPoPgXs)fohH~<=iEJYfo%y- zY+b+WNfi3D(zmxu#aoHar^0($1`l-R5uku4Y2mv9giE@b1}GzNB6L(56Fvive<5JN zT^Ex=Yk|)LrH<ulY!Q+d-ic~Gs!p%>kR@mJ{Mo8N#rEVM`>0_k6A!5&jMup5p?Z{CzMGwGbQxw)qQ zly*62L3R87k7>{%DsD}T` z&{JjChKta2>^7}@^MwD$(QqMPczW_^>HH-8SI*ykJT+9FIQMw!M6fhRPdl%I=nM@F z$8=R^D2-j2K7K=U>On!n%OEfvrU(KK<_Twp*_xZ1;SP;dEeLTejZFGOw`XPq90PJV zWDUY`#+SvAH{c8bbQo99#UEb;ZSTW7%g~x!)`F`dq)6A_{!pqK!=Cc`YLZI|{a@yFN)176YUmp3fG;{`HAIJ@vg@oCrb^hNG z+zxBcM(}F9frF$0KS~V9hPnTW-?+eEgP}o?#cA+yG-yCamq+fEF5a(t_TItrwUhp> zVb~q|H!cU!%P}2>af{b8$6$gq@HUq5yg0_*>bcNVJHL*dIXiVVmLXW4Wf*)zAROK#ID|r4Dv~C9=^k4SycR(R7a!e*1b10X? zhFY1wi46+Jp8)(V6l+k_2DC%kjHFV`%%oBQ-kyY)T5|Cm@0+>cDoN*Zp}T6Hsu^xT ss)q5iEC;0^J*u1XCUbK7W?a%j_$5W9s#vGQ7RQ#w7O8?j=lnhW2fR%ztN;K2 literal 0 HcmV?d00001 diff --git a/can_test_motor/__pycache__/motor_control_can_all.cpython-38.pyc b/can_test_motor/__pycache__/motor_control_can_all.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..330d00e868c917d8d86c3a7a8f6cbb5279444c60 GIT binary patch literal 4798 zcmc&&>u(g-6`$wM?8C;`4hGVcNn17E)@F^vBi&+R2S}AlxG1548qw9VcUZ5zFYe3~ zV~vq{gg6NXLQPR)T3(Jmm=8u zaEM+Og|UDrj5fm@VK*=>%t+Ytu&_fQk+Hy;z!BkRLS8xoH4tZma}DMInKMYzPR5O0rQhFJdjDMIqYI^p8%uL{>wBOyGbC=nSmTX-t2l$n zeBR7@tm^nj)w92>XCHSg`*hKa8|CqZ%KTp{pDdJq^8xP=m6(?D1~aYJ?e(b48k6*R z#->R-Q@}Nug1cjl87pCq*k+!X1X`VDIMdPk^BjYH^)R`s`-l?+o>9L3hw|k8o}*t% z9YzXOzx*HqP>UA6n}N8(vF0(62%rsdTA7jF25Fy^5Q_I?CUuR_b~}_7p)9Mqjkxz#PgaKu_k@-PuAanYH$8_4|J*9&>PiHCdEC{$%NozdL61 zM}tg;)mbo;w@rE?nKGIDnqx8TfN5FDw8<2lnyEz!9hfpy$mE%B4LUX{oPtsNM^a`H zH4xQk15D2X9s(%horbYR2AwfEXQi2Fu1JKoA+K@uzMqFS3++wNC9w&9A+cSIz?b52 zQ)qq5585Bv6}PE=D1hyV;2ruNq+$O%n{v+Yawtn!Nt#i^;e_lO$$&eJO&MBAU5-s&Z|h zxT;?P0k}3Ccw_IIKY8oW{)34FeFvGGHf<(@hNC(Q`e$K5e|t>j>#|0lnK*vhHd&+J z!>b2r@>Ig>8seYCU?x2XjuCgA$QDjLiGU3VXfR^a8wGATmrLeJ7i`mt01>n%F(O3( z1-eLgLjC`U0u51s2mmDL90ft=AuL&k0uXfNi~G+W1<8Cm3(j)s&NTq#DilC{O$^X> zI0;1z&?pi#rYgbG_kcuox)sYYAWY@6({0$$jSXC@ypid}g5~K{!_pcW-Q;T&I%K$a zt|^t<>Jv(p=x$iy>y`Rr;QlAmu9>w0X06uD8owF|)G5?6Yu))1OjxJT()-huTlb#5 zLQ@Wb0CaHT5BGN)Cydw1L5V` z8j5Z66$|mxNyl8a)x)rEovrFs3E_y>xS;vM^J6@VRG5 z&mIOfyRJ;%FVDW~ZOUk0cJRJw*TOOGRR7DX;7K>2fJ{8lL{>ZieFa<52d|)p{s60S zBtJy*GLr2`b|CRX9q(e_;$tOw+$v-p@bCO+%a|F4F&4&Vg6{iw&&$iS7g|%uT7Gbk zc!P1|jKZyk(KDtNlj0blXb56v+fOjk3P#A23Pi-n2>~Zelr-G`BtjY{)mnQaw5yPg zK-xjpr&UZl>bNboLRvBP@aJiRmW^c7q>STd)j45I99?w9p#V`J>bPxdP}boM!W%j- zk4bsTxhin~YKNcV9^%{paQ|jUp5pH0Ty0poD$hAXei z5`c3?_Ok>5;-VM(9lS^&zDGxObKWzV%H0o2^H&}(UMf#LC|^3ey>e%=e0Q$2cqJ;b zFfpwZ&G0C3G|<_ZD-hF*Pq^3Wf!TOU2eS6W%#% zF zE5Q#JVXa3C?^LGlKAOK=FFsFQIJ4xP9M9_X0(rVu_2&4}?X%@8S0DX(x;F8ZbC5?F z?Q1L_jm8#Bm)BQ zW|6)Lb1*%Tz*!Rs03gpXP;?X%>1c#$i3BO65($cC0M-b9%+Mbp|NBVz3Aj)Y{9Fd! zA(to4L*@#DS5Dz8lr0bn)Ld C>Vr%G literal 0 HcmV?d00001 diff --git a/can_test_motor/can_main.py b/can_test_motor/can_main.py new file mode 100644 index 0000000..97c37ef --- /dev/null +++ b/can_test_motor/can_main.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python 主程序 +# -*- coding: utf-8 -*- +''' +# @Time : 2025/7/21 +# @Author : hx +# @File : can_main.py +''' + +import can +import time + +# 导入你写的 generate_position_command 函数 +from motor_control_can_all import generate_position_command + + +# ======================================== +# 发送CAN帧函数 +# ======================================== +def send_can_frame(data, can_id=0x01, channel='vcan0', interface='socketcan'): + """ + 发送CAN帧 + :param data: 7字节的列表 + :param can_id: CAN ID + :param channel: CAN 通道 + :param interface: 总线类型 + """ + try: + bus = can.interface.Bus(channel=channel, interface=interface) + msg = can.Message(arbitration_id=can_id, data=data, is_extended_id=False) + bus.send(msg) + print(f"[发送CAN帧] ID={can_id:02X} 数据=[{', '.join(f'0x{x:02X}' for x in data)}]") + bus.shutdown() + except Exception as e: + print(f"[发送失败] {e}") + + +# ======================================== +# 监听CAN反馈函数 +# ======================================== +def listen_can_feedback(channel='vcan0', interface='socketcan', timeout=2.0): + """ + 监听是否有CAN反馈数据 + :param channel: CAN 通道 + :param interface: 总线类型 + :param timeout: 等待反馈的超时时间（秒） + """ + try: + bus = can.interface.Bus(channel=channel, interface=interface) + print(f"[监听反馈] 等待来自 {channel} 的反馈（{timeout}秒超时）...") + msg = bus.recv(timeout=timeout) + if msg: + print(f"[收到反馈] ID={msg.arbitration_id:02X} 数据=[{', '.join(f'0x{x:02X}' for x in msg.data)}]") + else: + print("[未收到反馈]") + bus.shutdown() + except Exception as e: + print(f"[监听失败] {e}") + + +# ======================================== +# 主函数：发送位置控制指令并监听反馈 +# ======================================== + +def can_motor_contral(direction=0, angle=100, microstep=32, can_id=0x01, channel='vcan0', listen_feedback=False): + print("=== 开始发送CAN位置控制指令 ===") + print(f"参数：方向={direction}（0=逆时针，1=顺时针），角度={angle}°，细分={microstep}") + + # 生成CAN数据帧 + can_data = generate_position_command(direction=direction, microstep=microstep, angle=angle) + print(f"生成的CAN数据帧: [{', '.join(f'0x{x:02X}' for x in can_data)}]") + + # 发送CAN帧 + send_can_frame(can_data, can_id=can_id, channel=channel) + + # 如果启用监听，等待反馈 + if listen_feedback: + listen_can_feedback(channel=channel) + + print("=== 电机控制流程完成 ===") + +# ======================================== +# 程序入口（直接运行时使用） +# ======================================== +if __name__ == "__main__": + # 这里写死参数，方便调试 + can_motor_contral( + direction=1, # 顺时针 + angle=180, # 180度 + microstep=16, # 细分值16 + can_id=0x02, # CAN ID 0x02 + channel='vcan0', # 使用虚拟CAN + listen_feedback=True # 是否监听反馈 + ) \ No newline at end of file diff --git a/can_test_motor/can_run_demo.py b/can_test_motor/can_run_demo.py new file mode 100644 index 0000000..0df8e0f --- /dev/null +++ b/can_test_motor/can_run_demo.py @@ -0,0 +1,15 @@ +# can_run_demo.py 调用程序demo + +from can_main import can_motor_contral + + + +if __name__ == "__main__": + can_motor_contral( + direction=1, + angle=90, + microstep=32, + can_id=0x03, + channel='vcan0', + listen_feedback=True + ) \ No newline at end of file diff --git a/can_test_motor/motor_control_can_all.py b/can_test_motor/motor_control_can_all.py new file mode 100644 index 0000000..acd7914 --- /dev/null +++ b/can_test_motor/motor_control_can_all.py @@ -0,0 +1,174 @@ +#!/usr/bin/env python +# 不同模式can发送指令集成 +# -*- coding: utf-8 -*- +''' +# @Time : 2025/7/21 +# @Author : hx +# @File : motor_control_can_all.py +''' + +import can +import time +import argparse + + +# 细分值映射表 +MICROSTEP_MAP = { + 2: 0x01, + 4: 0x02, + 8: 0x04, + 16: 0x10, + 32: 0x20 +} + +# ======================================== +# CAN 发送函数（使用 socketcan） +# ======================================== +def send_can_frame(data, can_id=0x01, channel='can0', bustype='socketcan'): + """ + 发送CAN帧 + :param data: 7字节的列表 + :param can_id: CAN ID + :param channel: CAN 通道 + :param bustype: 总线类型 + """ + try: + bus = can.interface.Bus(channel=channel, bustype=bustype) + msg = can.Message(arbitration_id=can_id, data=data, is_extended_id=False) + bus.send(msg) + print(f"[发送CAN帧] ID={can_id:02X} 数据=[{', '.join(f'0x{x:02X}' for x in data)}]") + bus.shutdown() + except Exception as e: + print(f"[发送失败] {e}") + + +# ======================================== +# 模式1：速度控制模式 (0x01) +# ======================================== +def generate_speed_command(direction, microstep, speed): + """ + 生成速度控制指令 + :param direction: 方向 (0:逆时针, 1:顺时针) + :param microstep: 细分值 (2, 4, 8, 16, 32) + :param speed: 速度值 (单位: rad/s) + """ + direction_byte = 0x01 if direction == 1 else 0x00 + microstep_byte = MICROSTEP_MAP.get(microstep, 0x20) + + raw_speed = int(speed * 10) + speed_high = (raw_speed >> 8) & 0xFF + speed_low = raw_speed & 0xFF + return [0x01, direction_byte, microstep_byte, 0x00, 0x00, speed_high, speed_low] + + +# ======================================== +# 模式2：位置控制模式 (0x02) +# ======================================== +def generate_position_command(direction, microstep, angle): + """ + 生成位置控制指令 + :param direction: 方向 (0:逆时针, 1:顺时针) + :param microstep: 细分值 (2, 4, 8, 16, 32) + :param angle: 角度值 (单位: 度) + """ + direction_byte = 0x01 if direction == 1 else 0x00 + microstep_byte = MICROSTEP_MAP.get(microstep, 0x20) + + raw_angle = int(angle * 10) + pos_high = (raw_angle >> 8) & 0xFF + pos_low = raw_angle & 0xFF + + return [0x02, direction_byte, microstep_byte, pos_high, pos_low, 0x00, 0x64] + + +# ======================================== +# 模式3：力矩控制模式 (0x03) +# ======================================== +def generate_torque_command(direction, microstep, current): + """ + 生成力矩控制指令 + :param direction: 方向 (0:逆时针, 1:顺时针) + :param microstep: 细分值 (2, 4, 8, 16, 32) + :param current: 电流值 (单位: mA) + """ + direction_byte = 0x01 if direction == 1 else 0x00 + microstep_byte = MICROSTEP_MAP.get(microstep, 0x20) + + raw_current = int(current) + current_high = (raw_current >> 8) & 0xFF + current_low = raw_current & 0xFF + + return [0x03, direction_byte, microstep_byte, current_high, current_low, 0x00, 0x64] + + +# ======================================== +# 模式4：单圈绝对角度控制模式 (0x04) +# ======================================== +def generate_absolute_angle_command(direction, microstep, angle, speed): + """ + 生成单圈绝对角度控制指令 + :param direction: 方向 (0:逆时针, 1:顺时针) + :param microstep: 细分值 (2, 4, 8, 16, 32) + :param angle: 目标角度 (单位: 度) + :param speed: 速度值 (单位: rad/s) + """ + direction_byte = 0x01 if direction == 1 else 0x00 + microstep_byte = MICROSTEP_MAP.get(microstep, 0x20) + + raw_angle = int(angle * 10) + pos_high = (raw_angle >> 8) & 0xFF + pos_low = raw_angle & 0xFF + + raw_speed = int(speed * 10) + speed_high = (raw_speed >> 8) & 0xFF + speed_low = raw_speed & 0xFF + + + return [0x04, direction_byte, microstep_byte, pos_high, pos_low, speed_high, speed_low] + + +# ======================================== +# 主函数（命令行调用） +# ======================================== +def main(): + parser = argparse.ArgumentParser(description="CAN电机控制程序，支持4种模式") + parser.add_argument("--mode", type=int, choices=[1, 2, 3, 4], + help="控制模式: 1=速度, 2=位置, 3=力矩, 4=绝对角度") + parser.add_argument("--direction", type=int, choices=[0, 1], default=1, + help="方向: 0=逆时针, 1=顺时针") + parser.add_argument("--microstep", type=int, choices=[2, 4, 8, 16, 32], default=32, + help="细分值: 2, 4, 8, 16, 32") + parser.add_argument("--value", type=float, required=True, + help="控制值（速度: rad/s, 位置/角度: 度, 力矩: mA）") + parser.add_argument("--speed_rad_per_sec", type=float, + help="速度值（仅用于绝对角度模式）") + args = parser.parse_args() + + try: + if args.mode == 1: + cmd = generate_speed_command(args.direction, args.microstep, args.value) + elif args.mode == 2: + cmd = generate_position_command(args.direction, args.microstep, args.value) + elif args.mode == 3: + cmd = generate_torque_command(args.direction, args.microstep, args.value) + elif args.mode == 4: + if args.speed_rad_per_sec is None: + raise ValueError("绝对角度模式需要提供速度参数 (--speed_rad_per_sec)") + cmd = generate_absolute_angle_command(args.direction, args.microstep, args.value, args.speed_rad_per_sec) + + print(f"生成CAN指令: [{', '.join(f'0x{x:02X}' for x in cmd)}]") + send_can_frame(cmd) + + except Exception as e: + print(f"错误: {e}") + + +# ======================================== +# 程序入口 +# ======================================== +if __name__ == "__main__": + main() + #python motor_control_can_all.py --mode 1 --direction 1 --microstep 32 --value 10 + #python motor_control_can_all.py --mode 2 --direction 1 --microstep 32 --value 1000 + #python motor_control_can_all.py --mode 3 --direction 1 --microstep 32 --value 1000 + diff --git a/main_limit.py b/main_limit.py new file mode 100644 index 0000000..2cedced --- /dev/null +++ b/main_limit.py @@ -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() \ No newline at end of file diff --git a/motor_control/__pycache__/can_main.cpython-38.pyc b/motor_control/__pycache__/can_main.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..6e8ee4bf65d7872ab6ea02cfada30a4bade82d14 GIT binary patch literal 3368 zcmd5;-ES0C6u)=Ac6R9pl&E;5K!$?48mryEFC9tgg+J zluwIvp&*K(RHy=_Mo<$Z3RM&TjCs}B?LP6;7oI$4X1i?xeJ~+$+C6hW&OP_sbAP{c z=AHWb2!ZFKa-e%%BO!lcXYQjyXDj@yZV-$l2%}8M(ggM2LPCJIm=$wULZXDNWFe*= zp$VC-ViC3gS_*rNHL``!3bEC!i7kSbs)yO){Q_rth)HY-TgsL(nJs54iVD~e-X_hD z_ic-;(YEf&Jaijtc zllErnuH2^^I#1bpve&dScE&W4X)~8g8LUD<#g#vVWi4*lGSmBY8`^0JmsXz;yfH-E z;b)x%(f7VhdT5W}2#&~TQRtVTFFMo#*O^!p`(;PEBsuaJrNow*x4sw`|(GkKTqw- z>sJmad&qml5cWV10#=_#*CG%r%{z6vGH`JFws!C5aett#H^oyqjiu~Vo3_cjes1d1 zL*?^Fyuou*W7p{~Lc~815O5P>LTD-Q1)$VELBM4p+Tzjz2SyNIOur1RShtzeW8H;VX>6!6`gM%u z&7MqbBQv{VIny>dz{-I0WMde}Dh%E5W`tj%mv4CzOjziUL8^xEUAdP- zpk1lQ%owg}b?0qneq^}e*9&RA7o*)A;s|M1!R<4QBgU)d_U0{D%vt+fs?VODFGlT# z`5xn#wGaeR1-cOaB6JzMW&(^dUG<>OmtdZ#kbovTaGWnMRWD;$W2@U~j zh>ieKsuD!(ksN`^McOYp67K>n#gTes76NJkseoFTZ-7WWp&GgBg0+SU+Vwuo#7pWN znuUR8*MMfSPcxAD(>&ubjJicXF^ie&+7v zaer%$k;#sqfZ=O2gW#B{I4wJ4QRpV9OCLe{P86)hBTbz(6iZ$1(_mXk*>47sr3Gb z5Q#5^Rrw|m9iWaZ0KD`(LiZrxOH^ct%cwF-)2fXnUwcP#Juf_7OH%ceRy}?_h zp-=w0b6EqeX}oIh)FI%_-O2N%FMjccPse=|0LPYiW#Aa%`^@0o$)OGL%Gm9h-f zV^>12Uxn2{{--KkB{on^Y!D?H5%?=G^S?zWe-2NLOyh`C{yd5oP{dF?;{5e4>@j=$ zYcas{J{|rkd+`=@ZUc%|6fdE`WO)D8G(BMQEzo-SJ#B&+S55k#D#>K!CUh&v#tH&w z7bLWQf(qXV3e6(N($4XVhReY~t<9gt0fpoD7=H%EvnXl{cK+*J#0@2r%uFYfej;`y zEI)~+Igv86nDhgL*TC%qkI$USfUhbrFq?M$mj5NAZqAu3pVhaZKb8hUQdFvnbxLGu QWO<}1vP2aGI_K}+-+|C|=Kufz literal 0 HcmV?d00001 diff --git a/motor_control/can_main.py b/motor_control/can_main.py new file mode 100644 index 0000000..d7cc91e --- /dev/null +++ b/motor_control/can_main.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +''' +# @Time : 2025/7/21 +# @Author : hx +# @File : can_main.py +''' + +import can +import time + +# 导入你写的 generate_position_command 函数 +from motor_control_can_all import generate_position_command + + +# ======================================== +# 发送CAN帧函数 +# ======================================== +def send_can_frame(data, can_id=0x01, channel='vcan0', interface='socketcan'): + """ + 发送CAN帧 + :param data: 7字节的列表 + :param can_id: CAN ID + :param channel: CAN 通道 + :param interface: 总线类型 + """ + try: + bus = can.interface.Bus(channel=channel, interface=interface) + msg = can.Message(arbitration_id=can_id, data=data, is_extended_id=False) + bus.send(msg) + print(f"[发送CAN帧] ID={can_id:02X} 数据=[{', '.join(f'0x{x:02X}' for x in data)}]") + bus.shutdown() + except Exception as e: + print(f"[发送失败] {e}") + + +# ======================================== +# 监听CAN反馈函数 +# ======================================== +def listen_can_feedback(channel='vcan0', interface='socketcan', timeout=2.0): + """ + 监听是否有CAN反馈数据 + :param channel: CAN 通道 + :param interface: 总线类型 + :param timeout: 等待反馈的超时时间（秒） + """ + try: + bus = can.interface.Bus(channel=channel, interface=interface) + print(f"[监听反馈] 等待来自 {channel} 的反馈（{timeout}秒超时）...") + msg = bus.recv(timeout=timeout) + if msg: + print(f"[收到反馈] ID={msg.arbitration_id:02X} 数据=[{', '.join(f'0x{x:02X}' for x in msg.data)}]") + else: + print("[未收到反馈]") + bus.shutdown() + except Exception as e: + print(f"[监听失败] {e}") + + +# ======================================== +# 主函数：发送位置控制指令并监听反馈 +# ======================================== + +def can_motor_contral(direction=0, angle=100, microstep=32, can_id=0x01, channel='vcan0', listen_feedback=False): + print("=== 开始发送CAN位置控制指令 ===") + print(f"参数：方向={direction}（0=逆时针，1=顺时针），角度={angle}°，细分={microstep}") + + # 生成CAN数据帧 + can_data = generate_position_command(direction=direction, microstep=microstep, angle=angle) + print(f"生成的CAN数据帧: [{', '.join(f'0x{x:02X}' for x in can_data)}]") + + # 发送CAN帧 + send_can_frame(can_data, can_id=can_id, channel=channel) + + # 如果启用监听，等待反馈 + if listen_feedback: + listen_can_feedback(channel=channel) + + print("=== 电机控制流程完成 ===") + +# ======================================== +# 程序入口（直接运行时使用） +# ======================================== +if __name__ == "__main__": + # 这里写死参数，方便调试 + can_motor_contral( + direction=1, # 顺时针 + angle=180, # 180度 + microstep=16, # 细分值16 + can_id=0x02, # CAN ID 0x02 + channel='vcan0', # 使用虚拟CAN + listen_feedback=True # 是否监听反馈 + ) \ No newline at end of file diff --git a/motor_control/can_run_demo.py b/motor_control/can_run_demo.py new file mode 100644 index 0000000..58ec7cf --- /dev/null +++ b/motor_control/can_run_demo.py @@ -0,0 +1,13 @@ +# can_run_demo.py + +from can_main import can_motor_contral + +if __name__ == "__main__": + can_motor_contral( + direction=1, + angle=90, + microstep=32, + can_id=0x03, + channel='vcan0', + listen_feedback=True + ) \ No newline at end of file diff --git a/motor_control/can_test.py b/motor_control/can_test.py new file mode 100644 index 0000000..ebbed05 --- /dev/null +++ b/motor_control/can_test.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +''' +# @Time : 2025/7/15 17:52 +# @Author : hx +# @File : motor.py +''' + +import time + +start_speed = 8 +stop_speed = 0 + +# 细分值映射表 +MICROSTEP_MAP = { + 2: 0x01, + 4: 0x02, + 8: 0x04, + 16: 0x10, + 32: 0x20 +} + +def generate_speed_command(direction=1, microstep=32, speed=10): + """ + 生成速度控制模式的 CAN 数据帧（7字节） + :param direction: 0=逆时针，1=顺时针 + :param microstep: 细分值（2, 4, 8, 16, 32） + :param speed: 速度值（Rad/s） + :return: 7字节 CAN 数据帧 + """ + control_mode = 0x01 # 速度控制模式 + direction_byte = 0x01 if direction == 1 else 0x00 + microstep_byte = MICROSTEP_MAP.get(microstep, 0x20) + + raw_speed = int(speed * 10) + speed_high = (raw_speed >> 8) & 0xFF + speed_low = raw_speed & 0xFF + + return [control_mode, direction_byte, microstep_byte, 0x00, 0x00, speed_high, speed_low] + + +def generate_position_command(angle, speed=10, direction=1, microstep=32): + """ + 生成位置控制模式的 CAN 数据帧（7字节） + :param angle: 目标角度（度） + :param speed: 速度值（Rad/s） + :param direction: 0=逆时针，1=顺时针 + :param microstep: 细分值（2, 4, 8, 16, 32） + :return: 7字节 CAN 数据帧 + """ + control_mode = 0x02 # 位置控制模式 + direction_byte = 0x01 if direction == 1 else 0x00 + microstep_byte = MICROSTEP_MAP.get(microstep, 0x20) + + raw_angle = int(angle * 10) + pos_high = (raw_angle >> 8) & 0xFF + pos_low = raw_angle & 0xFF + + raw_speed = int(speed * 10) + speed_high = (raw_speed >> 8) & 0xFF + speed_low = raw_speed & 0xFF + + return [control_mode, direction_byte, microstep_byte, pos_high, pos_low, speed_high, speed_low] + + +def send_can_frame(data, can_id=0x01): + """ + 模拟发送CAN帧（只打印，不实际发送） + """ + print(f"[发送CAN帧] ID={can_id:02X} 数据=[{', '.join(f'0x{x:02X}' for x in data)}]") + + +def listen_and_respond(): + """ + 模拟监听并响应信号的逻辑 + """ + try: + print("[监听中] 等待信号...") + + # 模拟收到信号后执行动作 + time.sleep(2) + + # 示例：位置控制模式 - 顺时针转动 1000 度，速度 10Rad/s，32细分 + pos_data = generate_position_command(angle=1000, speed=10, direction=1, microstep=32) + send_can_frame(pos_data, can_id=0x01) + + time.sleep(2) + + # 示例：速度控制模式 - 顺时针，速度 8Rad/s，32细分 + speed_data = generate_speed_command(direction=1, microstep=32, speed=8) + send_can_frame(speed_data, can_id=0x01) + + print("[已完成一次响应]") + + except Exception as e: + print(f"监听时发生错误: {e}") + + +if __name__ == "__main__": + try: + # 示例：执行一次电机控制流程 + listen_and_respond() + + except Exception as e: + print(f"运行错误: {e}") \ No newline at end of file diff --git a/motor_control/contral.py b/motor_control/contral.py new file mode 100644 index 0000000..5513d03 --- /dev/null +++ b/motor_control/contral.py @@ -0,0 +1,134 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +''' +# @Time : 2025/7/15 17:52 +# @Author : hx +# @File : motor.py +''' +import serial +import time +import math + +start_speed = 8 +stop_speed = 0 + +def send_hex_command(port, baudrate, hex_data): + """ + 通过串口发送十六进制指令 + :param port: 串口号 (如 'COM3' 或 '/dev/ttyUSB0') + :param baudrate: 波特率 (如 9600) + :param hex_data: 十六进制字符串 (如 "7B 01 02 01 20 0E 10 00 64 23 7D") + """ + try: + # 转换十六进制字符串为字节数据 + byte_data = bytes.fromhex(hex_data.replace(" ", "")) + + # 打开串口 + with serial.Serial(port, baudrate, timeout=1) as ser: + print(f"已连接串口 {port}, 波特率 {baudrate}") + + # 发送指令 + ser.write(byte_data) + print(f"已发送指令: {hex_data}") + + # 可选：等待并读取返回数据 + time.sleep(0.1) + if ser.in_waiting > 0: + response = ser.read(ser.in_waiting) + print(f"收到响应: {response.hex().upper()}") + + except Exception as e: + print(f"发生错误: {e}") + +def generate_fixed_command(speed): + """ + 生成固定的速度控制指令: 7B 01 01 00 20 00 00 00 7D + 参数: + speed (int): 十进制速度值 (0~255) + 返回: + str: 十六进制命令字符串 + """ + + actual_speed = int(speed * 10) + + if not (0 <= actual_speed <= 255): + raise ValueError("速度必须在 0~255 范围内") + + command_bytes = [ + 0x7B, 0x01, 0x01, # 帧头、地址、控制模式 + 0x00, 0x20, # 方向、细分 + 0x00, 0x00, # 位置高/低字节（持续旋转） + 0x00, actual_speed # 保持整数速度值 + ] + + # 计算校验和（前9个字节异或） + checksum = 0 + for byte in command_bytes: + checksum ^= byte + + full_command = command_bytes + [checksum, 0x7D] + return ' '.join(f'{byte:02X}' for byte in full_command) + +def listen_and_respond(port, baudrate): + """ + 持续监听串口，一旦有信号就执行开始和停止的指令 + """ + try: + with serial.Serial(port, baudrate, timeout=1) as ser: + ser.reset_input_buffer() + print(f"[监听中] 串口 {port} 已打开，等待信号...") + + while True: + if ser.in_waiting > 0: + incoming = ser.read(ser.in_waiting) + print(f"[收到信号] 内容: {incoming.hex().upper()}") + + # 执行开始旋转 + start_cmd = generate_fixed_command(8) + print("发送开始指令:", start_cmd) + ser.write(bytes.fromhex(start_cmd.replace(" ", ""))) + + # 延迟一段时间 + time.sleep(4) + + # 执行停止 + stop_cmd = generate_fixed_command(0) + print("发送停止指令:", stop_cmd) + ser.write(bytes.fromhex(stop_cmd.replace(" ", ""))) + + print("[已完成一次响应]\n等待下一次信号...\n") + + time.sleep(0.1) # 避免CPU占用过高 + + except Exception as e: + print(f"监听时发生错误: {e}") + +if __name__ == "__main__": + # 配置参数 + PORT = "/dev/ttyACM0" # 修改为你的串口号 + BAUD_RATE = 115200 # 修改为你的设备波特率 + HEX_COMMAND = { + "Clockwise_rotation":"7B 01 02 01 20 0E 10 00 64 23 7D", # 顺时针旋转360 + "Counterclockwise_rotation":"7B 01 02 00 20 0E 10 00 64 22 7D", + "One_revolution_per_second":"7B 01 01 00 20 00 00 00 3F 64 7D", # 有问题 + "Ten_radin_per_second":"7B 01 01 00 20 00 00 00 64 3F 7D", + "Eight_radin_per_second": "7B 01 01 00 20 00 00 00 50 0B 7D", + "stop":"7B 01 01 00 20 00 00 00 00 5B 7D" + }# 要发送的指令 + + try: + # 生成并发送开始旋转指令（速度 = 8） + start_cmd = generate_fixed_command(start_speed) + send_hex_command(PORT, BAUD_RATE, start_cmd) + + # 延迟一段时间（比如 4 秒） + time.sleep(4) + + # 生成并发送停止指令（速度 = 0） + stop_cmd = generate_fixed_command(stop_speed) + send_hex_command(PORT, BAUD_RATE, stop_cmd) + + print("电机控制流程执行完成。") + + except Exception as e: + print(f"运行错误: {e}") \ No newline at end of file diff --git a/motor_control/motor_control_can_all.py b/motor_control/motor_control_can_all.py new file mode 100644 index 0000000..b8104e5 --- /dev/null +++ b/motor_control/motor_control_can_all.py @@ -0,0 +1,170 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +''' +# @Time : 2025/7/18 +# @Author : hx +# @File : motor_control_can_all.py +''' + +import can +import time +import argparse + + +# 细分值映射表 +MICROSTEP_MAP = { + 2: 0x01, + 4: 0x02, + 8: 0x04, + 16: 0x0, + 32: 0x20 +} + +# ======================================== +# CAN 发送函数（使用 socketcan） +# ======================================== +def send_can_frame(data, can_id=0x01, channel='can0', bustype='socketcan'): + """ + 发送CAN帧 + :param data: 7字节的列表 + :param can_id: CAN ID + :param channel: CAN 通道 + :param bustype: 总线类型 + """ + try: + bus = can.interface.Bus(channel=channel, bustype=bustype) + msg = can.Message(arbitration_id=can_id, data=data, is_extended_id=False) + bus.send(msg) + print(f"[发送CAN帧] ID={can_id:02X} 数据=[{', '.join(f'0x{x:02X}' for x in data)}]") + bus.shutdown() + except Exception as e: + print(f"[发送失败] {e}") + + +# ======================================== +# 模式1：速度控制模式 (0x01) +# ======================================== +def generate_speed_command(direction, microstep, speed): + """ + 生成速度控制指令 + :param direction: 方向 (0:逆时针, 1:顺时针) + :param microstep: 细分值 (2, 4, 8, 16, 32) + :param speed: 速度值 (单位: rad/s) + """ + direction_byte = 0x01 if direction == 1 else 0x00 + microstep_byte = MICROSTEP_MAP.get(microstep, 0x20) + + raw_speed = int(speed * 10) + speed_high = (raw_speed >> 8) & 0xFF + speed_low = raw_speed & 0xFF + + return [0x01, direction_byte, microstep_byte, 0x00, 0x00, speed_high, speed_low] + + +# ======================================== +# 模式2：位置控制模式 (0x02) +# ======================================== +def generate_position_command(direction, microstep, angle): + """ + 生成位置控制指令 + :param direction: 方向 (0:逆时针, 1:顺时针) + :param microstep: 细分值 (2, 4, 8, 16, 32) + :param angle: 角度值 (单位: 度) + """ + direction_byte = 0x01 if direction == 1 else 0x00 + microstep_byte = MICROSTEP_MAP.get(microstep, 0x20) + + raw_angle = int(angle * 10) + pos_high = (raw_angle >> 8) & 0xFF + pos_low = raw_angle & 0xFF + + return [0x02, direction_byte, microstep_byte, pos_high, pos_low, 0x00, 0x64] + + +# ======================================== +# 模式3：力矩控制模式 (0x03) +# ======================================== +def generate_torque_command(direction, microstep, current): + """ + 生成力矩控制指令 + :param direction: 方向 (0:逆时针, 1:顺时针) + :param microstep: 细分值 (2, 4, 8, 16, 32) + :param current: 电流值 (单位: mA) + """ + direction_byte = 0x01 if direction == 1 else 0x00 + microstep_byte = MICROSTEP_MAP.get(microstep, 0x20) + + raw_current = int(current) + current_high = (raw_current >> 8) & 0xFF + current_low = raw_current & 0xFF + + return [0x03, direction_byte, microstep_byte, current_high, current_low, 0x00, 0x64] + + +# ======================================== +# 模式4：单圈绝对角度控制模式 (0x04) +# ======================================== +def generate_absolute_angle_command(direction, microstep, angle, speed): + """ + 生成单圈绝对角度控制指令 + :param direction: 方向 (0:逆时针, 1:顺时针) + :param microstep: 细分值 (2, 4, 8, 16, 32) + :param angle: 目标角度 (单位: 度) + :param speed: 速度值 (单位: rad/s) + """ + direction_byte = 0x01 if direction == 1 else 0x00 + microstep_byte = MICROSTEP_MAP.get(microstep, 0x20) + + raw_angle = int(angle * 10) + pos_high = (raw_angle >> 8) & 0xFF + pos_low = raw_angle & 0xFF + + raw_speed = int(speed * 10) + speed_high = (raw_speed >> 8) & 0xFF + speed_low = raw_speed & 0xFF + + + return [0x04, direction_byte, microstep_byte, pos_high, pos_low, speed_high, speed_low] + + +# ======================================== +# 主函数（命令行调用） +# ======================================== +def main(): + parser = argparse.ArgumentParser(description="CAN电机控制程序，支持4种模式") + parser.add_argument("--mode", type=int, choices=[1, 2, 3, 4], + help="控制模式: 1=速度, 2=位置, 3=力矩, 4=绝对角度") + parser.add_argument("--direction", type=int, choices=[0, 1], default=1, + help="方向: 0=逆时针, 1=顺时针") + parser.add_argument("--microstep", type=int, choices=[2, 4, 8, 16, 32], default=32, + help="细分值: 2, 4, 8, 16, 32") + parser.add_argument("--value", type=float, required=True, + help="控制值（速度: rad/s, 位置/角度: 度, 力矩: mA）") + parser.add_argument("--speed_rad_per_sec", type=float, + help="速度值（仅用于绝对角度模式）") + args = parser.parse_args() + + try: + if args.mode == 1: + cmd = generate_speed_command(args.direction, args.microstep, args.value) + elif args.mode == 2: + cmd = generate_position_command(args.direction, args.microstep, args.value) + elif args.mode == 3: + cmd = generate_torque_command(args.direction, args.microstep, args.value) + elif args.mode == 4: + if args.speed_rad_per_sec is None: + raise ValueError("绝对角度模式需要提供速度参数 (--speed_rad_per_sec)") + cmd = generate_absolute_angle_command(args.direction, args.microstep, args.value, args.speed_rad_per_sec) + + print(f"生成CAN指令: [{', '.join(f'0x{x:02X}' for x in cmd)}]") + send_can_frame(cmd) + + except Exception as e: + print(f"错误: {e}") + + +# ======================================== +# 程序入口 +# ======================================== +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/motor_control/motor_control_usb_all.py b/motor_control/motor_control_usb_all.py new file mode 100644 index 0000000..3ea4961 --- /dev/null +++ b/motor_control/motor_control_usb_all.py @@ -0,0 +1,212 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +''' +# @Time : 2025/7/18 +# @Author : hx +# @File : motor_control_modes.py +''' + +import serial +import time +import argparse + + +# ======================================== +# 通用发送函数 +# ======================================== +def send_hex_command(port, baudrate, hex_data): + """ + 发送十六进制指令 + """ + try: + byte_data = bytes.fromhex(hex_data.replace(" ", "")) + with serial.Serial(port, baudrate, timeout=1) as ser: + print(f"已连接串口 {port}, 波特率 {baudrate}") + ser.write(byte_data) + print(f"已发送指令: {hex_data}") + time.sleep(0.1) + if ser.in_waiting > 0: + response = ser.read(ser.in_waiting) + print(f"收到响应: {response.hex().upper()}") + except Exception as e: + print(f"发送失败: {e}") + + +# ======================================== +# 模式1：速度控制模式 (0x01) +# 控制字节3 = 0x01 +# 第6~7字节为0 +# 第8~9字节：速度值（单位 rad/s，放大10倍） +# ======================================== +def generate_speed_command(direction, microstep, speed): + """ + 生成速度控制指令 + :param direction: 方向 (0:逆时针, 1:顺时针) + :param microstep: 细分值 (2, 4, 8, 16, 32) + :param speed: 速度值 (单位: rad/s) + """ + command = [0x7B, 0x01, 0x01, direction, microstep, 0x00, 0x00] + + # 转速数据，单位为 rad/s，放大10倍传输 + raw_speed = int(speed * 10) + high_byte = (raw_speed >> 8) & 0xFF + low_byte = raw_speed & 0xFF + command += [high_byte, low_byte] + + # 计算 BCC 校验和（前9个字节异或） + checksum = 0 + for b in command: + checksum ^= b + + full_command = command + [checksum, 0x7D] + return ' '.join(f'{b:02X}' for b in full_command) + + +# ======================================== +# 模式2：位置控制模式 (0x02) +# 控制字节3 = 0x02 +# 第6~7字节：角度值（单位 度，放大10倍） +# 第8~9字节为0 +# ======================================== +def generate_position_command(direction, microstep, angle): + """ + 生成位置控制指令 + :param direction: 方向 (0:逆时针, 1:顺时针) + :param microstep: 细分值 (2, 4, 8, 16, 32) + :param angle: 角度值 (单位: 度) + """ + command = [0x7B, 0x01, 0x02, direction, microstep] + + # 角度数据，单位为度，放大10倍传输 + raw_angle = int(angle * 10) + high_byte = (raw_angle >> 8) & 0xFF + low_byte = raw_angle & 0xFF + command += [low_byte, high_byte, 0x00, 0x64] + + # 计算 BCC 校验和（前9个字节异或） + checksum = 0 + for b in command: + checksum ^= b + + full_command = command + [checksum, 0x7D] + return ' '.join(f'{b:02X}' for b in full_command) + + +# ======================================== +# 模式3：力矩控制模式 (0x03) +# 控制字节3 = 0x03 +# 第6~7字节：电流值（单位 mA） +# 第8~9字节为0 +# ======================================== +def generate_torque_command(direction, microstep, current): + """ + 生成力矩控制指令 + :param direction: 方向 (0:逆时针, 1:顺时针) + :param microstep: 细分值 (2, 4, 8, 16, 32) + :param current: 电流值 (单位: mA) + """ + command = [0x7B, 0x01, 0x03, direction, microstep] + + # 电流数据，单位为 mA + raw_current = int(current) + high_byte = (raw_current >> 8) & 0xFF + low_byte = raw_current & 0xFF + command += [low_byte, high_byte, 0x00, 0x64] + + # 计算 BCC 校验和（前9个字节异或） + checksum = 0 + for b in command: + checksum ^= b + + full_command = command + [checksum, 0x7D] + return ' '.join(f'{b:02X}' for b in full_command) + + +# ======================================== +# 模式4：单圈绝对角度控制模式 (0x04) +# 控制字节3 = 0x04 +# 第6~7字节：目标角度（单位 度，放大10倍） +# 第8~9字节为0 +# ======================================== +def generate_absolute_angle_command(direction, microstep, target_angle, speed_rad_per_sec): + """ + 生成单圈绝对角度控制指令 + :param direction: 方向 (0:逆时针, 1:顺时针) + :param microstep: 细分值 (2, 4, 8, 16, 32) + :param target_angle: 目标角度 (单位: 度) + :param speed_rad_per_sec: 速度 (单位: rad/s) + :return: 十六进制指令字符串 + """ + command = [0x7B, 0x01, 0x04, direction, microstep] + + # 目标角度（放大10倍） + raw_angle = int(target_angle * 10) + pos_high = (raw_angle >> 8) & 0xFF + pos_low = raw_angle & 0xFF + command += [pos_high, pos_low] + + # 速度（放大10倍） + raw_speed = int(speed_rad_per_sec * 10) + speed_high = (raw_speed >> 8) & 0xFF + speed_low = raw_speed & 0xFF + command += [speed_high, speed_low] + + # 计算 BCC 校验和（前9个字节异或） + checksum = 0 + for b in command: + checksum ^= b + + full_command = command + [checksum, 0x7D] + return ' '.join(f'{b:02X}' for b in full_command) + + +# ======================================1 +# 主函数（命令行调用） +# ======================================== +def main(): + parser = argparse.ArgumentParser(description="电机控制程序，支持4种模式") + parser.add_argument("--port", default="/dev/ttyACM0", help="串口号") + parser.add_argument("--baud", type=int, default=115200, help="波特率") + parser.add_argument("--mode", type=int, choices=[1, 2, 3, 4], + help="控制模式: 1=速度, 2=位置, 3=力矩, 4=绝对角度") + parser.add_argument("--direction", type=int, choices=[0, 1], default=1, + help="方向: 0=逆时针, 1=顺时针") + parser.add_argument("--microstep", type=int, choices=[2, 4, 8, 16, 32], default=32, + help="细分值: 2, 4, 8, 16, 32") + parser.add_argument("--value", type=float, required=True, + help="控制值（速度: rad/s, 位置/角度: 度, 力矩: mA）") + parser.add_argument("--speed_rad_per_sec", type=float, required=True, + help="控制值（速度）") + args = parser.parse_args() + + try: + if args.mode == 1: + cmd = generate_speed_command(args.direction, args.microstep, args.value) + elif args.mode == 2: + cmd = generate_position_command(args.direction, args.microstep, args.value) + elif args.mode == 3: + cmd = generate_torque_command(args.direction, args.microstep, args.value) + elif args.mode == 4: + cmd = generate_absolute_angle_command(args.direction, args.microstep, args.value,args.speed_rad_per_sec) + + print(f"生成指令: {cmd}") + send_hex_command(args.port, args.baud, cmd) + print("指令发送完成。") + + except Exception as e: + print(f"错误: {e}") + + +# ======================================== +# 程序入口 +# ======================================== +if __name__ == "__main__": + main() + + #python motor_control_usb_all.py --mode 1 --direction 1 --microstep 32 --value 10 --speed_rad_per_sec=10 + + #python motor_control_usb_all.py --mode 2 --direction 1 --microstep 32 --value 360.0 --speed_rad_per_sec=10 + + #python motor_control_usb_all.py --mode 3 --direction 1 --microstep 32 --value 1000 --speed_rad_per_sec=10 + + #python motor_control_usb_all.py --mode 4 --direction 1 --microstep 32 --value 100.0 --speed_rad_per_sec=10 diff --git a/motor_control/motor_test_mostimprotant.py b/motor_control/motor_test_mostimprotant.py new file mode 100644 index 0000000..8023253 --- /dev/null +++ b/motor_control/motor_test_mostimprotant.py @@ -0,0 +1,134 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +''' +# @Time : 2025/7/15 17:52 +# @Author : hx +# @File : motor.py +''' +import serial +import time +import math + +start_speed = 8 +stop_speed = 0 + +def send_hex_command(port, baudrate, hex_data): + """ + 通过串口发送十六进制指令 + :param port: 串口号 (如 'COM3' 或 '/dev/ttyUSB0') + :param baudrate: 波特率 (如 9600) + :param hex_data: 十六进制字符串 (如 "7B 01 02 01 20 0E 10 00 64 23 7D") + """ + try: + # 转换十六进制字符串为字节数据 + byte_data = bytes.fromhex(hex_data.replace(" ", "")) + + # 打开串口 + with serial.Serial(port, baudrate, timeout=1) as ser: + print(f"已连接串口 {port}, 波特率 {baudrate}") + + # 发送指令 + ser.write(byte_data) + print(f"已发送指令: {hex_data}") + + # 可选：等待并读取返回数据 + time.sleep(0.1) + if ser.in_waiting > 0: + response = ser.read(ser.in_waiting) + print(f"收到响应: {response.hex().upper()}") + + except Exception as e: + print(f"发生错误: {e}") + +def generate_fixed_command(speed): + """ + 生成固定的速度控制指令: 7B 01 01 00 20 00 00 00 7D + 参数: + speed (int): 十进制速度值 (0~255) + 返回: + str: 十六进制命令字符串 + """ + + actual_speed = int(speed * 10) + + if not (0 <= actual_speed <= 255): + raise ValueError("速度必须在 0~255 范围内") + + command_bytes = [ + 0x7B, 0x01, 0x01, # 帧头、地址、控制模式 + 0x00, 0x20, # 方向、细分 + 0x00, 0x00, # 位置高/低字节（持续旋转） + 0x00, actual_speed # 保持整数速度值 + ] + + # 计算校验和（前9个字节异或） + checksum = 0 + for byte in command_bytes: + checksum ^= byte + + full_command = command_bytes + [checksum, 0x7D] + return ' '.join(f'{byte:02X}' for byte in full_command) + +def listen_and_respond(port, baudrate): + """ + 持续监听串口，一旦有信号就执行开始和停止的指令 + """ + try: + with serial.Serial(port, baudrate, timeout=1) as ser: + ser.reset_input_buffer() + print(f"[监听中] 串口 {port} 已打开，等待信号...") + + while True: + if ser.in_waiting > 0: + incoming = ser.read(ser.in_waiting) + print(f"[收到信号] 内容: {incoming.hex().upper()}") + + # 执行开始旋转 + start_cmd = generate_fixed_command(8) + print("发送开始指令:", start_cmd) + ser.write(bytes.fromhex(start_cmd.replace(" ", ""))) + + # 延迟一段时间 + time.sleep(4) + + # 执行停止 + stop_cmd = generate_fixed_command(0) + print("发送停止指令:", stop_cmd) + ser.write(bytes.fromhex(stop_cmd.replace(" ", ""))) + + print("[已完成一次响应]\n等待下一次信号...\n") + + time.sleep(0.1) # 避免CPU占用过高 + + except Exception as e: + print(f"监听时发生错误: {e}") + +if __name__ == "__main__": + # 配置参数 + PORT = "/dev/ttyACM0" # 修改为你的串口号 + BAUD_RATE = 115200 # 修改为你的设备波特率 + HEX_COMMAND = { + "Clockwise_rotation":"7B 01 02 01 20 0E 10 00 64 23 7D", # 顺时针旋转360 + "Counterclockwise_rotation":"7B 01 02 00 20 0E 10 00 64 22 7D", + "One_revolution_per_second":"7B 01 01 00 20 00 00 00 3F 64 7D", # 有问题 + "Ten_radin_per_second":"7B 01 01 00 20 00 00 00 64 3F 7D", + "Eight_radin_per_second": "7B 01 01 00 20 00 00 00 50 0B 7D", + "stop":"7B 01 01 00 20 00 00 00 00 5B 7D" + }# 要发送的指令 + + try: + # 生成并发送开始旋转指令（速度 = 8） + start_cmd = generate_fixed_command(start_speed) + send_hex_command(PORT, BAUD_RATE, start_cmd) + + # 延迟一段时间（比如 4 秒） + time.sleep(4) + + # 生成并发送停止指令（速度 = 0） + stop_cmd = generate_fixed_command(stop_speed) + send_hex_command(PORT, BAUD_RATE, stop_cmd) + + print("电机控制流程执行完成。") + + except Exception as e: + print(f"运行错误: {e}") diff --git a/motor_control/test.py b/motor_control/test.py new file mode 100644 index 0000000..340d1d4 --- /dev/null +++ b/motor_control/test.py @@ -0,0 +1,9 @@ +import time + +print("守护进程已启动，将持续运行...") + +try: + while True: + time.sleep(1) +except KeyboardInterrupt: + print("守护进程已停止") \ No newline at end of file diff --git a/qiege_motor.py b/qiege_motor.py new file mode 100644 index 0000000..3799dc3 --- /dev/null +++ b/qiege_motor.py @@ -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 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("测试结束。") \ No newline at end of file diff --git a/qt/DM_CAN/DM_CAN.py b/qt/DM_CAN/DM_CAN.py new file mode 100644 index 0000000..d548f7f --- /dev/null +++ b/qt/DM_CAN/DM_CAN.py @@ -0,0 +1,624 @@ +from time import sleep +import numpy as np +from enum import IntEnum +from struct import unpack +from struct import pack + + +class Motor: + def __init__(self, MotorType, SlaveID, MasterID): + """ + define Motor object 定义电机对象 + :param MotorType: Motor type 电机类型 + :param SlaveID: CANID 电机ID + :param MasterID: MasterID 主机ID 建议不要设为0 + """ + self.Pd = float(0) + self.Vd = float(0) + self.state_q = float(0) + self.state_dq = float(0) + self.state_tau = float(0) + self.SlaveID = SlaveID + self.MasterID = MasterID + self.MotorType = MotorType + self.isEnable = False + self.NowControlMode = Control_Type.MIT + self.temp_param_dict = {} + + def recv_data(self, q: float, dq: float, tau: float): + self.state_q = q + self.state_dq = dq + self.state_tau = tau + + def getPosition(self): + """ + get the position of the motor 获取电机位置 + :return: the position of the motor 电机位置 + """ + return self.state_q + + def getVelocity(self): + """ + get the velocity of the motor 获取电机速度 + :return: the velocity of the motor 电机速度 + """ + return self.state_dq + + def getTorque(self): + """ + get the torque of the motor 获取电机力矩 + :return: the torque of the motor 电机力矩 + """ + return self.state_tau + + def getParam(self, RID): + """ + get the parameter of the motor 获取电机内部的参数，需要提前读取 + :param RID: DM_variable 电机参数 + :return: the parameter of the motor 电机参数 + """ + if RID in self.temp_param_dict: + return self.temp_param_dict[RID] + else: + return None + + +class MotorControl: + send_data_frame = np.array( + [0x55, 0xAA, 0x1e, 0x03, 0x01, 0x00, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0, 0, 0, 0, 0x00, 0x08, 0x00, + 0x00, 0, 0, 0, 0, 0, 0, 0, 0, 0x00], np.uint8) + # 4310 4310_48 4340 4340_48 + Limit_Param = [[12.5, 30, 10], [12.5, 50, 10], [12.5, 8, 28], [12.5, 10, 28], + # 6006 8006 8009 10010L 10010 + [12.5, 45, 20], [12.5, 45, 40], [12.5, 45, 54], [12.5, 25, 200], [12.5, 20, 200], + # H3510 DMG62150 DMH6220 + [12.5 , 280 , 1],[12.5 , 45 , 10],[12.5 , 45 , 10]] + + def __init__(self, serial_device): + """ + define MotorControl object 定义电机控制对象 + :param serial_device: serial object 串口对象 + """ + self.serial_ = serial_device + self.motors_map = dict() + self.data_save = bytes() # save data + if self.serial_.is_open: # open the serial port + print("Serial port is open") + serial_device.close() + self.serial_.open() + + def controlMIT(self, DM_Motor, kp: float, kd: float, q: float, dq: float, tau: float): + """ + MIT Control Mode Function 达妙电机MIT控制模式函数 + :param DM_Motor: Motor object 电机对象 + :param kp: kp + :param kd: kd + :param q: position 期望位置 + :param dq: velocity 期望速度 + :param tau: torque 期望力矩 + :return: None + """ + if DM_Motor.SlaveID not in self.motors_map: + print("controlMIT ERROR : Motor ID not found") + return + kp_uint = float_to_uint(kp, 0, 500, 12) + kd_uint = float_to_uint(kd, 0, 5, 12) + MotorType = DM_Motor.MotorType + Q_MAX = self.Limit_Param[MotorType][0] + DQ_MAX = self.Limit_Param[MotorType][1] + TAU_MAX = self.Limit_Param[MotorType][2] + q_uint = float_to_uint(q, -Q_MAX, Q_MAX, 16) + dq_uint = float_to_uint(dq, -DQ_MAX, DQ_MAX, 12) + tau_uint = float_to_uint(tau, -TAU_MAX, TAU_MAX, 12) + data_buf = np.array([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8) + data_buf[0] = (q_uint >> 8) & 0xff + data_buf[1] = q_uint & 0xff + data_buf[2] = dq_uint >> 4 + data_buf[3] = ((dq_uint & 0xf) << 4) | ((kp_uint >> 8) & 0xf) + data_buf[4] = kp_uint & 0xff + data_buf[5] = kd_uint >> 4 + data_buf[6] = ((kd_uint & 0xf) << 4) | ((tau_uint >> 8) & 0xf) + data_buf[7] = tau_uint & 0xff + self.__send_data(DM_Motor.SlaveID, data_buf) + self.recv() # receive the data from serial port + + def control_delay(self, DM_Motor, kp: float, kd: float, q: float, dq: float, tau: float, delay: float): + """ + MIT Control Mode Function with delay 达妙电机MIT控制模式函数带延迟 + :param DM_Motor: Motor object 电机对象 + :param kp: kp + :param kd: kd + :param q: position 期望位置 + :param dq: velocity 期望速度 + :param tau: torque 期望力矩 + :param delay: delay time 延迟时间 单位秒 + """ + self.controlMIT(DM_Motor, kp, kd, q, dq, tau) + sleep(delay) + + def control_Pos_Vel(self, Motor, P_desired: float, V_desired: float): + """ + control the motor in position and velocity control mode 电机位置速度控制模式 + :param Motor: Motor object 电机对象 + :param P_desired: desired position 期望位置 + :param V_desired: desired velocity 期望速度 + :return: None + """ + if Motor.SlaveID not in self.motors_map: + print("Control Pos_Vel Error : Motor ID not found") + return + motorid = 0x100 + Motor.SlaveID + data_buf = np.array([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8) + P_desired_uint8s = float_to_uint8s(P_desired) + V_desired_uint8s = float_to_uint8s(V_desired) + data_buf[0:4] = P_desired_uint8s + data_buf[4:8] = V_desired_uint8s + self.__send_data(motorid, data_buf) + # time.sleep(0.001) + self.recv() # receive the data from serial port + + def control_Vel(self, Motor, Vel_desired): + """ + control the motor in velocity control mode 电机速度控制模式 + :param Motor: Motor object 电机对象 + :param Vel_desired: desired velocity 期望速度 + """ + if Motor.SlaveID not in self.motors_map: + print("control_VEL ERROR : Motor ID not found") + return + motorid = 0x200 + Motor.SlaveID + data_buf = np.array([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8) + Vel_desired_uint8s = float_to_uint8s(Vel_desired) + data_buf[0:4] = Vel_desired_uint8s + self.__send_data(motorid, data_buf) + self.recv() # receive the data from serial port + + def control_pos_force(self, Motor, Pos_des: float, Vel_des, i_des): + """ + control the motor in EMIT control mode 电机力位混合模式 + :param Pos_des: desired position rad 期望位置 单位为rad + :param Vel_des: desired velocity rad/s 期望速度 为放大100倍 + :param i_des: desired current rang 0-10000 期望电流标幺值放大10000倍 + 电流标幺值：实际电流值除以最大电流值，最大电流见上电打印 + """ + if Motor.SlaveID not in self.motors_map: + print("control_pos_vel ERROR : Motor ID not found") + return + motorid = 0x300 + Motor.SlaveID + data_buf = np.array([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8) + Pos_desired_uint8s = float_to_uint8s(Pos_des) + data_buf[0:4] = Pos_desired_uint8s + Vel_uint = np.uint16(Vel_des) + ides_uint = np.uint16(i_des) + data_buf[4] = Vel_uint & 0xff + data_buf[5] = Vel_uint >> 8 + data_buf[6] = ides_uint & 0xff + data_buf[7] = ides_uint >> 8 + self.__send_data(motorid, data_buf) + self.recv() # receive the data from serial port + + def enable(self, Motor): + """ + enable motor 使能电机 + 最好在上电后几秒后再使能电机 + :param Motor: Motor object 电机对象 + """ + self.__control_cmd(Motor, np.uint8(0xFC)) + sleep(0.1) + self.recv() # receive the data from serial port + + def enable_old(self, Motor ,ControlMode): + """ + enable motor old firmware 使能电机旧版本固件，这个是为了旧版本电机固件的兼容性 + 可恶的旧版本固件使能需要加上偏移量 + 最好在上电后几秒后再使能电机 + :param Motor: Motor object 电机对象 + """ + data_buf = np.array([0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc], np.uint8) + enable_id = ((int(ControlMode)-1) << 2) + Motor.SlaveID + self.__send_data(enable_id, data_buf) + sleep(0.1) + self.recv() # receive the data from serial port + + def disable(self, Motor): + """ + disable motor 失能电机 + :param Motor: Motor object 电机对象 + """ + self.__control_cmd(Motor, np.uint8(0xFD)) + sleep(0.1) + self.recv() # receive the data from serial port + + def set_zero_position(self, Motor): + """ + set the zero position of the motor 设置电机0位 + :param Motor: Motor object 电机对象 + """ + self.__control_cmd(Motor, np.uint8(0xFE)) + sleep(0.1) + self.recv() # receive the data from serial port + + def recv(self): + # 把上次没有解析完的剩下的也放进来 + data_recv = b''.join([self.data_save, self.serial_.read_all()]) + packets = self.__extract_packets(data_recv) + for packet in packets: + data = packet[7:15] + CANID = (packet[6] << 24) | (packet[5] << 16) | (packet[4] << 8) | packet[3] + CMD = packet[1] + self.__process_packet(data, CANID, CMD) + + def recv_set_param_data(self): + data_recv = self.serial_.read_all() + packets = self.__extract_packets(data_recv) + for packet in packets: + data = packet[7:15] + CANID = (packet[6] << 24) | (packet[5] << 16) | (packet[4] << 8) | packet[3] + CMD = packet[1] + self.__process_set_param_packet(data, CANID, CMD) + + def __process_packet(self, data, CANID, CMD): + if CMD == 0x11: + if CANID != 0x00: + if CANID in self.motors_map: + q_uint = np.uint16((np.uint16(data[1]) << 8) | data[2]) + dq_uint = np.uint16((np.uint16(data[3]) << 4) | (data[4] >> 4)) + tau_uint = np.uint16(((data[4] & 0xf) << 8) | data[5]) + MotorType_recv = self.motors_map[CANID].MotorType + Q_MAX = self.Limit_Param[MotorType_recv][0] + DQ_MAX = self.Limit_Param[MotorType_recv][1] + TAU_MAX = self.Limit_Param[MotorType_recv][2] + recv_q = uint_to_float(q_uint, -Q_MAX, Q_MAX, 16) + recv_dq = uint_to_float(dq_uint, -DQ_MAX, DQ_MAX, 12) + recv_tau = uint_to_float(tau_uint, -TAU_MAX, TAU_MAX, 12) + self.motors_map[CANID].recv_data(recv_q, recv_dq, recv_tau) + else: + MasterID=data[0] & 0x0f + if MasterID in self.motors_map: + q_uint = np.uint16((np.uint16(data[1]) << 8) | data[2]) + dq_uint = np.uint16((np.uint16(data[3]) << 4) | (data[4] >> 4)) + tau_uint = np.uint16(((data[4] & 0xf) << 8) | data[5]) + MotorType_recv = self.motors_map[MasterID].MotorType + Q_MAX = self.Limit_Param[MotorType_recv][0] + DQ_MAX = self.Limit_Param[MotorType_recv][1] + TAU_MAX = self.Limit_Param[MotorType_recv][2] + recv_q = uint_to_float(q_uint, -Q_MAX, Q_MAX, 16) + recv_dq = uint_to_float(dq_uint, -DQ_MAX, DQ_MAX, 12) + recv_tau = uint_to_float(tau_uint, -TAU_MAX, TAU_MAX, 12) + self.motors_map[MasterID].recv_data(recv_q, recv_dq, recv_tau) + + + def __process_set_param_packet(self, data, CANID, CMD): + if CMD == 0x11 and (data[2] == 0x33 or data[2] == 0x55): + masterid=CANID + slaveId = ((data[1] << 8) | data[0]) + if CANID==0x00: #防止有人把MasterID设为0稳一手 + masterid=slaveId + + if masterid not in self.motors_map: + if slaveId not in self.motors_map: + return + else: + masterid=slaveId + + RID = data[3] + # 读取参数得到的数据 + if is_in_ranges(RID): + #uint32类型 + num = uint8s_to_uint32(data[4], data[5], data[6], data[7]) + self.motors_map[masterid].temp_param_dict[RID] = num + + else: + #float类型 + num = uint8s_to_float(data[4], data[5], data[6], data[7]) + self.motors_map[masterid].temp_param_dict[RID] = num + + + def addMotor(self, Motor): + """ + add motor to the motor control object 添加电机到电机控制对象 + :param Motor: Motor object 电机对象 + """ + self.motors_map[Motor.SlaveID] = Motor + if Motor.MasterID != 0: + self.motors_map[Motor.MasterID] = Motor + return True + + def __control_cmd(self, Motor, cmd: np.uint8): + data_buf = np.array([0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, cmd], np.uint8) + self.__send_data(Motor.SlaveID, data_buf) + + def __send_data(self, motor_id, data): + """ + send data to the motor 发送数据到电机 + :param motor_id: + :param data: + :return: + """ + self.send_data_frame[13] = motor_id & 0xff + self.send_data_frame[14] = (motor_id >> 8)& 0xff #id high 8 bits + self.send_data_frame[21:29] = data + self.serial_.write(bytes(self.send_data_frame.T)) + + def __read_RID_param(self, Motor, RID): + can_id_l = Motor.SlaveID & 0xff #id low 8 bits + can_id_h = (Motor.SlaveID >> 8)& 0xff #id high 8 bits + data_buf = np.array([np.uint8(can_id_l), np.uint8(can_id_h), 0x33, np.uint8(RID), 0x00, 0x00, 0x00, 0x00], np.uint8) + self.__send_data(0x7FF, data_buf) + + def __write_motor_param(self, Motor, RID, data): + can_id_l = Motor.SlaveID & 0xff #id low 8 bits + can_id_h = (Motor.SlaveID >> 8)& 0xff #id high 8 bits + data_buf = np.array([np.uint8(can_id_l), np.uint8(can_id_h), 0x55, np.uint8(RID), 0x00, 0x00, 0x00, 0x00], np.uint8) + if not is_in_ranges(RID): + # data is float + data_buf[4:8] = float_to_uint8s(data) + else: + # data is int + data_buf[4:8] = data_to_uint8s(int(data)) + self.__send_data(0x7FF, data_buf) + + def switchControlMode(self, Motor, ControlMode): + """ + switch the control mode of the motor 切换电机控制模式 + :param Motor: Motor object 电机对象 + :param ControlMode: Control_Type 电机控制模式 example:MIT:Control_Type.MIT MIT模式 + """ + max_retries = 20 + retry_interval = 0.1 #retry times + RID = 10 + self.__write_motor_param(Motor, RID, np.uint8(ControlMode)) + for _ in range(max_retries): + sleep(retry_interval) + self.recv_set_param_data() + if Motor.SlaveID in self.motors_map: + if RID in self.motors_map[Motor.SlaveID].temp_param_dict: + if abs(self.motors_map[Motor.SlaveID].temp_param_dict[RID] - ControlMode) < 0.1: + return True + else: + return False + return False + + def save_motor_param(self, Motor): + """ + save the all parameter to flash 保存所有电机参数 + :param Motor: Motor object 电机对象 + :return: + """ + can_id_l = Motor.SlaveID & 0xff #id low 8 bits + can_id_h = (Motor.SlaveID >> 8)& 0xff #id high 8 bits + data_buf = np.array([np.uint8(can_id_l), np.uint8(can_id_h), 0xAA, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8) + self.disable(Motor) # before save disable the motor + self.__send_data(0x7FF, data_buf) + sleep(0.001) + + def change_limit_param(self, Motor_Type, PMAX, VMAX, TMAX): + """ + change the PMAX VMAX TMAX of the motor 改变电机的PMAX VMAX TMAX + :param Motor_Type: + :param PMAX: 电机的PMAX + :param VMAX: 电机的VMAX + :param TMAX: 电机的TMAX + :return: + """ + self.Limit_Param[Motor_Type][0] = PMAX + self.Limit_Param[Motor_Type][1] = VMAX + self.Limit_Param[Motor_Type][2] = TMAX + + def refresh_motor_status(self,Motor): + """ + get the motor status 获得电机状态 + """ + can_id_l = Motor.SlaveID & 0xff #id low 8 bits + can_id_h = (Motor.SlaveID >> 8) & 0xff #id high 8 bits + data_buf = np.array([np.uint8(can_id_l), np.uint8(can_id_h), 0xCC, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8) + self.__send_data(0x7FF, data_buf) + self.recv() # receive the data from serial port + + def change_motor_param(self, Motor, RID, data): + """ + change the RID of the motor 改变电机的参数 + :param Motor: Motor object 电机对象 + :param RID: DM_variable 电机参数 + :param data: 电机参数的值 + :return: True or False ,True means success, False means fail + """ + max_retries = 20 + retry_interval = 0.05 #retry times + + self.__write_motor_param(Motor, RID, data) + for _ in range(max_retries): + self.recv_set_param_data() + if Motor.SlaveID in self.motors_map and RID in self.motors_map[Motor.SlaveID].temp_param_dict: + if abs(self.motors_map[Motor.SlaveID].temp_param_dict[RID] - data) < 0.1: + return True + else: + return False + sleep(retry_interval) + return False + + def read_motor_param(self, Motor, RID): + """ + read only the RID of the motor 读取电机的内部信息例如 版本号等 + :param Motor: Motor object 电机对象 + :param RID: DM_variable 电机参数 + :return: 电机参数的值 + """ + max_retries = 20 + retry_interval = 0.05 #retry times + self.__read_RID_param(Motor, RID) + for _ in range(max_retries): + sleep(retry_interval) + self.recv_set_param_data() + if Motor.SlaveID in self.motors_map: + if RID in self.motors_map[Motor.SlaveID].temp_param_dict: + return self.motors_map[Motor.SlaveID].temp_param_dict[RID] + return None + + # ------------------------------------------------- + # Extract packets from the serial data + def __extract_packets(self, data): + frames = [] + header = 0xAA + tail = 0x55 + frame_length = 16 + i = 0 + remainder_pos = 0 + + while i <= len(data) - frame_length: + if data[i] == header and data[i + frame_length - 1] == tail: + frame = data[i:i + frame_length] + frames.append(frame) + i += frame_length + remainder_pos = i + else: + i += 1 + self.data_save = data[remainder_pos:] + return frames + + +def LIMIT_MIN_MAX(x, min, max): + if x <= min: + x = min + elif x > max: + x = max + + +def float_to_uint(x: float, x_min: float, x_max: float, bits): + LIMIT_MIN_MAX(x, x_min, x_max) + span = x_max - x_min + data_norm = (x - x_min) / span + return np.uint16(data_norm * ((1 << bits) - 1)) + + +def uint_to_float(x: np.uint16, min: float, max: float, bits): + span = max - min + data_norm = float(x) / ((1 << bits) - 1) + temp = data_norm * span + min + return np.float32(temp) + + +def float_to_uint8s(value): + # Pack the float into 4 bytes + packed = pack('f', value) + # Unpack the bytes into four uint8 values + return unpack('4B', packed) + + +def data_to_uint8s(value): + # Check if the value is within the range of uint32 + if isinstance(value, int) and (0 <= value <= 0xFFFFFFFF): + # Pack the uint32 into 4 bytes + packed = pack('I', value) + else: + raise ValueError("Value must be an integer within the range of uint32") + + # Unpack the bytes into four uint8 values + return unpack('4B', packed) + + +def is_in_ranges(number): + """ + check if the number is in the range of uint32 + :param number: + :return: + """ + if (7 <= number <= 10) or (13 <= number <= 16) or (35 <= number <= 36): + return True + return False + + +def uint8s_to_uint32(byte1, byte2, byte3, byte4): + # Pack the four uint8 values into a single uint32 value in little-endian order + packed = pack('<4B', byte1, byte2, byte3, byte4) + # Unpack the packed bytes into a uint32 value + return unpack('x%WLuk)5N9WN&U;UO zy>1h@o{jc4pH&F?8y`+z8a!Nuk-QIv5k^DOr!MU)uF_Xsm6BD$6sF!MOy%0H>gr5e zCoO#jG-;{QaKuU!bE!LGH4UWYh?@bs!$s0X;Pj=y!&Ml`TVObGDI>1JC{ykeS7j>G z;I1(poEy^G2)s`NKaB2laoMX2=O&C57|9wXWJJdh@jj(E16NGlS11w6SRH{nBAzxP zJ#DNDV_ae2oEfaLuVp;ygqb)8-Yj?vc&p?sX0vL^Tfo-}{*`|lc)Q^1Z}_)?pDXxk z;lBoat@Phu^K7B?Ujx4R244rhUh-#vUo81L@MjBtuJGRgzESe$ApY|u-vE9I`4Jh< zBM<-F=q-$!`J53r`sI!^@`Df7RYHOfz|63KW{YSdG|OP5Jxr6$YpD>?AkZKa!9~R1 zh73t#E31In9&)u4&bvJO4A<|XS-_Wi=-=Yu>;(#EQK)1BfG$ekpk#(YBEAavR_lzc zCtQ4StH-;d~Z1D=SW3qj3$y zcn((^&UFkWZlZ#rxR!Ih*f?H7q8@`Ee~^T};2VKkbw209&x3KB-DofX&+B0%l55)- zTuYH!!o%CK0gHbj@ddcOY=LQSLPYJ&d+q1@d$XS&w^&U<8n7z)5DcLNEjGFdbFo}tw1BG zBkW7MtK`Lk4U{QLsThT#mP(~iv{Eq(MbDM2yo^qE9w{F4@sGbBA3mKPJ()iJ^Z4Me z$-^g^JbCnBa`gCL^6~e7Jb!dJJ^B^6>A`oC!=F<;mzedWaV&D1}!yy7q-cwc5Sb8lI1fu z7xDc&VDJN)U`Y{b(s|mZFvE;Y+p>Sn KLJ4-wqWTY8&YDC3 literal 0 HcmV?d00001 diff --git a/qt/calculate/__pycache__/ik.cpython-39.pyc b/qt/calculate/__pycache__/ik.cpython-39.pyc new file mode 100644 index 0000000000000000000000000000000000000000..6235dee05753a8c900043b85ccf34b8b9848f54c GIT binary patch literal 1819 zcma)7TTC2P7(Qoa*&PO0Zregjq4NmZx-6F$Vzh!9V;Wo1LLMWiRc_Yy;-3 zLFC$?u2q8Cf^1u$wGlLsf-5brKA8AE@xi1!!|oGLKI?zZESFSGJeza=`}cqUoSAI6 zs!Bw#&ixjT{wg5!gc?T|2gW(rMgR^4DL95?q;Lv9hGeYZw@{m>2odx~#79MGod#JB zyAQU}1sDXSeFUck;!|)vA_6@d5aFn7M1*%`x?I783xFL!X_1r@u?eLsNQJ@;dW`cR z`9Y+=Z6d|5PCcX^WCSK%8MHV-rbS2@5z&svOnZI^|Y6< zqM4;SmtWdX{7Ts%r~hJl6)){cs)!#rRg-GQ=?{>UQvo>nXq<7W&f;_pboK(HT2jlf z_b2walaex5rt4s05AZC5iR(!nsR#4`2~hM5HhMt={0!D#hYrF&rVg;Fj}b5NL9gS) z!zMUJCr>w!GG><%X2aBtq=8NYXtzn@z6O((Q``?;W853y^g$lIrAB%-pe$~JojyTM zKujRV|0@F+oO~B00vORB@D(ZUMU>#$=^d4m8bE1MDwuwfF+T;mNSeUk%(PRWwUB1g zGQh!`Rhjx7-XBqdmCt=y*jaGqC!AY3XJM>3GGS*vFYILO>-X&Mm+i?%&g>6++f#22 zo6%%KvR4;&KLvYQWW|NYU)bYId)wbIi^2_^k%oID=hlPW2W!ssN+COAe|^uM&p3;d z2RX6Ohq3M+$r zD>tAX7~0z_OqphViuCg;VtV`qD|HsM-J7dGV|VnY!ozWUGG}it|o?%`UYx!QCGV8*qa&g|$uR&WQ8vl6$AlS8H}=+0IPCtT0(|12EbuEb*Oi zETz7p>q*`6#KUIP;;v|xCk#VdyrCs5o@%wSswZF8u4#srOq7z1tGWq^G{>i?mvmJ% zET0}$v~a>`J8j`13lCddECjm)b|>sE*xfRBAtG}ZBeFk{imQ6qOzQnHb-1vln_vP+~r%}>7sBU>(e@S-SM|v|n{6Fa% z9E>L8YA`w!bh0zW`P)GymAtG4yOrcX@TwV%gkzCZENrSlZSc7(!x$cj)dxk|#dZGxUwpn#f=xGh^2v99XYS2ME9)KZnKS2{+|2xb=bXup zjini$cYfb0MGj+s(PH>9v3LQm|1yMRl9$;!mvhpR_BFmPyG5FvFvge-(r{cMfX+AibHhIlURi)7!bY3hGOA;5wHIW zM8I~qww5NS55|vt%Dti^bmM^%GsCki5sQQuNdzGvm*#)-}xGJ($ zUV3#uN>}Pz8?~xe3H->;SBhm9b7Q0I`9Wki^}Nqi2A}@tB1EoK-EwoKM(%ibyZg;v zPS&fNUhbl-Ud~+!a)o@kP%q~LHy5bERVcd~11kaKry!Wf@L3Es z3w>WG;!d9lNH===!|}a@kRHA#kTpVJ4lFg!^Q@1rfQ@`KGm?pKfU*&z{NSsfAKbk^ zjIu(mv8+%dv@Oa&z-jqGUImfiR-~GMDdK^eBL6fAf-M#^h2GWWWXpmNBl2%rdR!G1dGwfOI&sn4q>DdeFio^s1&ujUUQnt(aU{5e<(l)=4+{;Yr0U$7`*2-XrU zBS6huxw8uB`!*x{kQ%QRyk!SKEfho^u#5}O87A{^12nb6Xjvqkg-Q*&m;oE;rp z?|lF1fU~TrC_}TN`yM6@0Q5(Dx^{Looz#WnPGP9Wp+^RKt9k-jc65F8tvrgM>{g0F zDY8qhS1bjm8MNbWtj@$IfAZss-d_%&X(}G;TZMMU5r(2O42lhKtj5<%$r}8yEsx!>;>y YYIUnB>t*+OBHX7go)T$CkD2t?zZKxxGynhq literal 0 HcmV?d00001 diff --git a/qt/calculate/calculate_angle.py b/qt/calculate/calculate_angle.py new file mode 100644 index 0000000..6e8c764 --- /dev/null +++ b/qt/calculate/calculate_angle.py @@ -0,0 +1,138 @@ +# qt_main.py + +import numpy as np +import matplotlib.pyplot as plt +from ik import inverseF # 假设这是你自己的逆运动学函数 +from matplotlib.animation import FuncAnimation + +# 设置中文字体和解决负号显示问题 +plt.rcParams['font.sans-serif'] = ['SimHei', 'WenQuanYi Zen Hei', 'FangSong'] # 按优先级选择字体 +plt.rcParams['axes.unicode_minus'] = False # 显示负号 - + + +# 杆长参数 +L1 = 250 +L2 = 300 +L3 = 300 +L4 = 250 +L0 = 250 + + +# 1. 轨迹生成函数：每个轨迹类型独立封装，支持外部传参 +# -------------------------------------------------- + +def generate_circle(center=(100, 300), radius=40): + from trajectory import circle_trajectory + return circle_trajectory(center=center, radius=radius) + +def generate_line(start=(125, 300), end=(125, 400)): + from trajectory import line_trajectory + return line_trajectory(start=start, end=end) + +def generate_ellipse(center=(100, 200), rx=50, ry=25): + from trajectory import ellipse_trajectory + return ellipse_trajectory(center=center, rx=rx, ry=ry) + +def generate_square(side=60, start_point=(100, 200)): + from trajectory import square_trajectory + return square_trajectory(side=side, start_point=start_point) + +def generate_triangle(base_length=100, height=80, base_center=(100, 200)): + from trajectory import triangle_trajectory + return triangle_trajectory(base_length=base_length, height=height, base_center=base_center) + + +# 4. 主函数：根据轨迹类型调用对应函数并执行 +# -------------------------------------------------- + + +def main_of_5dof(trajectory_type='circle', show_animation=True, **kwargs): + """ + 主函数：根据轨迹类型和参数生成轨迹、计算角度、并可视化 + 支持传参： + - circle: center, radius + - line: start, end + - ellipse: center, rx, ry + - square: side, start_point + - triangle: base_length, height, base_center + - show_animation: 是否显示动画 (True/False) + """ + + # 根据轨迹类型生成轨迹 + if trajectory_type == 'circle': + x_list, y_list = generate_circle( + center=kwargs.get('center', (100, 300)), + radius=kwargs.get('radius', 40) + ) + elif trajectory_type == 'line': + x_list, y_list = generate_line( + start=kwargs.get('start', (125, 300)), + end=kwargs.get('end', (125, 400)) + ) + # 其他轨迹类型的处理... + + # 实时计算并输出角度（无论是否显示动画） + 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) + print(f"第 {i} 个点: theta1 = {np.degrees(theta1):.2f}°, theta4 = {np.degrees(theta4):.2f}°") + except Exception as e: + print(f"第 {i} 点计算失败: {e}") + + # 如果需要显示动画 + if show_animation: + fig, ax = plt.subplots() + ax.set_xlim(-300, 500) + ax.set_ylim(0, 500) + ax.set_aspect('equal') + ax.grid(True) + ax.set_title("五连杆末端沿轨迹运动") + ax.plot(x_list, y_list, 'b--', label='理想轨迹') + line, = ax.plot([], [], 'r-o', linewidth=2, markersize=6, label='五连杆结构') + + def draw_frame(i): + x = x_list[i] + y = y_list[i] + + try: + theta1, theta4 = inverseF(x, y, L1, L2, L3, L4, L0) + except Exception as e: + print(f"第 {i} 帧: 计算失败 -> {e}") + theta1 = theta4 = None + + if theta1 is None or theta4 is None: + line.set_data([], []) + return line, + + # 左右臂坐标计算 + x2 = L1 * np.cos(theta1) + y2 = L1 * np.sin(theta1) + x4 = L4 * np.cos(theta4) + L0 + 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) + return line, + + ani = FuncAnimation(fig, draw_frame, frames=len(x_list), interval=50, blit=True) + plt.legend() + plt.show() + +# 📌 运行主函数 +if __name__ == "__main__": + main_of_5dof( + trajectory_type='circle', + center=(150, 250), + radius=60, + show_animation=False # 设置为 False 则不显示动画 + ) + + # 示例：其他轨迹使用方式 + # main_of_5dof(trajectory_type='line', start=(0, 0), end=(200, 300), show_animation=False) + # main_of_5dof(trajectory_type='ellipse', center=(100, 200), rx=80, ry=40) + # main_of_5dof(trajectory_type='square', side=100, start_point=(100, 200)) + # main_of_5dof(trajectory_type='triangle', base_length=120, height=100, base_center=(100, 200)) \ No newline at end of file diff --git a/qt/calculate/fk.py b/qt/calculate/fk.py new file mode 100644 index 0000000..07200bf --- /dev/null +++ b/qt/calculate/fk.py @@ -0,0 +1,40 @@ +import numpy as np + + +def forwardF(u1, u4, omega1, omega4, l1, l2, l3, l4, l5, alpha1, alpha4): + # 位置分析 + xb = l1 * np.cos(u1) + yb = l1 * np.sin(u1) + xd = l5 + l4 * np.cos(u4) + yd = l4 * np.sin(u4) + lbd = np.sqrt((xd - xb) ** 2 + (yd - yb) ** 2) + A0 = 2 * l2 * (xd - xb) + B0 = 2 * l2 * (yd - yb) + C0 = l2 ** 2 + lbd ** 2 - l3 ** 2 + u2 = 2 * np.arctan((B0 + np.sqrt(A0 ** 2 + B0 ** 2 - C0 ** 2)) / (A0 + C0)) + xc = xb + l2 * np.cos(u2) + yc = yb + l2 * np.sin(u2) + u3 = np.arctan2((yc - yd), (xc - xd)) + np.pi + + # 速度分析 + A = np.array([[l2 * np.sin(u2), -l3 * np.sin(u3)], + [l2 * np.cos(u2), -l3 * np.cos(u3)]]) + B = np.array([-l1 * omega1 * np.sin(u1) + l4 * omega4 * np.sin(u4), + -l1 * omega1 * np.cos(u1) + l4 * omega4 * np.cos(u4)]) + omega = np.linalg.solve(A, B) + omega2, omega3 = omega[0], omega[1] + + # 加速度分析 + Aa = np.array([[l2 * np.sin(u2), -l3 * np.sin(u3)], + [l2 * np.cos(u2), -l3 * np.cos(u3)]]) + Ba = np.array([l3 * np.cos(u3) * omega3 ** 2 - l2 * np.cos(u2) * omega2 ** 2 + l4 * np.cos( + u4) * omega4 ** 2 + l4 * np.sin(u4) * alpha4 - l1 * np.cos(u1) * omega1 ** 2 - l1 * np.sin(u1) * alpha1, + -l3 * np.sin(u3) * omega3 ** 2 + l2 * np.sin(u2) * omega2 ** 2 - l4 * np.sin( + u4) * omega4 ** 2 + l4 * np.cos(u4) * alpha4 + l1 * np.sin(u1) * omega1 ** 2 - l1 * np.cos( + u1) * alpha1]) + alpha = np.linalg.solve(Aa, Ba) + + return xc, yc, u2, u3, omega2, omega3, alpha[0], alpha[1] + +# 示例调用 +# xc, yc, u2, u3, omega2, omega3, alpha2, alpha3 = five(np.pi/6, np.pi/4, 1, 2, 1, 2, 3, 4, 5, 0.1, 0.2) \ No newline at end of file diff --git a/qt/calculate/ik.py b/qt/calculate/ik.py new file mode 100644 index 0000000..6046976 --- /dev/null +++ b/qt/calculate/ik.py @@ -0,0 +1,69 @@ +import math + + +def inverseF(x, y, l1, l2, l3, l4, l5): + """ + 五连杆机构逆向运动学函数（Python 实现） + + 输入： + x, y: 末端执行器坐标 + l1~l5: 各杆长度 + + 输出： + theta1, theta2: 两个主动关节角度（弧度） + """ + Xc = x + Yc = y + + # ===== 左侧链路（l1, l2）===== + numerator_left = Xc ** 2 + Yc ** 2 - l1 ** 2 - l2 ** 2 + denominator_left = 2 * l1 * l2 + cosfoai_12 = numerator_left / denominator_left + + if cosfoai_12 > 1 or cosfoai_12 < -1: + raise ValueError("目标点超出工作空间！左侧无解。") + foai_12 = 2 * math.pi - math.acos(cosfoai_12) + + # 求第一个电机角度 foai_01 + numerator_foai01 = l2 * Yc * math.sin(foai_12) + Xc * (l2 * math.cos(foai_12) + l1) + denominator_foai01 = (l2 * math.cos(foai_12) + l1) ** 2 + (l2 * math.sin(foai_12)) ** 2 + + if denominator_foai01 == 0: + raise ZeroDivisionError("分母为零，无法计算左侧角度。") + + cosfoai_01 = numerator_foai01 / denominator_foai01 + if cosfoai_01 > 1 or cosfoai_01 < -1: + raise ValueError("cosfoai_01 超出 [-1, 1]，左侧无解。") + foai_01 = math.acos(cosfoai_01) + + # ===== 右侧链路（l3, l4）===== + Xc_shifted = Xc - l5 + numerator_right = Xc_shifted ** 2 + Yc ** 2 - l3 ** 2 - l4 ** 2 + denominator_right = 2 * l3 * l4 + cosfoai_34 = numerator_right / denominator_right + + if cosfoai_34 > 1 or cosfoai_34 < -1: + raise ValueError("目标点超出工作空间！右侧无解。") + foai_34 = 2 * math.pi - math.acos(cosfoai_34) + + A = l5 - Xc + B = l3 * math.sin(foai_34) + C = l4 + l3 * math.cos(foai_34) + + if B == 0 and C == 0: + raise ZeroDivisionError("B 和 C 均为零，无法计算右侧角度。") + + try: + foai_t = math.acos(B / math.sqrt(B ** 2 + C ** 2)) + foai_40 = foai_t - math.asin(A / math.sqrt(B ** 2 + C ** 2)) + except: + raise ValueError("右侧三角函数计算失败，请检查输入是否合法。") + + # 转换为角度再转回弧度 + theta1_deg = math.degrees(foai_01) + theta2_deg = 180 - math.degrees(foai_40) + + theta1 = math.radians(theta1_deg) + theta2 = math.radians(theta2_deg) + + return theta1, theta2 \ No newline at end of file diff --git a/qt/calculate/test_ik.py b/qt/calculate/test_ik.py new file mode 100644 index 0000000..5e3ee00 --- /dev/null +++ b/qt/calculate/test_ik.py @@ -0,0 +1,64 @@ +import math +from ik import inverseF +from 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 +x = 125 +#y = 382.338 +y = 300 +omega1 = omega4 = 500 +alpha1 = alpha4 = 0 + +# 逆解 +u1, u4 = inverseF(x, y, l1, l2, l3, l4, l5) + +# 正解验证 +xc, yc, u2, u3, omega, alpha, _, _ = forwardF(u1, u4, omega1, omega4, l1, l2, l3, l4, l5, alpha1, alpha4) + +# 左侧链路 +x1, y1 = 0, 0 +x2 = l1 * math.cos(u1) +y2 = l1 * math.sin(u1) + +# 右侧链路 +x5, y5 = l5, 0 +x4 = l4 * math.cos(u4)+l5 # 注意方向 +y4 = l4 * math.sin(u4) + +# 绘图 +plt.figure(figsize=(8, 8)) + +# 左侧链路 +plt.plot([x1, x2, xc], [y1, y2, yc], 'b-o', label='左侧链路') + +# 右侧链路 +plt.plot([x5, x4, xc], [y5, y4, yc], 'r-o', label='右侧链路') + +# 标记关键点 +plt.plot(x1, y1, 'ro') # O1 +plt.plot(x5, y5, 'go') # O2 +plt.plot(x2, y2, 'yo') # B +plt.plot(x4, y4, 'mo') # D +plt.plot(xc, yc, 'ko', markersize=10) # C（末端） + +# 设置图形 +plt.grid(True) +plt.axis('equal') +plt.xlim([-200, l5 + 200]) +plt.ylim([-200, 600]) +plt.title('SCARA 五连杆逆解结构图') +plt.xlabel('X (mm)') +plt.ylabel('Y (mm)') +plt.legend() +plt.show() \ No newline at end of file diff --git a/qt/calculate/traj_fk.py b/qt/calculate/traj_fk.py new file mode 100644 index 0000000..d12693f --- /dev/null +++ b/qt/calculate/traj_fk.py @@ -0,0 +1,62 @@ +import numpy as np + +from 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 = 50 +l2 = 50 +l3 = 50 +l4 = 50 +l5 = 50 + +# 初始角度（弧度） +u1_base = np.deg2rad(120) # 左侧电机初始角 +u4_base = np.deg2rad(120) # 右侧电机初始角 + +# 设置绘图区域 +plt.figure(figsize=(8, 8)) +ax = plt.gca() +ax.set_xlim(-100, l5 + 100) +ax.set_ylim(-100, 100) +ax.set_aspect('equal') +ax.grid(True) +ax.set_title("五连杆机构运动仿真（调用FK函数）") + +for i in range(1, 61): + # 更新两个驱动臂的角度 + angle1 = u1_base - np.deg2rad(1.75 * i) # 左侧角度变化 + angle4 = u4_base + np.deg2rad(1.75 * i) # 右侧角度变化 + + #正向运动学函数获取末端位置和中间角度 + result = forwardF(angle1, angle4, 0, 0, l1, l2, l3, l4, l5, 0, 0) + xc, yc, u2, u3 = result[:4] + + # 构建各点坐标 + x = [0, l1*np.cos(angle1), xc, l4*np.cos(angle4)+l5, l5] + y = [0, l1*np.sin(angle1), yc, l4*np.sin(angle4), 0] + + # 清除上一帧并绘制新图形 + ax.cla() + ax.set_xlim(-100, l5 + 100) + ax.set_ylim(-100, 100) + ax.set_aspect('equal') + ax.grid(True) + ax.set_title("五连杆机构运动仿真（调用FK函数）") + + # 绘制结构线和关键点 + ax.plot(x, y, 'r-o', linewidth=2, markersize=6, markerfacecolor='red') + ax.plot(x[0], y[0], 'go') # 原点 + ax.plot(x[1], y[1], 'bo') # 第二个点 + ax.plot(x[2], y[2], 'mo') # 中间点 + ax.plot(x[3], y[3], 'co') # 第四个点 + ax.plot(x[4], y[4], 'yo') # 最后一个点 + + plt.pause(0.1) + +plt.close() \ No newline at end of file diff --git a/qt/calculate/traj_main.py b/qt/calculate/traj_main.py new file mode 100644 index 0000000..5df2cfb --- /dev/null +++ b/qt/calculate/traj_main.py @@ -0,0 +1,71 @@ +# main_animation.py +import numpy as np +import matplotlib.pyplot as plt +from ik import inverseF +from trajectory import circle_trajectory, line_trajectory, ellipse_trajectory, square_trajectory, triangle_trajectory +from matplotlib.animation import FuncAnimation + +# 设置中文字体和解决负号显示问题 +plt.rcParams['font.sans-serif'] = ['SimHei', 'WenQuanYi Zen Hei', 'FangSong'] # 按优先级选择字体 +plt.rcParams['axes.unicode_minus'] = False # 显示负号 - + +# 杆长参数 +L1 = 250 +L2 = 300 +L3 = 300 +L4 = 250 +L0 = 250 + +# 设置绘图区域 +fig, ax = plt.subplots() +ax.set_xlim(-300, 500) +ax.set_ylim(0, 500) +ax.set_aspect('equal') +ax.grid(True) +ax.set_title("五连杆末端沿轨迹运动") + +line, = ax.plot([], [], 'r-o', linewidth=2, markersize=6) +# 选择轨迹类型： +TRAJECTORY_TYPE = 'ellipse' # 可选: circle, line, ellipse, square, triangle +if TRAJECTORY_TYPE == 'line': + x_list, y_list = circle_trajectory(center=(100, 300), radius=40) +elif TRAJECTORY_TYPE == 'line': + x_list, y_list = line_trajectory(start=(125, 300), end=(125, 400)) +elif TRAJECTORY_TYPE == 'ellipse': + x_list, y_list = ellipse_trajectory(center=(100, 200), rx=50, ry=25) +elif TRAJECTORY_TYPE == 'square': + x_list, y_list = square_trajectory(side=60) +elif TRAJECTORY_TYPE == 'triangle': + x_list, y_list = triangle_trajectory(base_length=100, height=80) +else: + raise ValueError("未知的轨迹类型，请选择 circle / line / ellipse / square / triangle") + +# 动画函数 +def draw_frame(i): + x = x_list[i] + y = y_list[i] + + try: + theta1, theta4 = inverseF(x, y, L1, L2, L3, L4, L0) + print(theta1) + print(theta4) + # 左侧电机臂末端 + x2 = L1 * np.cos(theta1) + y2 = L1 * np.sin(theta1) + # 右侧电机臂末端 + x4 = L4 * np.cos(theta4)+L0 + 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) + except Exception as e: + print(f"第 {i} 帧跳过，错误: {e}") + line.set_data([], []) + + return line, + +# 创建动画 +ani = FuncAnimation(fig, draw_frame, frames=len(x_list), interval=100, repeat=True) + +plt.show() \ No newline at end of file diff --git a/qt/calculate/trajectory.py b/qt/calculate/trajectory.py new file mode 100644 index 0000000..031033d --- /dev/null +++ b/qt/calculate/trajectory.py @@ -0,0 +1,62 @@ +# trajectory.py + +import numpy as np + +def circle_trajectory(center=(80, 0), radius=40, num_points=60): + """ 圆形轨迹 """ + 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) + return x_list, y_list + +def line_trajectory(start=(40, 0), end=(120, 0), num_points=60): + """ 直线轨迹 """ + t = np.linspace(0, 1, num_points) + x_list = start[0] + t * (end[0] - start[0]) + y_list = start[1] + t * (end[1] - start[1]) + return x_list, y_list + +def ellipse_trajectory(center=(80, 0), rx=50, ry=25, num_points=60): + """ 椭圆轨迹 """ + angles = np.linspace(0, 2 * np.pi, num_points) + x_list = center[0] + rx * np.cos(angles) + y_list = center[1] + ry * np.sin(angles) + return x_list, y_list + +def square_trajectory(side=60, num_points=60): + """ 正方形轨迹 """ + x_list, y_list = [], [] + for i in range(num_points): + t = i / num_points + if t < 0.25: + x = 80 + 60 * t * 4 + y = 0 + elif t < 0.5: + x = 140 + y = 0 + 60 * (t - 0.25) * 4 + elif t < 0.75: + x = 140 - 60 * (t - 0.5) * 4 + y = 60 + else: + x = 80 + y = 60 - 60 * (t - 0.75) * 4 + x_list.append(x) + y_list.append(y) + return x_list, y_list + +def triangle_trajectory(base_length=100, height=80, num_points=60): + """ 三角形轨迹 """ + x_list, y_list = [], [] + points = [(80, 0), (130, 80), (30, 80), (80, 0)] + for i in range(num_points): + idx = int(i / num_points * 3) + t = (i % (num_points // 3)) / (num_points // 3) + x = points[idx][0] + t * (points[idx+1][0] - points[idx][0]) + y = points[idx][1] + t * (points[idx+1][1] - points[idx][1]) + x_list.append(x) + y_list.append(y) + return x_list, y_list + +def custom_trajectory(custom_x, custom_y): + """ 自定义轨迹，输入两个列表即可 """ + return custom_x, custom_y \ No newline at end of file diff --git a/qt/can_test_motor/can_main.py b/qt/can_test_motor/can_main.py new file mode 100644 index 0000000..97c37ef --- /dev/null +++ b/qt/can_test_motor/can_main.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python 主程序 +# -*- coding: utf-8 -*- +''' +# @Time : 2025/7/21 +# @Author : hx +# @File : can_main.py +''' + +import can +import time + +# 导入你写的 generate_position_command 函数 +from motor_control_can_all import generate_position_command + + +# ======================================== +# 发送CAN帧函数 +# ======================================== +def send_can_frame(data, can_id=0x01, channel='vcan0', interface='socketcan'): + """ + 发送CAN帧 + :param data: 7字节的列表 + :param can_id: CAN ID + :param channel: CAN 通道 + :param interface: 总线类型 + """ + try: + bus = can.interface.Bus(channel=channel, interface=interface) + msg = can.Message(arbitration_id=can_id, data=data, is_extended_id=False) + bus.send(msg) + print(f"[发送CAN帧] ID={can_id:02X} 数据=[{', '.join(f'0x{x:02X}' for x in data)}]") + bus.shutdown() + except Exception as e: + print(f"[发送失败] {e}") + + +# ======================================== +# 监听CAN反馈函数 +# ======================================== +def listen_can_feedback(channel='vcan0', interface='socketcan', timeout=2.0): + """ + 监听是否有CAN反馈数据 + :param channel: CAN 通道 + :param interface: 总线类型 + :param timeout: 等待反馈的超时时间（秒） + """ + try: + bus = can.interface.Bus(channel=channel, interface=interface) + print(f"[监听反馈] 等待来自 {channel} 的反馈（{timeout}秒超时）...") + msg = bus.recv(timeout=timeout) + if msg: + print(f"[收到反馈] ID={msg.arbitration_id:02X} 数据=[{', '.join(f'0x{x:02X}' for x in msg.data)}]") + else: + print("[未收到反馈]") + bus.shutdown() + except Exception as e: + print(f"[监听失败] {e}") + + +# ======================================== +# 主函数：发送位置控制指令并监听反馈 +# ======================================== + +def can_motor_contral(direction=0, angle=100, microstep=32, can_id=0x01, channel='vcan0', listen_feedback=False): + print("=== 开始发送CAN位置控制指令 ===") + print(f"参数：方向={direction}（0=逆时针，1=顺时针），角度={angle}°，细分={microstep}") + + # 生成CAN数据帧 + can_data = generate_position_command(direction=direction, microstep=microstep, angle=angle) + print(f"生成的CAN数据帧: [{', '.join(f'0x{x:02X}' for x in can_data)}]") + + # 发送CAN帧 + send_can_frame(can_data, can_id=can_id, channel=channel) + + # 如果启用监听，等待反馈 + if listen_feedback: + listen_can_feedback(channel=channel) + + print("=== 电机控制流程完成 ===") + +# ======================================== +# 程序入口（直接运行时使用） +# ======================================== +if __name__ == "__main__": + # 这里写死参数，方便调试 + can_motor_contral( + direction=1, # 顺时针 + angle=180, # 180度 + microstep=16, # 细分值16 + can_id=0x02, # CAN ID 0x02 + channel='vcan0', # 使用虚拟CAN + listen_feedback=True # 是否监听反馈 + ) \ No newline at end of file diff --git a/qt/can_test_motor/can_run_demo.py b/qt/can_test_motor/can_run_demo.py new file mode 100644 index 0000000..0df8e0f --- /dev/null +++ b/qt/can_test_motor/can_run_demo.py @@ -0,0 +1,15 @@ +# can_run_demo.py 调用程序demo + +from can_main import can_motor_contral + + + +if __name__ == "__main__": + can_motor_contral( + direction=1, + angle=90, + microstep=32, + can_id=0x03, + channel='vcan0', + listen_feedback=True + ) \ No newline at end of file diff --git a/qt/can_test_motor/motor_control_can_all.py b/qt/can_test_motor/motor_control_can_all.py new file mode 100644 index 0000000..acd7914 --- /dev/null +++ b/qt/can_test_motor/motor_control_can_all.py @@ -0,0 +1,174 @@ +#!/usr/bin/env python +# 不同模式can发送指令集成 +# -*- coding: utf-8 -*- +''' +# @Time : 2025/7/21 +# @Author : hx +# @File : motor_control_can_all.py +''' + +import can +import time +import argparse + + +# 细分值映射表 +MICROSTEP_MAP = { + 2: 0x01, + 4: 0x02, + 8: 0x04, + 16: 0x10, + 32: 0x20 +} + +# ======================================== +# CAN 发送函数（使用 socketcan） +# ======================================== +def send_can_frame(data, can_id=0x01, channel='can0', bustype='socketcan'): + """ + 发送CAN帧 + :param data: 7字节的列表 + :param can_id: CAN ID + :param channel: CAN 通道 + :param bustype: 总线类型 + """ + try: + bus = can.interface.Bus(channel=channel, bustype=bustype) + msg = can.Message(arbitration_id=can_id, data=data, is_extended_id=False) + bus.send(msg) + print(f"[发送CAN帧] ID={can_id:02X} 数据=[{', '.join(f'0x{x:02X}' for x in data)}]") + bus.shutdown() + except Exception as e: + print(f"[发送失败] {e}") + + +# ======================================== +# 模式1：速度控制模式 (0x01) +# ======================================== +def generate_speed_command(direction, microstep, speed): + """ + 生成速度控制指令 + :param direction: 方向 (0:逆时针, 1:顺时针) + :param microstep: 细分值 (2, 4, 8, 16, 32) + :param speed: 速度值 (单位: rad/s) + """ + direction_byte = 0x01 if direction == 1 else 0x00 + microstep_byte = MICROSTEP_MAP.get(microstep, 0x20) + + raw_speed = int(speed * 10) + speed_high = (raw_speed >> 8) & 0xFF + speed_low = raw_speed & 0xFF + return [0x01, direction_byte, microstep_byte, 0x00, 0x00, speed_high, speed_low] + + +# ======================================== +# 模式2：位置控制模式 (0x02) +# ======================================== +def generate_position_command(direction, microstep, angle): + """ + 生成位置控制指令 + :param direction: 方向 (0:逆时针, 1:顺时针) + :param microstep: 细分值 (2, 4, 8, 16, 32) + :param angle: 角度值 (单位: 度) + """ + direction_byte = 0x01 if direction == 1 else 0x00 + microstep_byte = MICROSTEP_MAP.get(microstep, 0x20) + + raw_angle = int(angle * 10) + pos_high = (raw_angle >> 8) & 0xFF + pos_low = raw_angle & 0xFF + + return [0x02, direction_byte, microstep_byte, pos_high, pos_low, 0x00, 0x64] + + +# ======================================== +# 模式3：力矩控制模式 (0x03) +# ======================================== +def generate_torque_command(direction, microstep, current): + """ + 生成力矩控制指令 + :param direction: 方向 (0:逆时针, 1:顺时针) + :param microstep: 细分值 (2, 4, 8, 16, 32) + :param current: 电流值 (单位: mA) + """ + direction_byte = 0x01 if direction == 1 else 0x00 + microstep_byte = MICROSTEP_MAP.get(microstep, 0x20) + + raw_current = int(current) + current_high = (raw_current >> 8) & 0xFF + current_low = raw_current & 0xFF + + return [0x03, direction_byte, microstep_byte, current_high, current_low, 0x00, 0x64] + + +# ======================================== +# 模式4：单圈绝对角度控制模式 (0x04) +# ======================================== +def generate_absolute_angle_command(direction, microstep, angle, speed): + """ + 生成单圈绝对角度控制指令 + :param direction: 方向 (0:逆时针, 1:顺时针) + :param microstep: 细分值 (2, 4, 8, 16, 32) + :param angle: 目标角度 (单位: 度) + :param speed: 速度值 (单位: rad/s) + """ + direction_byte = 0x01 if direction == 1 else 0x00 + microstep_byte = MICROSTEP_MAP.get(microstep, 0x20) + + raw_angle = int(angle * 10) + pos_high = (raw_angle >> 8) & 0xFF + pos_low = raw_angle & 0xFF + + raw_speed = int(speed * 10) + speed_high = (raw_speed >> 8) & 0xFF + speed_low = raw_speed & 0xFF + + + return [0x04, direction_byte, microstep_byte, pos_high, pos_low, speed_high, speed_low] + + +# ======================================== +# 主函数（命令行调用） +# ======================================== +def main(): + parser = argparse.ArgumentParser(description="CAN电机控制程序，支持4种模式") + parser.add_argument("--mode", type=int, choices=[1, 2, 3, 4], + help="控制模式: 1=速度, 2=位置, 3=力矩, 4=绝对角度") + parser.add_argument("--direction", type=int, choices=[0, 1], default=1, + help="方向: 0=逆时针, 1=顺时针") + parser.add_argument("--microstep", type=int, choices=[2, 4, 8, 16, 32], default=32, + help="细分值: 2, 4, 8, 16, 32") + parser.add_argument("--value", type=float, required=True, + help="控制值（速度: rad/s, 位置/角度: 度, 力矩: mA）") + parser.add_argument("--speed_rad_per_sec", type=float, + help="速度值（仅用于绝对角度模式）") + args = parser.parse_args() + + try: + if args.mode == 1: + cmd = generate_speed_command(args.direction, args.microstep, args.value) + elif args.mode == 2: + cmd = generate_position_command(args.direction, args.microstep, args.value) + elif args.mode == 3: + cmd = generate_torque_command(args.direction, args.microstep, args.value) + elif args.mode == 4: + if args.speed_rad_per_sec is None: + raise ValueError("绝对角度模式需要提供速度参数 (--speed_rad_per_sec)") + cmd = generate_absolute_angle_command(args.direction, args.microstep, args.value, args.speed_rad_per_sec) + + print(f"生成CAN指令: [{', '.join(f'0x{x:02X}' for x in cmd)}]") + send_can_frame(cmd) + + except Exception as e: + print(f"错误: {e}") + + +# ======================================== +# 程序入口 +# ======================================== +if __name__ == "__main__": + main() + #python motor_control_can_all.py --mode 1 --direction 1 --microstep 32 --value 10 + #python motor_control_can_all.py --mode 2 --direction 1 --microstep 32 --value 1000 + #python motor_control_can_all.py --mode 3 --direction 1 --microstep 32 --value 1000 + diff --git a/qt/qt_main.py b/qt/qt_main.py new file mode 100644 index 0000000..8687957 --- /dev/null +++ b/qt/qt_main.py @@ -0,0 +1,146 @@ +import sys +import numpy as np +import matplotlib + +matplotlib.use('Qt5Agg') # 必须在导入 pyplot 前设置 + +from PyQt5.QtWidgets import QApplication, QDialog, QVBoxLayout +from PyQt5.QtGui import QStandardItemModel, QStandardItem # ✅ 正确的模块 +from calculate.trajectory import circle_trajectory, line_trajectory +from calculate.ik import inverseF +from matplotlib.figure import Figure +from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas +from matplotlib.animation import FuncAnimation + +# 导入你生成的界面 +from untitled import Ui_Dialog + +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 +L0 = 250 + + +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) + self.ax.set_title("五连杆沿轨迹运动") + + # 创建画布 + self.canvas = FigureCanvas(self.figure) + self.widget.setLayout(QVBoxLayout()) + self.widget.layout().addWidget(self.canvas) + + # 添加 line 对象 + self.line, = self.ax.plot([], [], 'r-o', linewidth=2, markersize=6) + # 初始化轨迹数据 + self.x_list = [] + self.y_list = [] + self.ani = None # 动画对象 + + # 连接按钮 + self.pushButton.clicked.connect(self.start_line_trajectory) + self.pushButton_2.clicked.connect(self.start_circle_trajectory) + + def start_line_trajectory(self): + # 清除旧的角度数据显示 + self.angle_model.clear() + + self.ax.set_title("五连杆 - 直线轨迹运动") + x, y = line_trajectory(start=(125, 300), end=(125, 400)) + self.start_animation(x, y) + + def start_circle_trajectory(self): + # 清除旧的角度数据显示 + self.angle_model.clear() + + self.ax.set_title("五连杆 - 圆形轨迹运动") + x, y = circle_trajectory(center=(100, 200), radius=40) + self.start_animation(x, y) + + def start_animation(self, x_list, y_list): + self.x_list = x_list + self.y_list = y_list + + # 安全停止旧动画 + try: + if self.ani is not None and self.ani.event_source is not None: + self.ani.event_source.stop() + except: + pass # 忽略停止过程中的任何异常 + + + # 动画函数 + 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) + angle_text = f"点 {i}: θ1={theta1_deg:.1f}°, θ4={theta4_deg:.1f}°" + + # 限制 listView 只显示最近 10 条 + if self.angle_model.rowCount() >= 10: + self.angle_model.removeRow(0) + # 创建新项并添加 + item = QStandardItem(angle_text) + self.angle_model.appendRow(item) + + # 左侧电机臂末端 + x2 = L1 * np.cos(theta1) + y2 = L1 * np.sin(theta1) + + # 右侧电机臂末端 + x4 = L4 * np.cos(theta4) + L0 + 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) + except Exception as e: + print(f"第 {i} 帧错误: {e}") + return self.line, + + # 创建新动画 + self.ani = FuncAnimation( + self.figure, + draw_frame, + frames=len(x_list), + interval=100, + repeat=False , + blit=False # PyQt 中 blit=True 有时有问题，先设 False + ) + self.canvas.draw() + + +if __name__ == "__main__": + app = QApplication(sys.argv) + dialog = MyDialog() + dialog.show() + sys.exit(app.exec_()) \ No newline at end of file diff --git a/qt/qt_motor_main.py b/qt/qt_motor_main.py new file mode 100644 index 0000000..eea04f8 --- /dev/null +++ b/qt/qt_motor_main.py @@ -0,0 +1,305 @@ +#!/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_()) \ No newline at end of file diff --git a/qt/untitled.py b/qt/untitled.py new file mode 100644 index 0000000..78075de --- /dev/null +++ b/qt/untitled.py @@ -0,0 +1,46 @@ +# -*- coding: utf-8 -*- + +# Form implementation generated from reading ui file 'untitled.ui' +# +# Created by: PyQt5 UI code generator 5.15.10 +# +# WARNING: Any manual changes made to this file will be lost when pyuic5 is +# run again. Do not edit this file unless you know what you are doing. + + +from PyQt5 import QtCore, QtGui, QtWidgets + + +class Ui_Dialog(object): + def setupUi(self, Dialog): + Dialog.setObjectName("Dialog") + Dialog.resize(851, 811) + self.pushButton = QtWidgets.QPushButton(Dialog) + self.pushButton.setGeometry(QtCore.QRect(10, 20, 111, 25)) + self.pushButton.setObjectName("pushButton") + self.widget = QtWidgets.QWidget(Dialog) + self.widget.setGeometry(QtCore.QRect(10, 260, 801, 541)) + self.widget.setObjectName("widget") + self.pushButton_2 = QtWidgets.QPushButton(Dialog) + self.pushButton_2.setGeometry(QtCore.QRect(140, 20, 111, 25)) + self.pushButton_2.setObjectName("pushButton_2") + self.label = QtWidgets.QLabel(Dialog) + self.label.setGeometry(QtCore.QRect(10, 70, 131, 17)) + self.label.setObjectName("label") + self.label_2 = QtWidgets.QLabel(Dialog) + self.label_2.setGeometry(QtCore.QRect(10, 240, 131, 21)) + self.label_2.setObjectName("label_2") + self.listView = QtWidgets.QListView(Dialog) + self.listView.setGeometry(QtCore.QRect(10, 90, 801, 141)) + self.listView.setObjectName("listView") + + self.retranslateUi(Dialog) + QtCore.QMetaObject.connectSlotsByName(Dialog) + + def retranslateUi(self, Dialog): + _translate = QtCore.QCoreApplication.translate + Dialog.setWindowTitle(_translate("Dialog", "Dialog")) + self.pushButton.setText(_translate("Dialog", "直线轨迹移动")) + self.pushButton_2.setText(_translate("Dialog", "圆形轨迹移动")) + self.label.setText(_translate("Dialog", "电机转动角度")) + self.label_2.setText(_translate("Dialog", "轨迹可视化")) diff --git a/qt/untitled.ui b/qt/untitled.ui new file mode 100644 index 0000000..4572cfa --- /dev/null +++ b/qt/untitled.ui @@ -0,0 +1,91 @@ + + + Dialog + + + + 0 + 0 + 851 + 811 + + + + Dialog + + + + + 10 + 20 + 111 + 25 + + + + 直线轨迹移动 + + + + + + 10 + 260 + 801 + 541 + + + + + + + 140 + 20 + 111 + 25 + + + + 圆形轨迹移动 + + + + + + 10 + 70 + 131 + 17 + + + + 电机转动角度 + + + + + + 10 + 240 + 131 + 21 + + + + 轨迹可视化 + + + + + + 10 + 90 + 801 + 141 + + + + + + + diff --git a/test1.py b/test1.py new file mode 100644 index 0000000..aabf481 --- /dev/null +++ b/test1.py @@ -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() \ No newline at end of file diff --git a/test_chuankou.py b/test_chuankou.py new file mode 100644 index 0000000..b675856 --- /dev/null +++ b/test_chuankou.py @@ -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("串口已关闭") \ No newline at end of file diff --git a/test_duoji.py b/test_duoji.py new file mode 100644 index 0000000..1f335f2 --- /dev/null +++ b/test_duoji.py @@ -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() \ No newline at end of file