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

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