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

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