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

test_duoji.py

@@ -0,0 +1,95 @@
+import time
+import numpy as np
+
+# 导入运动学函数
+from calculate.ik import inverseF
+
+# 导入串口控制函数
+from duoji_test.motor2 import send_hex_command, generate_position_command
+
+# 机械臂参数（和 qt_main.py 保持一致）
+L1 = 250
+L2 = 300
+L3 = 300
+L4 = 250
+L0 = 250
+
+# 串口配置
+MOTOR1_PORT = "/dev/ttyACM1"  # 电机1使用的串口
+MOTOR2_PORT = "/dev/ttyACM0" # 电机2使用的串口
+BAUD_RATE = 115200
+
+# 控制参数
+SPEED_MOTOR1 = 10  # Rad/s
+SPEED_MOTOR2 = 10  # Rad/s
+DIRECTION_MOTOR1 = 1  # 0: 逆时针, 1: 顺时针
+DIRECTION_MOTOR2 = 1
+
+# 初始化电机角度为0
+current_angle_motor1 = 0
+current_angle_motor2 = 0
+
+
+def control_two_motors(theta1, theta4):
+    """
+    根据 theta1 和 theta4 控制两个电机转动指定角度
+    :param theta1: 左臂角度（弧度）
+    :param theta4: 右臂角度（弧度）
+    """
+    global current_angle_motor1, current_angle_motor2
+
+    # 弧度转角度
+    angle1_target = np.degrees(theta1)
+    angle4_target = np.degrees(theta4)
+
+    # 计算相对角度
+    relative_angle1 = angle1_target - current_angle_motor1
+    relative_angle4 = angle4_target - current_angle_motor2
+
+    print(f"发送相对角度: 电机1={relative_angle1:.2f}°, 电机2={relative_angle4:.2f}°")
+
+    # 更新当前角度
+    current_angle_motor1 += relative_angle1
+    current_angle_motor2 += relative_angle4
+
+    # 生成指令
+    cmd1 = generate_position_command(angle=relative_angle1, speed=SPEED_MOTOR1, direction=DIRECTION_MOTOR1)
+    cmd2 = generate_position_command(angle=relative_angle4, speed=SPEED_MOTOR2, direction=DIRECTION_MOTOR2)
+
+    # 发送指令到两个电机
+    send_hex_command(MOTOR1_PORT, BAUD_RATE, cmd1)
+    send_hex_command(MOTOR2_PORT, BAUD_RATE, cmd2)
+
+
+def run_trajectory():
+    """
+    主函数：模拟轨迹点并控制电机
+    """
+
+    # 模拟一个圆形轨迹（你可以替换成从文件读取或用户输入）
+    center = (150, 250)
+    radius = 60
+    num_points = 100
+    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)
+
+    print("开始执行轨迹控制...")
+
+    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)
+            control_two_motors(theta1, theta4)
+        except Exception as e:
+            print(f"第 {i} 点计算失败: {e}")
+
+        time.sleep(0.1)  # 控制发送频率（可根据实际需求调整）
+
+    print("轨迹执行完成。")
+
+
+if __name__ == "__main__":
+    run_trajectory()