feat: 初始化项目，添加电机控制、CAN通信、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()