mian.py测试完成的多轨迹运动控制代码

main.py

@@ -13,7 +13,7 @@ plt.rcParams['font.sans-serif'] = ['SimHei', 'WenQuanYi Zen Hei', 'FangSong']
 plt.rcParams['axes.unicode_minus'] = False
 
 # ------------------------ 调试开关 ------------------------
-DEBUG_MODE = False  # <<< 设为 False 控制真实电机
+DEBUG_MODE = True  # <<< 设为 False 控制真实电机
 
 # 导入运动学和轨迹函数（确保路径正确）
 try:
@@ -56,7 +56,7 @@ CAN_SERIAL_PORT = '/dev/ttyACM0'
 BAUD_RATE = 921600
 KP = 50.0
 KD = 1.0
-DT = 0.01  # 控制周期 10ms（每点延时）
+DT = 0.01# 控制周期 10ms（每点延时）
 
 # ------------------------ 全局变量 ------------------------
 motor1 = motor2 = motor_control = None
@@ -264,7 +264,8 @@ def draw_frame(i):
     return line,
 
 # ------------------------ 轨迹动画与执行 + 可视化 + FK 验证 ------------------------
-def run_trajectory_with_animation(trajectory_func, **params):
+# ------------------------ 轨迹动画预览 ------------------------
+def preview_trajectory(trajectory_func, **params):
     global x_list, y_list, line
 
     print(f"生成轨迹: {trajectory_func.__name__}")
@@ -286,13 +287,18 @@ def run_trajectory_with_animation(trajectory_func, **params):
     ani = FuncAnimation(fig, draw_frame, frames=len(x_list),
                         interval=50, blit=True, repeat=False)
     plt.show()
+    return x_list, y_list
 
-    # --- 执行并记录 ---
+
+# ------------------------ 执行轨迹 ------------------------
+def execute_trajectory(x_list, y_list, dt=DT):
     print("开始执行轨迹（直接发送角度）...")
-    time_log, theta1_log, theta4_log, x_fk_log, y_fk_log = execute_direct_trajectory(
-        x_list, y_list, dt=DT  # 每个点发送后延时 1ms，这个地方控制电机延时
-    )
+    return execute_direct_trajectory(x_list, y_list, dt=dt)
 
+
+# ------------------------ 可视化与误差统计 ------------------------
+# ------------------------ 可视化与误差统计 ------------------------
+def visualize_results(x_list, y_list, time_log, theta1_log, theta4_log, x_fk_log, y_fk_log):
     # --- 绘制关节角度 ---
     fig2, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 6), sharex=True)
     ax1.plot(time_log, theta1_log, 'b-o', markersize=3, linewidth=1.5, label='θ₁ (电机1)')
@@ -332,7 +338,6 @@ def run_trajectory_with_animation(trajectory_func, **params):
         print("【警告】FK 数据全为 nan，无法计算误差")
         return
 
-    # 插值对齐长度
     t_target = np.linspace(0, 1, len(x_list))
     t_fk = np.linspace(0, 1, len(x_fk_valid))
     from scipy.interpolate import interp1d
@@ -349,11 +354,39 @@ def run_trajectory_with_animation(trajectory_func, **params):
     except Exception as e:
         print(f"误差计算失败: {e}")
 
-    # --- 关节角度范围 ---
+    # --- 关节角度统计与保存 ---
     print("\n=== 关节角度范围统计 ===")
     print(f"θ₁ 范围: {min(theta1_log):.2f}° ~ {max(theta1_log):.2f}°")
     print(f"θ₄ 范围: {min(theta4_log):.2f}° ~ {max(theta4_log):.2f}°")
 
+# 角度转换为弧度，并组合成配对列表
+    theta1_rad = np.radians(theta1_log)
+    theta4_rad = np.radians(theta4_log)
+
+    # 构建嵌套列表：[[θ1_0, θ4_0], [θ1_1, θ41], ...]
+    angles_rad_list = [[t1, t4] for t1, t4 in zip(theta1_rad, theta4_rad)]
+
+    # 格式化为字符串：保留6位小数，用逗号分隔
+    nested_str = ",\n    ".join([f"[{t1:.6f}, {t4:.6f}]" for t1, t4 in angles_rad_list])
+    nested_str = f"[\n    {nested_str}\n]"
+
+    # 保存为 txt 文件
+    filename = 'joint_angles_rad_nested.txt'
+    with open(filename, 'w') as f:
+        f.write("angles_rad_list = ")
+        f.write(nested_str)
+        f.write("\n")
+
+    print(f"\n✅ 嵌套列表格式的弧度数据已保存至: {filename}")
+
+
+# ------------------------ 主流程 ------------------------
+def run_full_trajectory(trajectory_func, **params):
+    x_list, y_list = preview_trajectory(trajectory_func, **params)
+    time_log, theta1_log, theta4_log, x_fk_log, y_fk_log = execute_trajectory(x_list, y_list, dt=DT)
+    visualize_results(x_list, y_list, time_log, theta1_log, theta4_log, x_fk_log, y_fk_log)
+
+
 # ==================== 获取电机当前位置 =====================
 def get_current_motor_position():
     """
@@ -448,19 +481,41 @@ if __name__ == "__main__":
 
         # --- 获取当前位置测试 ---
         print("\n=== 测试：获取电机当前位置 ===")
-        theta1, theta4 = get_current_motor_position()
+        #theta1, theta4 = get_current_motor_position()
 
         # --- MIT 模式回零 ---
         print("\n=== MIT 模式回零 ===")
-        move_motors_to_zero(duration=1.0)  # 主函数里执行回零
+        #move_motors_to_zero(duration=1.0)  # 主函数里执行回零
 
         # ---------------- 选择轨迹 ----------------
+        '''
+        #直线轨迹：
         trajectory_config = {
             'func': line_trajectory_fix,
             'params': {'start': (25, 300), 'end': (200, 300), 'num_points': 100}
         }
-
-        run_trajectory_with_animation(trajectory_config['func'], **trajectory_config['params'])
+        
+        #圆形轨迹
+        trajectory_config = {
+             'func': circle_trajectory,
+             'params': {'center': (100, 300), 'radius': 100}
+        }
+        
+         trajectory_config = {
+            'func': square_trajectory,
+            'params': {'side':60, 'num_points': 200}
+        }
+        #椭圆轨迹
+        trajectory_config = {
+            'func': ellipse_trajectory,
+            'params': {'center': (100, 250), 'rx': 100, 'ry': 60, 'num_points': 200}
+        }
+        '''
+        trajectory_config = {
+             'func': circle_trajectory,
+             'params': {'center': (100, 300), 'radius': 100, 'num_points': 100}
+        }
+        run_full_trajectory(trajectory_config['func'], **trajectory_config['params'])
 
     except KeyboardInterrupt:
         print("\n【用户中断】")