From 02268ca185dbabe741abe29808b5a9f103bcf665 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E7=90=89=E7=92=83=E6=9C=88=E5=85=89?= <15630071+llyg777@user.noreply.gitee.com> Date: Wed, 17 Sep 2025 16:39:51 +0800 Subject: [PATCH] =?UTF-8?q?mian.py=E6=B5=8B=E8=AF=95=E5=AE=8C=E6=88=90?= =?UTF-8?q?=E7=9A=84=E5=A4=9A=E8=BD=A8=E8=BF=B9=E8=BF=90=E5=8A=A8=E6=8E=A7?= =?UTF-8?q?=E5=88=B6=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- calculate/trajectory.py | 6 +- 5dof_motor_main.py => else/5dof_motor_main.py | 0 5dof_motor_test.py => else/5dof_motor_test.py | 0 .../5dof_test_ctrl_motor.py | 0 main_limit.py => else/main_limit.py | 0 new_main.py => else/new_main.py | 0 qiege_motor.py => else/qiege_motor.py | 0 test1.py => else/test1.py | 0 test_chuankou.py => else/test_chuankou.py | 0 test_duoji.py => else/test_duoji.py | 0 jicheng1.py | 457 ---------------- main.py | 81 ++- main_test.py | 490 ++++++++++++++++++ mian_test.py | 130 ----- 14 files changed, 561 insertions(+), 603 deletions(-) rename 5dof_motor_main.py => else/5dof_motor_main.py (100%) rename 5dof_motor_test.py => else/5dof_motor_test.py (100%) rename 5dof_test_ctrl_motor.py => else/5dof_test_ctrl_motor.py (100%) rename main_limit.py => else/main_limit.py (100%) rename new_main.py => else/new_main.py (100%) rename qiege_motor.py => else/qiege_motor.py (100%) rename test1.py => else/test1.py (100%) rename test_chuankou.py => else/test_chuankou.py (100%) rename test_duoji.py => else/test_duoji.py (100%) delete mode 100644 jicheng1.py create mode 100644 main_test.py delete mode 100644 mian_test.py diff --git a/calculate/trajectory.py b/calculate/trajectory.py index 11b59ba..cf0b530 100644 --- a/calculate/trajectory.py +++ b/calculate/trajectory.py @@ -2,14 +2,14 @@ import numpy as np -def circle_trajectory(center=(80, 0), radius=40, num_points=60): +def circle_trajectory(center=(80, 0), radius=40, num_points=200): """ 圆形轨迹 """ 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) return x_list, y_list -def line_trajectory(start=(40, 0), end=(120, 0), num_points=20): +def line_trajectory(start=(40, 0), end=(120, 0), num_points=100): """ 直线轨迹 """ t = np.linspace(0, 1, num_points) x_list = start[0] + t * (end[0] - start[0]) @@ -53,7 +53,7 @@ def line_trajectory_fix(start=(40, 0), end=(120, 100), vx=0.1, vy=0.1, num_point return x_list, y_list -def ellipse_trajectory(center=(80, 0), rx=50, ry=25, num_points=60): +def ellipse_trajectory(center=(80, 0), rx=50, ry=25, num_points=200): """ 椭圆轨迹 """ angles = np.linspace(0, 2 * np.pi, num_points) x_list = center[0] + rx * np.cos(angles) diff --git a/5dof_motor_main.py b/else/5dof_motor_main.py similarity index 100% rename from 5dof_motor_main.py rename to else/5dof_motor_main.py diff --git a/5dof_motor_test.py b/else/5dof_motor_test.py similarity index 100% rename from 5dof_motor_test.py rename to else/5dof_motor_test.py diff --git a/5dof_test_ctrl_motor.py b/else/5dof_test_ctrl_motor.py similarity index 100% rename from 5dof_test_ctrl_motor.py rename to else/5dof_test_ctrl_motor.py diff --git a/main_limit.py b/else/main_limit.py similarity index 100% rename from main_limit.py rename to else/main_limit.py diff --git a/new_main.py b/else/new_main.py similarity index 100% rename from new_main.py rename to else/new_main.py diff --git a/qiege_motor.py b/else/qiege_motor.py similarity index 100% rename from qiege_motor.py rename to else/qiege_motor.py diff --git a/test1.py b/else/test1.py similarity index 100% rename from test1.py rename to else/test1.py diff --git a/test_chuankou.py b/else/test_chuankou.py similarity index 100% rename from test_chuankou.py rename to else/test_chuankou.py diff --git a/test_duoji.py b/else/test_duoji.py similarity index 100% rename from test_duoji.py rename to else/test_duoji.py diff --git a/jicheng1.py b/jicheng1.py deleted file mode 100644 index a1f3985..0000000 --- a/jicheng1.py +++ /dev/null @@ -1,457 +0,0 @@ -# ==================== 五连杆机械臂轨迹控制集成版（S型速度 + 五次插值）==================== -# 功能：轨迹生成 + 逆解 + S型速度规划 + 五次多项式平滑控制 + 动画显示 + 关节角度可视化 + FK 验证 -# ======================================================================================= - -import time -import numpy as np -import matplotlib.pyplot as plt -from matplotlib.animation import FuncAnimation -from time import perf_counter # 高精度计时 - -# 设置中文字体和负号显示 -plt.rcParams['font.sans-serif'] = ['SimHei', 'WenQuanYi Zen Hei', 'FangSong'] -plt.rcParams['axes.unicode_minus'] = False - -# ------------------------ 调试开关 ------------------------ -DEBUG_MODE = True # <<< 设为 False 控制真实电机 - -# 导入运动学和轨迹函数（确保路径正确） -try: - from calculate.ik import inverseF - from calculate.fk import forwardF - from calculate.trajectory import ( - circle_trajectory, - line_trajectory, - line_trajectory_fix, - ellipse_trajectory, - square_trajectory, - triangle_trajectory - ) -except ImportError as e: - print(f"【警告】无法导入运动学模块: {e}, 使用 DEBUG_MODE") - DEBUG_MODE = True - -# -------------------- 非 Debug 模式导入硬件库 -------------------- -if not DEBUG_MODE: - try: - from DM_CAN.DM_CAN import Motor, MotorControl, DM_Motor_Type, Control_Type - import serial - except ImportError as e: - print(f"硬件库导入失败: {e}") - exit(1) -else: - print("【DEBUG MODE】: 已启用调试模式，不连接真实硬件。") - -# ------------------------ 机械臂参数 ------------------------ -L1 = 250 # 左臂长度 (mm) -L2 = 300 -L3 = 300 -L4 = 250 # 右臂长度 -L0 = 250 # 基座右端偏移 - -# ------------------------ 电机与通信配置 ------------------------ -MOTOR1_ID = 0x01 -MOTOR2_ID = 0x04 -CAN_SERIAL_PORT = '/dev/ttyACM0' -BAUD_RATE = 921600 -KP = 50.0 -KD = 1.0 -DT = 0.001 # 控制周期 1ms - -# ------------------------ 全局变量 ------------------------ -motor1 = motor2 = motor_control = None -current_theta1 = current_theta4 = 0.0 -x_list = y_list = [] # 轨迹点 -line = None - -# ------------------------ 精确延时 ------------------------ -def busy_wait(dt): - """高精度延时""" - start = perf_counter() - while perf_counter() - start < dt: - pass - -# ------------------------ 角度连续性调整 ------------------------ -def adjust_angle_continuity(new_angle, prev_angle): - """ - 防止角度跳变（如 3.14 → -3.14），保持连续 - """ - diff = new_angle - prev_angle - while diff > np.pi: - diff -= 2 * np.pi - while diff < -np.pi: - diff += 2 * np.pi - return prev_angle + diff - -# ==================== 五次多项式插值 ==================== -def quintic_interpolation(q0, qf, t, T): - """ - 五次多项式平滑插值 - q0: 起始位置 - qf: 目标位置 - t: 当前时间（0~T） - T: 总时间 - 返回：平滑位置 - """ - s = t / T - s = np.clip(s, 0, 1) # 限制在0~1 - alpha = s**3 * (10 - 15*s + 6*s**2) - return q0 + (qf - q0) * alpha - -# ==================== S型速度规划（5段式）==================== -def s_curve_timing(total_time, dt=0.001, max_accel_ratio=2.0): - """ - 生成 S 型速度剖面的时间参数化 s(t) ∈ [0,1] - 使用 5 段：加加速 → 匀加速 → 匀速 → 匀减速 → 减减速 - """ - t_list = np.arange(0, total_time, dt) - if len(t_list) == 0: - t_list = np.array([0]) - - # 参数：最大加速度设为 4v/T（S型典型） - v_max = 0.5 * max_accel_ratio * total_time # 归一化速度 - a_max = 4 * v_max / total_time # 加速度 - t_jerk = v_max / a_max # 加加速时间 - t_acc = 2 * t_jerk # 总加速时间 - t_cruise = total_time - 2 * t_acc - - if t_cruise < 0: - t_acc = total_time / 2 - t_jerk = t_acc / 2 - t_cruise = 0 - - s_list = [] - for t in t_list: - if t <= t_jerk: - s = (a_max / t_jerk) * t**3 / 6 - elif t <= t_acc - t_jerk: - t2 = t - t_jerk - s = (a_max / t_jerk) * t_jerk**3 / 6 + a_max * t_jerk * t2 + 0.5 * a_max * t2**2 - elif t <= t_acc: - t3 = t - (t_acc - t_jerk) - s1 = (a_max / t_jerk) * t_jerk**3 / 6 - s2 = a_max * t_jerk * (t_acc - 2*t_jerk) + 0.5 * a_max * (t_acc - 2*t_jerk)**2 - s3 = a_max * t_jerk * t3 - 0.5 * a_max * t3**2 - s = s1 + s2 + s3 - elif t <= t_acc + t_cruise: - t4 = t - t_acc - s = s_list[-1] if s_list else 0 - s += v_max * t4 - else: - t5 = t - (t_acc + t_cruise) - if t5 <= t_jerk: - s = 1 - (v_max * (total_time - t) - 0.5 * a_max * t5**2) - elif t5 <= t_acc - t_jerk: - s = 1 - (v_max * (total_time - t - t_jerk) - 0.5 * a_max * t_jerk**2) - else: - t6 = t5 - (t_acc - t_jerk) - s = 1 - (a_max / t_jerk) * t6**3 / 6 - s_list.append(s) - - # 归一化到 [0,1] - s_max = max(s_list) if s_list else 1 - s_normalized = np.array(s_list) / s_max if s_max > 0 else np.zeros_like(s_list) - return t_list, s_normalized - -# ------------------------ 初始化电机 ------------------------ -def init_motors(): - global motor1, motor2, motor_control - - if DEBUG_MODE: - print("【DEBUG】跳过电机初始化") - motor1 = motor2 = type('Motor', (), {'id': 0})() - motor_control = type('MotorControl', (), { - 'enable': lambda x: True, - 'disable': lambda x: None, - 'controlMIT': lambda m, kp, kd, pos, vel, torq: None, - 'refresh_motor_status': lambda m: None, - 'switchControlMode': lambda m, mode: None, - 'save_motor_param': lambda m: None - })() - return motor1, motor2, motor_control - - try: - can_serial = serial.Serial(CAN_SERIAL_PORT, BAUD_RATE, timeout=0.5) - print(f"CAN 串口 {CAN_SERIAL_PORT} 打开成功") - except Exception as e: - print(f"无法打开串口: {e}") - exit(1) - - motor_control = MotorControl(can_serial) - motor1 = Motor(DM_Motor_Type.DM4310, MOTOR1_ID, 0x11) - motor2 = Motor(DM_Motor_Type.DM4310, MOTOR2_ID, 0x15) - - motor_control.addMotor(motor1) - motor_control.addMotor(motor2) - motor_control.switchControlMode(motor1, Control_Type.MIT) - motor_control.switchControlMode(motor2, Control_Type.MIT) - time.sleep(0.1) - motor_control.save_motor_param(motor1) - motor_control.save_motor_param(motor2) - motor_control.enable(motor1) - motor_control.enable(motor2) - print("电机已使能。") - return motor1, motor2, motor_control - -# ------------------------ MIT 控制函数 ------------------------ -def control_two_motors_mit(theta1, theta4): - """发送 MIT 控制指令（角度单位：弧度）""" - global current_theta1, current_theta4 - - pos1 = np.degrees(theta1) - pos4 = np.degrees(theta4) - vel = 0.0 - torq = 0.0 - - if not DEBUG_MODE: - motor_control.controlMIT(motor1, KP, KD, pos1, vel, torq) - motor_control.controlMIT(motor2, KP, KD, pos4, vel, torq) - else: - print(f"[DEBUG] 控制 -> θ1={pos1:.2f}°, θ4={pos4:.2f}°") - - current_theta1 = theta1 - current_theta4 = theta4 - -# ------------------------ 平滑轨迹执行函数（带 S 型速度）------------------------ -def execute_smooth_trajectory(x_list, y_list, - total_time=8.0, - kp=KP, kd=KD, dt=DT, - read_interval=0.01): - """ - 执行平滑轨迹：使用 S 型速度剖面 + 五次多项式插值 - 返回: (time_log, theta1_log, theta4_log, x_fk_log, y_fk_log) - """ - global motor_control, motor1, motor2 - - time_log = [] - theta1_log = [] - theta4_log = [] - x_fk_log = [] - y_fk_log = [] - - # FK 参数 - l1, l2, l3, l4, l5 = L1, L2, L3, L4, L0 - omega1 = omega4 = 0.0 - alpha1 = alpha4 = 0.0 - - # 获取起始角度 - if not DEBUG_MODE: - motor_control.refresh_motor_status(motor1) - motor_control.refresh_motor_status(motor2) - time.sleep(0.1) - motor_control.refresh_motor_status(motor1) - motor_control.refresh_motor_status(motor2) - start_theta1 = np.radians(motor1.getPosition()) - start_theta4 = np.radians(motor2.getPosition()) - else: - start_theta1 = start_theta4 = 0.0 - - # 生成 S 型时间剖面 - t_profile, s_profile = s_curve_timing(total_time=total_time, dt=dt) - if len(s_profile) == 0: - s_profile = np.linspace(0, 1, len(x_list)) - - # 插值轨迹点（使用 S 型进度） - num_s = len(s_profile) - x_interp = np.interp(s_profile, np.linspace(0, 1, len(x_list)), x_list) - y_interp = np.interp(s_profile, np.linspace(0, 1, len(y_list)), y_list) - - prev_theta1 = start_theta1 - prev_theta4 = start_theta4 - - start_time = perf_counter() - - for i in range(num_s): - x = x_interp[i] - y = y_interp[i] - - try: - target_theta1_raw, target_theta4_raw = inverseF(x, y, L1, L2, L3, L4, L0) - target_theta1 = float(target_theta1_raw) - target_theta4 = float(target_theta4_raw) - except Exception as e: - print(f"逆解失败: ({x:.2f}, {y:.2f}) -> {e}") - target_theta1 = prev_theta1 - target_theta4 = prev_theta4 - else: - target_theta1 = adjust_angle_continuity(target_theta1, prev_theta1) - target_theta4 = adjust_angle_continuity(target_theta4, prev_theta4) - - # 五次多项式插值（单点瞬时） - T_seg = dt - steps = 1 # 每个点只执行一次 - for step in range(steps): - t_step = step * T_seg / steps - theta1 = quintic_interpolation(prev_theta1, target_theta1, t_step, T_seg) - theta4 = quintic_interpolation(prev_theta4, target_theta4, t_step, T_seg) - - control_two_motors_mit(theta1, theta4) - - # FK 验证 - try: - xc, yc, _, _, _, _, _, _ = forwardF( - u1=theta1, u4=theta4, omega1=omega1, omega4=omega4, - l1=l1, l2=l2, l3=l3, l4=l4, l5=l5, alpha1=alpha1, alpha4=alpha4 - ) - x_fk_log.append(xc) - y_fk_log.append(yc) - except: - x_fk_log.append(np.nan) - y_fk_log.append(np.nan) - - # 读取反馈 - now = perf_counter() - if not DEBUG_MODE and (now - start_time) % read_interval < dt: - motor_control.refresh_motor_status(motor1) - motor_control.refresh_motor_status(motor2) - - time_log.append(now - start_time) - theta1_log.append(np.degrees(theta1)) - theta4_log.append(np.degrees(theta4)) - - next_time = start_time + t_profile[i] + dt - busy_wait(next_time - perf_counter()) - - prev_theta1, prev_theta4 = target_theta1, target_theta4 - - return time_log, theta1_log, theta4_log, x_fk_log, y_fk_log - -# ------------------------ 动画绘制函数 ------------------------ -def draw_frame(i): - x = x_list[i] - y = y_list[i] - try: - theta1, theta4 = inverseF(x, y, L1, L2, L3, L4, L0) - x2 = L1 * np.cos(theta1) - y2 = L1 * np.sin(theta1) - x4 = L0 + L4 * np.cos(theta4) - y4 = L4 * np.sin(theta4) - x_coords = [0, x2, x, x4, L0] - y_coords = [0, y2, y, y4, 0] - line.set_data(x_coords, y_coords) - except: - line.set_data([], []) - return line, - -# ------------------------ 轨迹动画与执行 + 可视化 + FK 验证 ------------------------ -def run_trajectory_with_animation(trajectory_func, **params): - global x_list, y_list, line - - print(f"生成轨迹: {trajectory_func.__name__}") - x_list, y_list = trajectory_func(**params) - print(f"轨迹点数: {len(x_list)}") - - # --- 动画 --- - fig, ax = plt.subplots(figsize=(10, 8)) - ax.set_xlim(-50, L0 + 100) - ax.set_ylim(0, 500) - ax.set_aspect('equal') - ax.grid(True, alpha=0.6) - ax.set_title(f"五连杆机械臂 - 轨迹: {trajectory_func.__name__}") - - ax.plot(x_list, y_list, 'b--', alpha=0.5, label='目标轨迹') - line, = ax.plot([], [], 'r-o', lw=3, ms=6, label='机械臂') - ax.legend() - - ani = FuncAnimation(fig, draw_frame, frames=len(x_list), - interval=50, blit=True, repeat=False) - plt.show() - - # --- 执行并记录 --- - print("开始执行轨迹（S型速度 + 五次插值）...") - time_log, theta1_log, theta4_log, x_fk_log, y_fk_log = execute_smooth_trajectory( - x_list, y_list, - total_time=8.0, - kp=50.0, kd=1.0, dt=0.001 - ) - - # --- 绘制关节角度 --- - 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)') - ax1.set_ylabel('关节角 θ₁ (°)') - ax1.set_title('电机关节角度随时间变化') - ax1.grid(True, alpha=0.5) - ax1.legend() - - ax2.plot(time_log, theta4_log, 'r-o', markersize=3, linewidth=1.5, label='θ₄ (电机2)') - ax2.set_xlabel('时间 (秒)') - ax2.set_ylabel('关节角 θ₄ (°)') - ax2.grid(True, alpha=0.5) - ax2.legend() - plt.tight_layout() - plt.show() - - # --- FK 验证 --- - fig3, ax3 = plt.subplots(figsize=(10, 8)) - ax3.plot(x_list, y_list, 'b--', linewidth=2, label='目标轨迹', alpha=0.8) - ax3.plot(x_fk_log, y_fk_log, 'g-', linewidth=2, marker='o', markersize=3, label='FK 重建轨迹', alpha=0.7) - ax3.scatter(x_fk_log[0], y_fk_log[0], c='green', s=100, marker='s', label='起点') - ax3.scatter(x_fk_log[-1], y_fk_log[-1], c='red', s=100, marker='x', label='终点') - ax3.set_xlabel('X (mm)') - ax3.set_ylabel('Y (mm)') - ax3.set_title('FK 验证：目标 vs 实际轨迹') - ax3.grid(True, alpha=0.5) - ax3.legend() - ax3.set_aspect('equal') - plt.tight_layout() - plt.show() - - # --- 误差统计 --- - mask = ~np.isnan(x_fk_log) & ~np.isnan(y_fk_log) - x_fk_valid = np.array(x_fk_log)[mask] - y_fk_valid = np.array(y_fk_log)[mask] - if len(x_fk_valid) == 0: - 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 - try: - f_x = interp1d(t_fk, x_fk_valid, kind='linear', fill_value='extrapolate') - f_y = interp1d(t_fk, y_fk_valid, kind='linear', fill_value='extrapolate') - x_interp = f_x(t_target) - y_interp = f_y(t_target) - errors = np.sqrt((x_interp - x_list)**2 + (y_interp - y_list)**2) - print("\n=== FK 验证误差统计 ===") - print(f"平均误差: {np.mean(errors):.3f} mm") - print(f"最大误差: {np.max(errors):.3f} mm") - print(f"标准差: {np.std(errors):.3f} mm") - 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}°") - -# ------------------------ 主函数 ------------------------ -if __name__ == "__main__": - try: - init_motors() - - # 选择轨迹 - trajectory_config = { - 'func': line_trajectory_fix, - 'params': {'start': (25, 300), 'end': (200, 300), 'num_points': 100} - } - - # 示例：圆形轨迹 - # trajectory_config = { - # 'func': circle_trajectory, - # 'params': {'center': (100, 300), 'radius': 40, 'num_points': 100} - # } - - run_trajectory_with_animation(trajectory_config['func'], **trajectory_config['params']) - - except KeyboardInterrupt: - print("\n【用户中断】") - except Exception as e: - print(f"程序异常: {e}") - finally: - if not DEBUG_MODE: - motor_control.disable(motor1) - motor_control.disable(motor2) - print("电机已停机。") - else: - print("【DEBUG】程序结束") \ No newline at end of file diff --git a/main.py b/main.py index 68f2e09..6b8a118 100644 --- a/main.py +++ b/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【用户中断】") diff --git a/main_test.py b/main_test.py new file mode 100644 index 0000000..a377f21 --- /dev/null +++ b/main_test.py @@ -0,0 +1,490 @@ +# ==================== 五连杆机械臂轨迹控制（直接角度发送版）==================== +# 功能：轨迹生成 + 逆解 + 直接控制 + 动画显示 + 关节角度可视化 + FK 验证 +# ============================================================================== + +import time +import numpy as np +import matplotlib.pyplot as plt +from matplotlib.animation import FuncAnimation +from time import perf_counter # 高精度计时 + +# 设置中文字体和负号显示 +plt.rcParams['font.sans-serif'] = ['SimHei', 'WenQuanYi Zen Hei', 'FangSong'] +plt.rcParams['axes.unicode_minus'] = False + +# ------------------------ 调试开关 ------------------------ +DEBUG_MODE = False # <<< 设为 False 控制真实电机 + +# 导入运动学和轨迹函数（确保路径正确） +try: + from calculate.ik import inverseF + from calculate.fk import forwardF + from calculate.trajectory import ( + circle_trajectory, + line_trajectory, + line_trajectory_fix, + ellipse_trajectory, + square_trajectory, + triangle_trajectory + ) +except ImportError as e: + print(f"【警告】无法导入运动学模块: {e}, 使用 DEBUG_MODE") + DEBUG_MODE = True + +# -------------------- 非 Debug 模式导入硬件库 -------------------- +if not DEBUG_MODE: + try: + from DM_CAN.DM_CAN import Motor, MotorControl, DM_Motor_Type, Control_Type + import serial + except ImportError as e: + print(f"硬件库导入失败: {e}") + exit(1) +else: + print("【DEBUG MODE】: 已启用调试模式，不连接真实硬件。") + +# ------------------------ 机械臂参数 ------------------------ +L1 = 250 # 左臂长度 (mm) +L2 = 300 +L3 = 300 +L4 = 250 # 右臂长度 +L0 = 250 # 基座右端偏移 + +# ------------------------ 电机与通信配置 ------------------------ +MOTOR1_ID = 0x01 +MOTOR2_ID = 0x02 +CAN_SERIAL_PORT = '/dev/ttyACM0' +BAUD_RATE = 921600 +KP = 50.0 +KD = 1.0 +DT = 0.01# 控制周期 10ms（每点延时） + +# ------------------------ 全局变量 ------------------------ +motor1 = motor2 = motor_control = None +current_theta1 = current_theta4 = 0.0 +x_list = y_list = [] # 轨迹点 +line = None + +# ------------------------ 精确延时 ------------------------ +def busy_wait(dt): + """高精度延时""" + start = perf_counter() + while perf_counter() - start < dt: + pass + +# ------------------------ 角度连续性调整 ------------------------ +def adjust_angle_continuity(new_angle, prev_angle): + """ + 防止角度跳变（如 3.14 → -3.14），保持连续 + """ + diff = new_angle - prev_angle + while diff > np.pi: + diff -= 2 * np.pi + while diff < -np.pi: + diff += 2 * np.pi + return prev_angle + diff + +# ------------------------ 初始化电机 ------------------------ +def init_motors(): + global motor1, motor2, motor_control + + if DEBUG_MODE: + print("【DEBUG】跳过电机初始化") + motor1 = motor2 = type('Motor', (), {'id': 0})() + motor_control = type('MotorControl', (), { + 'enable': lambda x: True, + 'disable': lambda x: None, + 'controlMIT': lambda m, kp, kd, pos, vel, torq: None, + 'refresh_motor_status': lambda m: None, + 'switchControlMode': lambda m, mode: None, + 'save_motor_param': lambda m: None + })() + return motor1, motor2, motor_control + + try: + can_serial = serial.Serial(CAN_SERIAL_PORT, BAUD_RATE, timeout=0.5) + print(f"CAN 串口 {CAN_SERIAL_PORT} 打开成功") + except Exception as e: + print(f"无法打开串口: {e}") + exit(1) + + motor_control = MotorControl(can_serial) + motor1 = Motor(DM_Motor_Type.DM4310, MOTOR1_ID, 0x11) + motor2 = Motor(DM_Motor_Type.DM4310, MOTOR2_ID, 0x12) + + motor_control.addMotor(motor1) + motor_control.addMotor(motor2) + motor_control.switchControlMode(motor1, Control_Type.MIT) + motor_control.switchControlMode(motor2, Control_Type.MIT) + time.sleep(0.1) + motor_control.save_motor_param(motor1) + motor_control.save_motor_param(motor2) + motor_control.enable(motor1) + motor_control.enable(motor2) + print("电机已使能。") + return motor1, motor2, motor_control + +# 电机1、4安全角度范围（弧度） +THETA1_MIN = -np.pi/2 # -60° +THETA1_MAX = np.pi/2 # 60° +THETA4_MIN = -np.pi/2 # -60° +THETA4_MAX = np.pi/2 # 60° + +# ------------------------ MIT 控制函数（带限位 + 回零逻辑） ------------------------ +def control_two_motors_mit(theta1_rad, theta4_rad): + """ + 发送 MIT 控制指令（角度单位：弧度） + 带限位检测：如果超限，自动触发回零。 + """ + global current_theta1, current_theta4 + + # --- 限位检测 --- + if not (THETA1_MIN <= theta1_rad <= THETA1_MAX): + print(f"θ1 超出限位 [{THETA1_MIN}, {THETA1_MAX}]，执行回零...") + move_motors_to_zero() + return + + if not (THETA4_MIN <= theta4_rad <= THETA4_MAX): + print(f"θ4 超出限位 [{THETA4_MIN}, {THETA4_MAX}]，执行回零...") + move_motors_to_zero() + return + + # --- 正常控制 --- + pos1 = theta1_rad + pos4 = theta4_rad + vel = 0.1 + torq = 0.0 + + if not DEBUG_MODE: + motor_control.controlMIT(motor1, KP, KD, pos1, vel, torq) + motor_control.controlMIT(motor2, KP, KD, pos4, vel, torq) + else: + print(f"[DEBUG] 控制 -> θ1={np.degrees(theta1_rad):.2f}°, θ4={np.degrees(theta4_rad):.2f}°") + + current_theta1 = theta1_rad + current_theta4 = theta4_rad + + +# ------------------------ 直接轨迹执行函数（无插值）------------------------ +from time import perf_counter +import numpy as np + + +# 假设其他所需函数和全局变量已定义，例如 inverseF, forwardF, adjust_angle_continuity, control_two_motors_mit, 等等. + +def execute_direct_trajectory(x_list, y_list, dt=DT): + """ + 直接执行轨迹：对每个 (x,y) 计算逆解并立即发送角度，不做插值或速度规划 + 执行前：先把电机从零点平滑移动到轨迹起点 + 返回: (time_log, theta1_log, theta4_log, x_fk_log, y_fk_log) + """ + global motor_control, motor1, motor2 + + ANGLE_OFFSET_RAD = np.pi / 2 # 90° 偏移量 + + time_log, theta1_log, theta4_log, x_fk_log, y_fk_log = [], [], [], [], [] + l1, l2, l3, l4, l5 = L1, L2, L3, L4, L0 + omega1 = omega4 = 0.0 + alpha1 = alpha4 = 0.0 + + # --- 轨迹起点逆解 --- + x0, y0 = x_list[0], y_list[0] + try: + raw_theta1, raw_theta4 = inverseF(x0, y0, L1, L2, L3, L4, L0) + theta1_start = float(raw_theta1) - ANGLE_OFFSET_RAD + theta4_start = float(raw_theta4) - ANGLE_OFFSET_RAD + print(f"起点逆解成功 -> θ1={np.degrees(theta1_start):.2f}°, θ4={np.degrees(theta4_start):.2f}°") + except Exception as e: + print(f"❌ 起点逆解失败: {e}") + return [], [], [], [], [] + + # --- 电机移动到起点 --- + print("电机从零点移动到轨迹起点...") + move_motors_to_target(theta1_start, theta4_start, duration=1.5, dt=dt) + current_theta1, current_theta4 = theta1_start, theta4_start + + # --- 轨迹执行 --- + start_time = perf_counter() + for i, (x, y) in enumerate(zip(x_list, y_list)): + try: + raw_theta1, raw_theta4 = inverseF(x, y, L1, L2, L3, L4, L0) + target_theta1 = float(raw_theta1) - ANGLE_OFFSET_RAD + target_theta4 = float(raw_theta4) - ANGLE_OFFSET_RAD + target_theta1 = adjust_angle_continuity(target_theta1, current_theta1) + target_theta4 = adjust_angle_continuity(target_theta4, current_theta4) + except Exception as e: + print(f"第 {i} 点逆解失败 ({x:.2f}, {y:.2f}): {e}") + target_theta1, target_theta4 = current_theta1, current_theta4 + else: + current_theta1, current_theta4 = target_theta1, target_theta4 + + # 控制电机 + control_two_motors_mit(target_theta1, target_theta4) + + # FK 验证 + try: + xc, yc, *_ = forwardF( + u1=target_theta1 + ANGLE_OFFSET_RAD, + u4=target_theta4 + ANGLE_OFFSET_RAD, + omega1=omega1, omega4=omega4, + l1=l1, l2=l2, l3=l3, l4=l4, l5=l5, + alpha1=alpha1, alpha4=alpha4 + ) + x_fk_log.append(xc) + y_fk_log.append(yc) + except: + x_fk_log.append(np.nan) + y_fk_log.append(np.nan) + + # 记录日志 + elapsed = perf_counter() - start_time + time_log.append(elapsed) + theta1_log.append(np.degrees(target_theta1)) + theta4_log.append(np.degrees(target_theta4)) + + busy_wait(dt) + + return time_log, theta1_log, theta4_log, x_fk_log, y_fk_log + + +# ------------------------ 动画绘制函数 ------------------------ +def draw_frame(i): + x = x_list[i] + y = y_list[i] + try: + theta1, theta4 = inverseF(x, y, L1, L2, L3, L4, L0) + x2 = L1 * np.cos(theta1) + y2 = L1 * np.sin(theta1) + x4 = L0 + L4 * np.cos(theta4) + y4 = L4 * np.sin(theta4) + x_coords = [0, x2, x, x4, L0] + y_coords = [0, y2, y, y4, 0] + line.set_data(x_coords, y_coords) + except: + line.set_data([], []) + return line, + +# ------------------------ 轨迹循环动画与执行 + 可视化 + FK 验证 ------------------------ +def run_trajectory_with_animation_loop(trajectory_func, loop_count=3, **params): + """ + 循环执行轨迹指定次数，保持关节连续性 + loop_count: 循环次数 + """ + global x_list, y_list, line, current_theta1, current_theta4 + + print(f"生成轨迹: {trajectory_func.__name__}") + x_list, y_list = trajectory_func(**params) + print(f"轨迹点数: {len(x_list)}") + + # --- 动画初始化 --- + fig, ax = plt.subplots(figsize=(10, 8)) + ax.set_xlim(-50, L0 + 100) + ax.set_ylim(0, 500) + ax.set_aspect('equal') + ax.grid(True, alpha=0.6) + ax.set_title(f"五连杆机械臂 - 轨迹循环执行: {trajectory_func.__name__}") + ax.plot(x_list, y_list, 'b--', alpha=0.5, label='目标轨迹') + line, = ax.plot([], [], 'r-o', lw=3, ms=6, label='机械臂') + ax.legend() + plt.ion() + plt.show() + + for count in range(1, loop_count + 1): + print(f"\n=== 第 {count} 次执行轨迹 ===") + # --- 执行轨迹 --- + time_log, theta1_log, theta4_log, x_fk_log, y_fk_log = execute_direct_trajectory( + x_list, y_list, dt=DT + ) + + # --- 动画更新 --- + for i in range(len(x_list)): + draw_frame(i) + plt.pause(0.01) + + # --- 绘制关节角度 --- + 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)') + ax1.set_ylabel('关节角 θ₁ (°)') + ax1.set_title(f'电机关节角度随时间变化 - 第 {count} 次') + ax1.grid(True, alpha=0.5) + ax1.legend() + + ax2.plot(time_log, theta4_log, 'r-o', markersize=3, linewidth=1.5, label='θ₄ (电机2)') + ax2.set_xlabel('时间 (秒)') + ax2.set_ylabel('关节角 θ₄ (°)') + ax2.grid(True, alpha=0.5) + ax2.legend() + plt.tight_layout() + plt.show() + + # --- FK 验证 --- + fig3, ax3 = plt.subplots(figsize=(10, 8)) + ax3.plot(x_list, y_list, 'b--', linewidth=2, label='目标轨迹', alpha=0.8) + ax3.plot(x_fk_log, y_fk_log, 'g-', linewidth=2, marker='o', markersize=3, label='FK 重建轨迹', alpha=0.7) + ax3.scatter(x_fk_log[0], y_fk_log[0], c='green', s=100, marker='s', label='起点') + ax3.scatter(x_fk_log[-1], y_fk_log[-1], c='red', s=100, marker='x', label='终点') + ax3.set_xlabel('X (mm)') + ax3.set_ylabel('Y (mm)') + ax3.set_title(f'FK 验证：目标 vs 实际轨迹 - 第 {count} 次') + ax3.grid(True, alpha=0.5) + ax3.legend() + ax3.set_aspect('equal') + plt.tight_layout() + plt.show() + + # --- 误差统计 --- + mask = ~np.isnan(x_fk_log) & ~np.isnan(y_fk_log) + x_fk_valid = np.array(x_fk_log)[mask] + y_fk_valid = np.array(y_fk_log)[mask] + if len(x_fk_valid) == 0: + print("【警告】FK 数据全为 nan，无法计算误差") + continue + + t_target = np.linspace(0, 1, len(x_list)) + t_fk = np.linspace(0, 1, len(x_fk_valid)) + from scipy.interpolate import interp1d + try: + f_x = interp1d(t_fk, x_fk_valid, kind='linear', fill_value='extrapolate') + f_y = interp1d(t_fk, y_fk_valid, kind='linear', fill_value='extrapolate') + x_interp = f_x(t_target) + y_interp = f_y(t_target) + errors = np.sqrt((x_interp - x_list)**2 + (y_interp - y_list)**2) + print("\n=== FK 验证误差统计 ===") + print(f"平均误差: {np.mean(errors):.3f} mm") + print(f"最大误差: {np.max(errors):.3f} mm") + print(f"标准差: {np.std(errors):.3f} mm") + 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}°") + + plt.ioff() + print(f"\n=== 循环执行 {loop_count} 次完成 ===") + + +# ==================== 获取电机当前位置 ===================== +def get_current_motor_position(): + """ + 获取当前电机角度（电机原始单位，通常是度），并打印。 + 返回：theta1_current, theta4_current（度） + """ + if not DEBUG_MODE: + try: + motor_control.refresh_motor_status(motor1) + motor_control.refresh_motor_status(motor2) + time.sleep(0.05) + theta1_current = motor1.getPosition() + theta4_current = motor2.getPosition() + except Exception: + theta1_current = theta4_current = 0.0 + else: + theta1_current = motor1.getPosition() + theta4_current = motor2.getPosition() + + print(f"当前电机弧度 -> θ1={theta1_current:.2f}, θ4={theta4_current:.2f}") + return theta1_current, theta4_current + +# ==================== MIT 模式回零函数 ===================== +def move_motors_to_zero(duration=1.0, dt=DT): + """ + 使用 MIT 模式将电机从当前位置平滑回到零点 + duration: 总运动时间 (秒) + dt: 控制周期 (秒) + """ + global motor1, motor2, motor_control, current_theta1, current_theta4 + + # 获取当前位置 + theta1_start, theta4_start = get_current_motor_position() + # 计算步数 + steps = int(duration / dt) + for i in range(steps + 1): + t = i / steps + # 线性插值 + theta1_target = theta1_start * (1 - t) + theta4_target = theta4_start * (1 - t) + # 发送 MIT 控制 + control_two_motors_mit(theta1_target, theta4_target) + busy_wait(dt) + + # 确保最终位置为零 + control_two_motors_mit(0.0, 0.0) + current_theta1 = current_theta4 = 0.0 + print("电机已回零完成。") + +# ==================== MIT 模式移动到指定角度（任意两点） ===================== +def move_motors_to_target(theta1_target_rad, theta4_target_rad, duration=1.0, dt=DT, + theta1_start_rad=None, theta4_start_rad=None): + """ + 使用 MIT 模式将电机从起点平滑移动到指定目标角度 + 参数: + theta1_target_rad: 电机1目标角度（弧度） + theta4_target_rad: 电机4目标角度（弧度） + duration: 总运动时间 (秒) + dt: 控制周期 (秒) + theta1_start_rad: 电机1起始角度（弧度），默认 None 表示从零点 + theta4_start_rad: 电机4起始角度（弧度），默认 None 表示从零点 + """ + global motor1, motor2, motor_control, current_theta1, current_theta4 + + # 默认从零点出发 + if theta1_start_rad is None: + theta1_start_rad = 0.0 + if theta4_start_rad is None: + theta4_start_rad = 0.0 + + steps = int(duration / dt) + for i in range(steps + 1): + t = i / steps + # 线性插值 + theta1_now = theta1_start_rad + (theta1_target_rad - theta1_start_rad) * t + theta4_now = theta4_start_rad + (theta4_target_rad - theta4_start_rad) * t + # 发送 MIT 控制 + control_two_motors_mit(theta1_now, theta4_now) + busy_wait(dt) + + # 确保最终位置精确到目标 + control_two_motors_mit(theta1_target_rad, theta4_target_rad) + current_theta1 = theta1_target_rad + current_theta4 = theta4_target_rad + print(f"电机已移动到目标角度: θ1={np.degrees(theta1_target_rad):.2f}°, θ4={np.degrees(theta4_target_rad):.2f}°") + + +if __name__ == "__main__": + try: + # ---------------- 初始化电机 ---------------- + init_motors() + + # --- 获取当前位置测试 --- + print("\n=== 测试：获取电机当前位置 ===") + theta1, theta4 = get_current_motor_position() + + # --- MIT 模式回零 --- + print("\n=== MIT 模式回零 ===") + move_motors_to_zero(duration=1.0) # 主函数里执行回零 + + # ---------------- 选择轨迹 ---------------- + #trajectory_config = { + # 'func': line_trajectory_fix, + # 'params': {'start': (25, 300), 'end': (200, 300), 'num_points': 100} + #} + trajectory_config = { + 'func': circle_trajectory, + 'params': {'center': (100, 300), 'radius': 100} + } + run_trajectory_with_animation_loop(trajectory_config['func'], loop_count=2, **trajectory_config['params']) + + + except KeyboardInterrupt: + print("\n【用户中断】") + except Exception as e: + print(f"程序异常: {e}") + finally: + if not DEBUG_MODE: + motor_control.disable(motor1) + motor_control.disable(motor2) + print("电机已停机。") + else: + print("【DEBUG】程序结束") + diff --git a/mian_test.py b/mian_test.py deleted file mode 100644 index 355733d..0000000 --- a/mian_test.py +++ /dev/null @@ -1,130 +0,0 @@ -import time -import numpy as np -import matplotlib.pyplot as plt -from matplotlib.animation import FuncAnimation -from time import perf_counter - -# ------------------------ 调试开关 ------------------------ -DEBUG_MODE = False # True: 模拟电机, False: 真机 - -# ------------------------ 电机与控制 ------------------------ -if DEBUG_MODE: - class MockMotor: - def __init__(self): - self._pos_deg = 45.0 - def getPosition(self): - return self._pos_deg - def setPosition(self, deg): - self._pos_deg = deg - print(f"[DEBUG] 电机位置 -> {deg:.2f}°") - - class MockControl: - def enable(self, m): return True - def disable(self, m): return None - def controlMIT(self, m, kp, kd, pos, vel, torq): - deg = np.degrees(pos) - m.setPosition(deg) - - motor1 = MockMotor() - motor2 = MockMotor() - motor_control = MockControl() -else: - import serial - from DM_CAN.DM_CAN import Motor, MotorControl, DM_Motor_Type, Control_Type - - CAN_SERIAL_PORT = '/dev/ttyACM0' - BAUD_RATE = 921600 - MOTOR1_ID = 0x01 - MOTOR2_ID = 0x02 - - def init_real_motors(): - try: - can_serial = serial.Serial(CAN_SERIAL_PORT, BAUD_RATE, timeout=0.5) - print(f"CAN 串口 {CAN_SERIAL_PORT} 打开成功") - except Exception as e: - print(f"无法打开串口: {e}") - exit(1) - - mc = MotorControl(can_serial) - m1 = Motor(DM_Motor_Type.DM4310, MOTOR1_ID, 0x11) - m2 = Motor(DM_Motor_Type.DM4310, MOTOR2_ID, 0x12) - mc.addMotor(m1) - mc.addMotor(m2) - mc.switchControlMode(m1, Control_Type.MIT) - mc.switchControlMode(m2, Control_Type.MIT) - time.sleep(0.1) - mc.save_motor_param(m1) - mc.save_motor_param(m2) - mc.enable(m1) - mc.enable(m2) - print("真实电机已使能。") - return m1, m2, mc - -# ------------------------ 通用函数 ------------------------ -def busy_wait(dt): - start = perf_counter() - while perf_counter() - start < dt: - pass - -def adjust_angle_continuity(new_angle, prev_angle): - diff = new_angle - prev_angle - while diff > np.pi: - diff -= 2 * np.pi - while diff < -np.pi: - diff += 2 * np.pi - return prev_angle + diff - -def control_two_motors_mit(theta1_rad, theta4_rad, dt=0.01): - motor_control.controlMIT(motor1, 50, 1, theta1_rad, 0.1, 0.0) - motor_control.controlMIT(motor2, 50, 1, theta4_rad, 0.1, 0.0) - busy_wait(dt) - -# ------------------------ 移动到零点 ------------------------ -def move_to_zero_point(steps=60, dt=0.01): - theta1_current = np.radians(motor1.getPosition()) - theta4_current = np.radians(motor2.getPosition()) - theta1_list = np.linspace(theta1_current, 0.0, steps) - theta4_list = np.linspace(theta4_current, 0.0, steps) - - for t1, t4 in zip(theta1_list, theta4_list): - t1_adj = adjust_angle_continuity(t1, theta1_current) - t4_adj = adjust_angle_continuity(t4, theta4_current) - control_two_motors_mit(t1_adj, t4_adj, dt) - - print("已到达零点。") - print(f"电机1角度: {motor1.getPosition():.2f}°") - print(f"电机2角度: {motor2.getPosition():.2f}°") - -# ------------------------ 示例轨迹动画 ------------------------ -def draw_demo_trajectory(): - x_list = np.linspace(0, 200, 50) - y_list = 100 + 50 * np.sin(np.linspace(0, 2*np.pi, 50)) - - fig, ax = plt.subplots(figsize=(8, 6)) - ax.set_xlim(-50, 250) - ax.set_ylim(0, 250) - ax.set_aspect('equal') - ax.grid(True) - ax.plot(x_list, y_list, 'b--', label='示例轨迹') - line, = ax.plot([], [], 'ro-', lw=2, label='机械臂') - - def update(i): - line.set_data([0, x_list[i]], [0, y_list[i]]) - return line, - - ani = FuncAnimation(fig, update, frames=len(x_list), interval=50, blit=True, repeat=False) - ax.legend() - plt.show() - -# ------------------------ 主程序 ------------------------ -if __name__ == "__main__": - if not DEBUG_MODE: - motor1, motor2, motor_control = init_real_motors() - else: - print("【DEBUG模式】使用模拟电机") - - print(f"当前电机角度 -> 电机1: {motor1.getPosition():.2f}°, 电机2: {motor2.getPosition():.2f}°") - print("移动到零点...") - move_to_zero_point(steps=60, dt=0.01) - print("显示示例轨迹动画...") - draw_demo_trajectory()