feat: 初始化项目，添加电机控制、CAN通信、QT界面等模块

calculate/angle_A.txt

@@ -0,0 +1 @@
+2.25,2.24,2.23,2.22,2.21,2.20,2.19,2.19,2.18,2.17,2.16,2.15,2.14,2.12,2.11,2.10,2.09,2.08,2.07,2.05

calculate/angle_B.txt

@@ -0,0 +1 @@
+0.90,0.90,0.91,0.92,0.93,0.94,0.95,0.96,0.97,0.98,0.99,1.00,1.01,1.02,1.03,1.04,1.05,1.06,1.08,1.09

calculate/calculate_angle.py

@@ -0,0 +1,179 @@
+# qt_main.py
+
+import numpy as np
+import matplotlib.pyplot as plt
+from ik import inverseF  # 假设这是你自己的逆运动学函数
+from matplotlib.animation import FuncAnimation
+
+# 设置中文字体和解决负号显示问题
+plt.rcParams['font.sans-serif'] = ['SimHei', 'WenQuanYi Zen Hei', 'FangSong']  # 按优先级选择字体
+plt.rcParams['axes.unicode_minus'] = False  # 显示负号 -
+
+
+# 杆长参数
+L1 = 250
+L2 = 300
+L3 = 300
+L4 = 250
+L0 = 250
+
+
+# 1. 轨迹生成函数：每个轨迹类型独立封装，支持外部传参
+# --------------------------------------------------
+
+def generate_circle(center=(100, 300), radius=40):
+    from trajectory import circle_trajectory
+    return circle_trajectory(center=center, radius=radius)
+
+def generate_line(start=(125, 300), end=(125, 400)):
+    from trajectory import line_trajectory
+    return line_trajectory(start=start, end=end)
+
+def generate_ellipse(center=(100, 200), rx=50, ry=25):
+    from trajectory import ellipse_trajectory
+    return ellipse_trajectory(center=center, rx=rx, ry=ry)
+
+def generate_square(side=60, start_point=(100, 200)):
+    from trajectory import square_trajectory
+    return square_trajectory(side=side, start_point=start_point)
+
+def generate_triangle(base_length=100, height=80, base_center=(100, 200)):
+    from trajectory import triangle_trajectory
+    return triangle_trajectory(base_length=base_length, height=height, base_center=base_center)
+
+
+# 4. 主函数：根据轨迹类型调用对应函数并执行
+# --------------------------------------------------
+
+
+def main_of_5dof(trajectory_type='line', show_animation=True, save_angle_a='angle_A.txt',
+                 save_angle_b='angle_B.txt', **kwargs):
+    """
+    主函数：生成轨迹、计算逆解，并将 theta1 和 theta4 分别保存为两个 txt 文件，逗号分隔
+    参数:
+        - trajectory_type: 轨迹类型 ('circle', 'line', 'ellipse', 'square', 'triangle')
+        - show_animation: 是否显示动画
+        - save_angle_a: 保存 theta1（A角）的文件名，设为 None 不保存
+        - save_angle_b: 保存 theta4（B角）的文件名，设为 None 不保存
+        - **kwargs: 传递给轨迹生成函数的参数
+    """
+    # 生成轨迹
+    if trajectory_type == 'circle':
+        x_list, y_list = generate_circle(
+            center=kwargs.get('center', (100, 300)),
+            radius=kwargs.get('radius', 40)
+        )
+    elif trajectory_type == 'line':
+        x_list, y_list = generate_line(
+            start=kwargs.get('start', (125, 300)),
+            end=kwargs.get('end', (125, 400))
+        )
+    elif trajectory_type == 'ellipse':
+        x_list, y_list = generate_ellipse(
+            center=kwargs.get('center', (100, 200)),
+            rx=kwargs.get('rx', 50),
+            ry=kwargs.get('ry', 25)
+        )
+    elif trajectory_type == 'square':
+        x_list, y_list = generate_square(
+            side=kwargs.get('side', 60),
+            start_point=kwargs.get('start_point', (100, 200))
+        )
+    elif trajectory_type == 'triangle':
+        x_list, y_list = generate_triangle(
+            base_length=kwargs.get('base_length', 100),
+            height=kwargs.get('height', 80),
+            base_center=kwargs.get('base_center', (100, 200))
+        )
+    else:
+        raise ValueError(f"不支持的轨迹类型: {trajectory_type}")
+
+    # 存储角度值的列表
+    angle_A_list = []  # theta1 (弧度或角度)
+    angle_B_list = []  # theta4 (弧度或角度)
+
+    # 计算每个点的逆运动学并存储角度（以角度制保存）
+    for i in range(len(x_list)):
+        x = x_list[i]
+        y = y_list[i]
+        try:
+            theta1, theta4 = inverseF(x, y, L1, L2, L3, L4, L0)
+            angle_A_list.append(theta1)
+            angle_B_list.append(theta4)
+            print(f"第 {i} 个点: A角 = {theta1:.2f}°, B角 = {theta4:.2f}°")
+        except Exception as e:
+            print(f"第 {i} 点逆解失败: {e}")
+            # 可选择插入 NaN 或上一个有效值
+            angle_A_list.append(np.nan)
+            angle_B_list.append(np.nan)
+
+    # ==================== 保存为两个独立的 txt 文件 ====================
+    if save_angle_a:
+        with open(save_angle_a, 'w') as f:
+            # 将所有 A 角度转为字符串，保留2位小数，用逗号连接
+            formatted = ",".join([f"{angle:.2f}" for angle in angle_A_list])
+            f.write(formatted)
+        print(f"\n✅ A角度（theta1）已保存至: {save_angle_a}")
+
+    if save_angle_b:
+        with open(save_angle_b, 'w') as f:
+            formatted = ",".join([f"{angle:.2f}" for angle in angle_B_list])
+            f.write(formatted)
+        print(f"✅ B角度（theta4）已保存至: {save_angle_b}")
+
+    # ==================== 可选：显示动画 ====================
+    if show_animation:
+        fig, ax = plt.subplots()
+        ax.set_xlim(-300, 500)
+        ax.set_ylim(0, 500)
+        ax.set_aspect('equal')
+        ax.grid(True)
+        ax.set_title("五连杆末端沿轨迹运动")
+        ax.plot(x_list, y_list, 'b--', label='理想轨迹')
+        line, = ax.plot([], [], 'r-o', linewidth=2, markersize=6, label='五连杆结构')
+
+        def draw_frame(i):
+            x = x_list[i]
+            y = y_list[i]
+
+            try:
+                theta1, theta4 = inverseF(x, y, L1, L2, L3, L4, L0)
+            except Exception as e:
+                print(f"第 {i} 帧: 计算失败 -> {e}")
+                theta1 = theta4 = None
+
+            if theta1 is None or theta4 is None:
+                line.set_data([], [])
+                return line,
+
+            # 计算连杆坐标
+            x2 = L1 * np.cos(theta1)
+            y2 = L1 * np.sin(theta1)
+            x4 = L4 * np.cos(theta4) + L0
+            y4 = L4 * np.sin(theta4)
+
+            x_coords = [0, x2, x, x4, L0]
+            y_coords = [0, y2, y, y4, 0]
+
+            line.set_data(x_coords, y_coords)
+            return line,
+
+        ani = FuncAnimation(fig, draw_frame, frames=len(x_list), interval=50, blit=True)
+        plt.legend()
+        plt.show()
+
+# 📌 运行主函数
+if __name__ == "__main__":
+    main_of_5dof(trajectory_type='line',start=(125, 300), end=(125, 400), show_animation=False)
+    #main_of_5dof(
+    #    trajectory_type='circle',
+    #    center=(150, 250),
+    #    radius=60,
+    #    show_animation=False  # 设置为 False 则不显示动画
+    #)
+
+    # 示例：其他轨迹使用方式
+    # main_of_5dof(trajectory_type='line', start=(0, 0), end=(200, 300), show_animation=False)
+    # main_of_5dof(trajectory_type='ellipse', center=(100, 200), rx=80, ry=40)
+    # main_of_5dof(trajectory_type='square', side=100, start_point=(100, 200))
+    # main_of_5dof(trajectory_type='triangle', base_length=120, height=100, base_center=(100, 200))

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)

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

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()

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()

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()

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