commit 9477f25a5117e66f7673fc1a3424d5a2c6cc0f3e
Author: 琉璃月光 <15630071+llyg777@user.noreply.gitee.com>
Date: Wed Aug 13 12:36:04 2025 +0800
feat: 初始化项目，添加电机控制、CAN通信、QT界面等模块
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(' {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 0000000..d83a184
Binary files /dev/null and b/can_test_motor/__pycache__/can_main.cpython-38.pyc differ
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 0000000..330d00e
Binary files /dev/null and b/can_test_motor/__pycache__/motor_control_can_all.cpython-38.pyc differ
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 0000000..6e8ee4b
Binary files /dev/null and b/motor_control/__pycache__/can_main.cpython-38.pyc differ
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(' {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