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

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