diff --git a/main.py b/main.py index a220f79..68f2e09 100644 --- a/main.py +++ b/main.py @@ -13,8 +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 控制真实电机 +DEBUG_MODE = False # <<< 设为 False 控制真实电机 # 导入运动学和轨迹函数（确保路径正确） try: @@ -90,35 +89,15 @@ def init_motors(): if DEBUG_MODE: print("【DEBUG】跳过电机初始化") - # 给个简单的 mock，包含 getPosition 接口（返回度数） - class MockMotor: - def __init__(self): - self._pos_deg = 90.0 # 默认为 90°（即偏移后的值） - def getPosition(self): - return self._pos_deg - def setPosition(self, deg): - self._pos_deg = deg - - motor1 = MockMotor() - motor2 = MockMotor() - class MockControl: - def enable(self, m): return True - def disable(self, m): return None - def controlMIT(self, m, kp, kd, pos, vel, torq): - # pos 为弧度，记录到mock motor（用于DEBUG可视化一致性） - try: - deg = np.degrees(pos + np.pi/2) # 因为 motor 的 getPosition 返回带偏移的度值 - if m is motor1: - motor1.setPosition(deg) - elif m is motor2: - motor2.setPosition(deg) - except: - pass - return None - def refresh_motor_status(self, m): return None - def switchControlMode(self, m, mode): return None - def save_motor_param(self, m): return None - motor_control = MockControl() + 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: @@ -144,14 +123,32 @@ def init_motors(): print("电机已使能。") return motor1, motor2, motor_control -# ------------------------ MIT 控制函数 ------------------------ +# 电机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 控制指令（角度单位：弧度） - 参数: theta1_rad, theta4_rad —— 目标角度（弧度），这里 theta 已去除 ANGLE_OFFSET_RAD + 带限位检测：如果超限，自动触发回零。 """ 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 @@ -161,174 +158,69 @@ def control_two_motors_mit(theta1_rad, theta4_rad): motor_control.controlMIT(motor1, KP, KD, pos1, vel, torq) motor_control.controlMIT(motor2, KP, KD, pos4, vel, torq) else: - # DEBUG 模式：打印并更新 mock motor（mock control 已在 init 中实现） print(f"[DEBUG] 控制 -> θ1={np.degrees(theta1_rad):.2f}°, θ4={np.degrees(theta4_rad):.2f}°") - motor_control.controlMIT(motor1, KP, KD, pos1, vel, torq) - motor_control.controlMIT(motor2, KP, KD, pos4, vel, torq) current_theta1 = theta1_rad current_theta4 = theta4_rad -# ------------------------ 平滑移动到起点（插值过渡）------------------------ -def move_to_start_interpolated(start_x, start_y, steps=60, dt=DT): - """ - 从电机当前位置平滑插值过渡到轨迹起点 - 返回：time_log_pre, theta1_log_pre, theta4_log_pre, x_fk_log_pre, y_fk_log_pre - """ - ANGLE_OFFSET_RAD = np.pi / 2 - - # 计算目标角度（逆解） - try: - theta1_target_raw, theta4_target_raw = inverseF(start_x, start_y, L1, L2, L3, L4, L0) - theta1_target = float(theta1_target_raw) - ANGLE_OFFSET_RAD - theta4_target = float(theta4_target_raw) - ANGLE_OFFSET_RAD - except Exception as e: - print(f"起点逆解失败: {e}") - return [], [], [], [], [] - - # 获取当前电机角度（转换为去偏移的弧度值） - if not DEBUG_MODE: - try: - motor_control.refresh_motor_status(motor1) - motor_control.refresh_motor_status(motor2) - time.sleep(0.05) - pos1_deg = motor1.getPosition() # 电机读数通常为度（含偏移） - pos2_deg = motor2.getPosition() - current_theta1 = np.radians(pos1_deg) - ANGLE_OFFSET_RAD - current_theta4 = np.radians(pos2_deg) - ANGLE_OFFSET_RAD - except Exception as e: - print(f"读取电机当前位置失败: {e}; 使用默认 0") - current_theta1 = current_theta4 = 0.0 - else: - # DEBUG 下使用 mock motor 的 getPosition（init 已设置） - try: - pos1_deg = motor1.getPosition() - pos2_deg = motor2.getPosition() - current_theta1 = np.radians(pos1_deg) - ANGLE_OFFSET_RAD - current_theta4 = np.radians(pos2_deg) - ANGLE_OFFSET_RAD - except: - current_theta1 = current_theta4 = 0.0 - - # 生成插值序列（考虑角度连续性） - theta1_target = adjust_angle_continuity(theta1_target, current_theta1) - theta4_target = adjust_angle_continuity(theta4_target, current_theta4) - theta1_list = np.linspace(current_theta1, theta1_target, steps) - theta4_list = np.linspace(current_theta4, theta4_target, steps) - - # 日志 - time_log = [] - theta1_log = [] - theta4_log = [] - x_fk_log = [] - y_fk_log = [] - - print(f"平滑移动到起点 -> θ1_target={np.degrees(theta1_target):.2f}°, θ4_target={np.degrees(theta4_target):.2f}° (steps={steps})") - - start_t = perf_counter() - l1, l2, l3, l4, l5 = L1, L2, L3, L4, L0 - omega1 = omega4 = 0.0 - alpha1 = alpha4 = 0.0 - - for t1, t4 in zip(theta1_list, theta4_list): - # 发送控制 - control_two_motors_mit(float(t1), float(t4)) - - # FK 验证（使用带偏移角度） - try: - xc, yc, *_ = forwardF( - u1=float(t1 + ANGLE_OFFSET_RAD), - u4=float(t4 + ANGLE_OFFSET_RAD), - omega1=omega1, omega4=omega4, - l1=l1, l2=l2, l3=l3, l4=l4, l5=l5, - alpha1=alpha1, alpha4=alpha4 - ) - except Exception: - xc, yc = np.nan, np.nan - - elapsed = perf_counter() - start_t - time_log.append(elapsed) - theta1_log.append(np.degrees(float(t1))) # 记录发送给电机的角度（度） - theta4_log.append(np.degrees(float(t4))) - x_fk_log.append(xc) - y_fk_log.append(yc) - - busy_wait(dt) - - return time_log, theta1_log, theta4_log, x_fk_log, y_fk_log # ------------------------ 直接轨迹执行函数（无插值）------------------------ from time import perf_counter import numpy as np -def execute_direct_trajectory(x_list_in, y_list_in, dt=DT): +# 假设其他所需函数和全局变量已定义，例如 inverseF, forwardF, adjust_angle_continuity, control_two_motors_mit, 等等. + +def execute_direct_trajectory(x_list, y_list, dt=DT): """ - 直接执行轨迹：对每个 (x,y) 计算逆解并立即发送角度，不做任何插值或速度规划 + 直接执行轨迹：对每个 (x,y) 计算逆解并立即发送角度，不做插值或速度规划 + 执行前：先把电机从零点平滑移动到轨迹起点 返回: (time_log, theta1_log, theta4_log, x_fk_log, y_fk_log) """ global motor_control, motor1, motor2 - # 定义90度对应于弧度的偏移量 - ANGLE_OFFSET_RAD = np.pi / 2 # 即90度对应的弧度值 - - time_log = [] - theta1_log = [] - theta4_log = [] - x_fk_log = [] - y_fk_log = [] + 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 - # 获取当前角度（用于连续性调整） - if not DEBUG_MODE: - try: - 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) - current_theta1 = np.radians(motor1.getPosition()) - ANGLE_OFFSET_RAD - current_theta4 = np.radians(motor2.getPosition()) - ANGLE_OFFSET_RAD - except Exception: - # 若读取失败则以前次 global 值或 0 为准 - current_theta1 = current_theta4 = 0.0 - else: - # DEBUG 下 mock motor 初始已设置为 90deg，减偏移得到 0 - try: - current_theta1 = np.radians(motor1.getPosition()) - ANGLE_OFFSET_RAD - current_theta4 = np.radians(motor2.getPosition()) - ANGLE_OFFSET_RAD - except Exception: - current_theta1 = current_theta4 = 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_in, y_list_in)): + 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 = current_theta1 - target_theta4 = current_theta4 + target_theta1, target_theta4 = current_theta1, current_theta4 else: current_theta1, current_theta4 = target_theta1, target_theta4 - # === 直接发送角度到电机（假设motor api接受弧度值）=== + # 控制电机 control_two_motors_mit(target_theta1, target_theta4) - # === FK 验证实际到达位置 === + # FK 验证 try: xc, yc, *_ = forwardF( u1=target_theta1 + ANGLE_OFFSET_RAD, @@ -339,21 +231,21 @@ def execute_direct_trajectory(x_list_in, y_list_in, dt=DT): ) x_fk_log.append(xc) y_fk_log.append(yc) - except Exception: + 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)) # 将弧度转回角度以便记录（电机实际角度） + 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] @@ -367,7 +259,7 @@ def draw_frame(i): x_coords = [0, x2, x, x4, L0] y_coords = [0, y2, y, y4, 0] line.set_data(x_coords, y_coords) - except Exception: + except: line.set_data([], []) return line, @@ -379,13 +271,6 @@ def run_trajectory_with_animation(trajectory_func, **params): x_list, y_list = trajectory_func(**params) print(f"轨迹点数: {len(x_list)}") - if len(x_list) == 0: - print("轨迹点为空，退出。") - return - - # --- 平滑移动到轨迹起点（插值过渡），并收集 pre-move 日志 --- - t_pre, th1_pre, th4_pre, xfk_pre, yfk_pre = move_to_start_interpolated(x_list[0], y_list[0], steps=60, dt=DT) - # --- 动画 --- fig, ax = plt.subplots(figsize=(10, 8)) ax.set_xlim(-50, L0 + 100) @@ -398,62 +283,25 @@ def run_trajectory_with_animation(trajectory_func, **params): 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) + ani = FuncAnimation(fig, draw_frame, frames=len(x_list), + interval=50, blit=True, repeat=False) plt.show() - # --- 执行并记录（正向）--- - print("开始执行轨迹（正向）...") - t_fwd, th1_fwd, th4_fwd, xfk_fwd, yfk_fwd = execute_direct_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，这个地方控制电机延时 + ) - # --- 执行并记录（反向）--- - print("开始执行轨迹（反向）...") - t_rev, th1_rev, th4_rev, xfk_rev, yfk_rev = execute_direct_trajectory(x_list[::-1], y_list[::-1], dt=DT) - - # --- 合并三个阶段的数据（仅用于角度时间线显示） --- - time_offset = 0.0 - all_time = [] - all_th1 = [] - all_th4 = [] - all_xfk = [] - all_yfk = [] - - # add pre - for dt0, a, b, c, d in zip(t_pre, th1_pre, th4_pre, xfk_pre, yfk_pre): - all_time.append(time_offset + dt0) - all_th1.append(a) - all_th4.append(b) - all_xfk.append(c) - all_yfk.append(d) - if len(t_pre) > 0: - time_offset = all_time[-1] - - # add fwd (shifted) - for dt0, a, b, c, d in zip(t_fwd, th1_fwd, th4_fwd, xfk_fwd, yfk_fwd): - all_time.append(time_offset + dt0 + DT) - all_th1.append(a) - all_th4.append(b) - all_xfk.append(c) - all_yfk.append(d) - if len(t_fwd) > 0: - time_offset = all_time[-1] - - # add rev (shifted) - for dt0, a, b, c, d in zip(t_rev, th1_rev, th4_rev, xfk_rev, yfk_rev): - all_time.append(time_offset + dt0 + DT) - all_th1.append(a) - all_th4.append(b) - all_xfk.append(c) - all_yfk.append(d) - - # --- 绘制关节角度（包含预定位） --- + # --- 绘制关节角度 --- fig2, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 6), sharex=True) - ax1.plot(all_time, all_th1, 'b-o', markersize=3, linewidth=1.0, label='θ₁ (电机1)') + ax1.plot(time_log, theta1_log, 'b-o', markersize=3, linewidth=1.5, label='θ₁ (电机1)') ax1.set_ylabel('关节角 θ₁ (°)') - ax1.set_title('电机关节角度随时间变化（含预定位 + 正向 + 反向）') + ax1.set_title('电机关节角度随时间变化') ax1.grid(True, alpha=0.5) ax1.legend() - ax2.plot(all_time, all_th4, 'r-o', markersize=3, linewidth=1.0, label='θ₄ (电机2)') + 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) @@ -461,80 +309,157 @@ def run_trajectory_with_animation(trajectory_func, **params): plt.tight_layout() plt.show() - # --- FK 验证（只使用正向段；若需要也绘制反向段）--- + # --- FK 验证 --- fig3, ax3 = plt.subplots(figsize=(10, 8)) ax3.plot(x_list, y_list, 'b--', linewidth=2, label='目标轨迹', alpha=0.8) - - # 正向 FK（对比目标） - if len(xfk_fwd) > 0: - ax3.plot(xfk_fwd, yfk_fwd, 'g-', linewidth=2, marker='o', markersize=3, label='FK 重建轨迹（正向）', alpha=0.8) - # 标注正向起点/终点（若存在有效点） - valid_idx = [i for i, (xx, yy) in enumerate(zip(xfk_fwd, yfk_fwd)) if not (np.isnan(xx) or np.isnan(yy))] - if len(valid_idx) > 0: - ax3.scatter(xfk_fwd[valid_idx[0]], yfk_fwd[valid_idx[0]], c='green', s=100, marker='s', label='正向起点') - ax3.scatter(xfk_fwd[valid_idx[-1]], yfk_fwd[valid_idx[-1]], c='lime', s=100, marker='x', label='正向终点') - - # 反向 FK（对比反向目标） - if len(xfk_rev) > 0: - # xfk_rev 对应的是 x_list[::-1] - ax3.plot(xfk_rev, yfk_rev, 'm-', linewidth=1.5, marker='x', markersize=3, label='FK 重建轨迹（反向）', alpha=0.7) - + 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.set_title('FK 验证：目标 vs 实际轨迹') ax3.grid(True, alpha=0.5) ax3.legend() - ax3.set_aspect('equal', adjustable='box') + ax3.set_aspect('equal') plt.tight_layout() plt.show() - # --- 误差统计（仅统计正向段的误差，与目标 x_list/y_list 对比） --- - mask = ~np.isnan(xfk_fwd) & ~np.isnan(yfk_fwd) - if np.sum(mask) == 0: - print("【警告】正向段 FK 数据全为 nan，无法计算误差") - else: + # --- 误差统计 --- + 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}°") + +# ==================== 获取电机当前位置 ===================== +def get_current_motor_position(): + """ + 获取当前电机角度（电机原始单位，通常是度），并打印。 + 返回：theta1_current, theta4_current（度） + """ + if not DEBUG_MODE: try: - valid_x = np.array(xfk_fwd)[mask] - valid_y = np.array(yfk_fwd)[mask] - # 用线性插值重采样 valid 序列到目标轨迹长度 - from scipy.interpolate import interp1d - t_fk = np.linspace(0, 1, len(valid_x)) - f_x = interp1d(t_fk, valid_x, kind='linear', fill_value='extrapolate') - f_y = interp1d(t_fk, valid_y, kind='linear', fill_value='extrapolate') - t_target = np.linspace(0, 1, len(x_list)) - 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}") + 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 len(all_th1) > 0 and len(all_th4) > 0: - print("\n=== 关节角度范围统计 ===") - print(f"θ₁ 范围: {min(all_th1):.2f}° ~ {max(all_th1):.2f}°") - print(f"θ₄ 范围: {min(all_th4):.2f}° ~ {max(all_th4):.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': 40, 'num_points': 100} - # } - run_trajectory_with_animation(trajectory_config['func'], **trajectory_config['params']) except KeyboardInterrupt: @@ -543,11 +468,9 @@ if __name__ == "__main__": print(f"程序异常: {e}") finally: if not DEBUG_MODE: - try: - motor_control.disable(motor1) - motor_control.disable(motor2) - print("电机已停机。") - except Exception: - pass + motor_control.disable(motor1) + motor_control.disable(motor2) + print("电机已停机。") else: print("【DEBUG】程序结束") + diff --git a/mian_test.py b/mian_test.py new file mode 100644 index 0000000..355733d --- /dev/null +++ b/mian_test.py @@ -0,0 +1,130 @@ +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()