From e28a3e5930d106216de29ffa45c5c3c8bf492736 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: Mon, 15 Sep 2025 10:45:15 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E7=94=B5=E6=9C=BA=E6=8E=A7?= =?UTF-8?q?=E5=88=B6=E6=97=B6=E9=97=B4=E5=BB=B6=E8=BF=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .idea/5dof.iml | 2 +- .idea/misc.xml | 2 +- 5dof_motor_main.py | 2 +- DM_control.py | 144 +++++++++++++ calculate/angle_A.txt | 1 - calculate/angle_B.txt | 1 - calculate/ik.py | 111 ++++++---- calculate/trajectory.py | 37 ++++ jicheng1.py | 457 ++++++++++++++++++++++++++++++++++++++++ main.py | 378 +++++++++++++++++++++++++++++++++ new_main.py | 301 ++++++++++++++++++++++++++ 11 files changed, 1389 insertions(+), 47 deletions(-) create mode 100644 DM_control.py delete mode 100644 calculate/angle_A.txt delete mode 100644 calculate/angle_B.txt create mode 100644 jicheng1.py create mode 100644 main.py create mode 100644 new_main.py diff --git a/.idea/5dof.iml b/.idea/5dof.iml index ef778ac..0c27a7f 100644 --- a/.idea/5dof.iml +++ b/.idea/5dof.iml @@ -2,7 +2,7 @@ - + \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml index ed483f3..c952d09 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -3,5 +3,5 @@ - + \ No newline at end of file diff --git a/5dof_motor_main.py b/5dof_motor_main.py index f618612..e024c76 100644 --- a/5dof_motor_main.py +++ b/5dof_motor_main.py @@ -199,7 +199,7 @@ if __name__ == "__main__": # 可选其他轨迹： trajectory_config = { 'func': line_trajectory, - 'params': {'start': (125, 300), 'end': (125, 400)} + 'params': {'start': (125, 300), 'end': (275, 300)} } #trajectory_config = { # 'func': circle_trajectory, diff --git a/DM_control.py b/DM_control.py new file mode 100644 index 0000000..18664ce --- /dev/null +++ b/DM_control.py @@ -0,0 +1,144 @@ +import time +import numpy as np +import matplotlib.pyplot as plt +from DM_CAN import * +import serial +from time import perf_counter, sleep + +# ================= 初始化电机 ================= +Motor1 = Motor(DM_Motor_Type.DM4310, 0x01, 0x11) +serial_device = serial.Serial('COM3', 921600, timeout=0.5) +MotorControl1 = MotorControl(serial_device) +MotorControl1.addMotor(Motor1) +MotorControl1.enable(Motor1) +sleep(1) +# ================= 五次多项式轨迹函数 ================= +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 + +# ================= 执行轨迹函数 ================= +def execute_trajectory(MotorControl, Motor, waypoints, + total_time=6.0, slow_down_time=2.0, + kp=50, kd=1.0, dt=0.001, read_interval=0.01, + ramp_time=2.0): + """ + 五次多项式轨迹执行函数（均分时间 + 缓启动 + 缓停） + """ + # 获取当前电机位置 + MotorControl.refresh_motor_status(Motor) + sleep(0.1) + MotorControl.refresh_motor_status(Motor) + current_pos = Motor.getPosition() + positions = [current_pos] + waypoints + + # ===== 均分时间，不按距离 ===== + num_segments = len(positions) - 1 + seg_times = [total_time / num_segments] * num_segments + + # ===== 轨迹执行 ===== + actual_pos, desired_pos, time_axis = [], [], [] + start_time = perf_counter() + next_time = start_time + last_read_time = start_time + + for i in range(num_segments): + q0 = positions[i] + qf = positions[i+1] + T = seg_times[i] + steps = int(T/dt) + for j in range(steps): + t = j*dt + # 五次多项式插值 + pos_cmd = quintic_interpolation(q0, qf, t, T) + # 启动缓动 + ramp = 0.5*(1 - np.cos(np.pi*min(t, ramp_time)/ramp_time)) if t < ramp_time else 1.0 + pos_cmd = current_pos + (pos_cmd - current_pos) * ramp + vel_cmd = 0 # 由五次插值保证平滑 + + MotorControl.controlMIT(Motor, kp, kd, pos_cmd, vel_cmd, 0) + + # 低频读取 + now = perf_counter() + if now - last_read_time >= read_interval: + MotorControl.refresh_motor_status(Motor) + actual = Motor.getPosition() + last_read_time = now + else: + actual = np.nan + + actual_pos.append(actual) + desired_pos.append(pos_cmd) + time_axis.append(now - start_time) + + # 固定周期 + next_time += dt + while perf_counter() < next_time: + pass + + current_pos = qf # 更新起点用于下一段 + + # ===== 缓停段 ===== + final_pos = positions[-1] + MotorControl.refresh_motor_status(Motor) + start_pos = Motor.getPosition() + steps = int(slow_down_time / dt) + for i in range(steps): + t = i * dt + pos_cmd = quintic_interpolation(start_pos, final_pos, t, slow_down_time) + vel_cmd = 0 + MotorControl.controlMIT(Motor, kp, kd, pos_cmd, vel_cmd, 0) + + now = perf_counter() + if now - last_read_time >= read_interval: + MotorControl.refresh_motor_status(Motor) + actual = Motor.getPosition() + last_read_time = now + else: + actual = np.nan + + actual_pos.append(actual) + desired_pos.append(pos_cmd) + time_axis.append(now - start_time) + + next_time += dt + while perf_counter() < next_time: + pass + + # ===== 绘图 ===== + actual_clean = np.array(actual_pos) + desired_clean = np.array(desired_pos) + mask = ~np.isnan(actual_clean) + + plt.figure() + plt.plot(np.array(time_axis)[mask], desired_clean[mask], label='Desired') + plt.plot(np.array(time_axis)[mask], actual_clean[mask], label='Actual', linestyle='--') + plt.xlabel('Time [s]') + plt.ylabel('Position [rad]') + plt.title(f'Quintic Trajectory (Ramp + Smooth Stop)') + plt.legend() + plt.grid(True) + plt.show() + + +# ================= 执行轨迹 ================= +target_angles = [ 0.0, 0.3, 0.6, 0.88, 0.9, 1.5, + 0.5, 0.2,-0.2,-0.8] +execute_trajectory(MotorControl1, Motor1, target_angles, + total_time=10.0, slow_down_time=1.0, + kp=50, kd=1, dt=0.001, read_interval=0.005, + ramp_time=1.0) + +# 关闭 +MotorControl1.disable(Motor1) +serial_device.close() \ No newline at end of file diff --git a/calculate/angle_A.txt b/calculate/angle_A.txt deleted file mode 100644 index 6c53205..0000000 --- a/calculate/angle_A.txt +++ /dev/null @@ -1 +0,0 @@ -2.25,2.24,2.23,2.22,2.21,2.20,2.19,2.19,2.18,2.17,2.16,2.15,2.14,2.12,2.11,2.10,2.09,2.08,2.07,2.05 \ No newline at end of file diff --git a/calculate/angle_B.txt b/calculate/angle_B.txt deleted file mode 100644 index 84d3363..0000000 --- a/calculate/angle_B.txt +++ /dev/null @@ -1 +0,0 @@ -0.90,0.90,0.91,0.92,0.93,0.94,0.95,0.96,0.97,0.98,0.99,1.00,1.01,1.02,1.03,1.04,1.05,1.06,1.08,1.09 \ No newline at end of file diff --git a/calculate/ik.py b/calculate/ik.py index 6046976..dfd2f3c 100644 --- a/calculate/ik.py +++ b/calculate/ik.py @@ -3,67 +3,94 @@ import math def inverseF(x, y, l1, l2, l3, l4, l5): """ - 五连杆机构逆向运动学函数（Python 实现） + 五连杆机构逆向运动学函数（优化版） 输入： x, y: 末端执行器坐标 - l1~l5: 各杆长度 + l1, l2: 左侧连杆长度 (O1→B, B→C) + l3, l4: 右侧连杆长度 (C→D, D→O2) + l5: 两基座距离 (O1O2) 输出： - theta1, theta2: 两个主动关节角度（弧度） + theta1, theta2: 两个主动关节角度（弧度，从 x 轴逆时针） + + 注意： + - 构型为“肘部向上”（通过 2π - acos 保证） + - 若目标点不可达，抛出 ValueError """ - Xc = x - Yc = y + Xc, Yc = x, y - # ===== 左侧链路（l1, l2）===== - numerator_left = Xc ** 2 + Yc ** 2 - l1 ** 2 - l2 ** 2 - denominator_left = 2 * l1 * l2 - cosfoai_12 = numerator_left / denominator_left + # ---------------- 左侧链路：求解 θ1 (O1 处电机角) ---------------- + R2_left = Xc * Xc + Yc * Yc # O1 到末端距离平方 + cos_phi12 = (R2_left - l1 * l1 - l2 * l2) / (2 * l1 * l2) - if cosfoai_12 > 1 or cosfoai_12 < -1: - raise ValueError("目标点超出工作空间！左侧无解。") - foai_12 = 2 * math.pi - math.acos(cosfoai_12) + if cos_phi12 < -1 or cos_phi12 > 1: + raise ValueError(f"左侧无解：cosφ12 = {cos_phi12:.4f} 超出 [-1,1]") - # 求第一个电机角度 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 + # φ12 = ∠B (三角形 O1-B-C 的内角) + phi12 = 2 * math.pi - math.acos(cos_phi12) # 肘部向上构型 - if denominator_foai01 == 0: - raise ZeroDivisionError("分母为零，无法计算左侧角度。") + # 计算 B 点坐标（从 C 反推） + # 向量 BC 的方向角 = atan2(Yc, Xc) + delta_angle + # 更稳定的方法：使用向量投影或直接几何 + cos_phi12_val = math.cos(phi12) + sin_phi12_val = math.sin(phi12) - 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) + # 使用向量法求 B 点 + # B = C - l2 * (cos(u2), sin(u2))，但 u2 未知 + # 改用：B 在以 O1 为圆心 l1 的圆 和 以 C 为圆心 l2 的圆 的交点 + # 我们已有 φ12，可用三角法 - # ===== 右侧链路（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 + # 方法：从 O1 出发，B 的方向角 foai_01 + numerator = l2 * Yc * sin_phi12_val + Xc * (l2 * cos_phi12_val + l1) + denominator = (l2 * cos_phi12_val + l1) ** 2 + (l2 * sin_phi12_val) ** 2 - if cosfoai_34 > 1 or cosfoai_34 < -1: - raise ValueError("目标点超出工作空间！右侧无解。") - foai_34 = 2 * math.pi - math.acos(cosfoai_34) + if abs(denominator) < 1e-12: + raise ZeroDivisionError("左侧分母接近零，机构处于奇异位形。") + cos_theta1 = numerator / denominator + if cos_theta1 < -1 or cos_theta1 > 1: + raise ValueError(f"cosθ1 = {cos_theta1:.4f} 超出 [-1,1]，左侧无解。") + + theta1 = math.acos(cos_theta1) + # 根据 Yc 符号判断象限（保持上凸构型） + if Yc < 0: + theta1 = 2 * math.pi - theta1 # 或根据机构限制决定 + + # ---------------- 右侧链路：求解 θ2 (O2 处电机角) ---------------- + Xc_rel = Xc - l5 # C 点相对于 O2 的 x 坐标 + R2_right = Xc_rel * Xc_rel + Yc * Yc # O2 到末端距离平方 + cos_phi34 = (R2_right - l3 * l3 - l4 * l4) / (2 * l3 * l4) + + if cos_phi34 < -1 or cos_phi34 > 1: + raise ValueError(f"右侧无解：cosφ34 = {cos_phi34:.4f} 超出 [-1,1]") + + phi34 = 2 * math.pi - math.acos(cos_phi34) # 肘部向上 + + # 计算辅助量 A = l5 - Xc - B = l3 * math.sin(foai_34) - C = l4 + l3 * math.cos(foai_34) + B = l3 * math.sin(phi34) + C = l4 + l3 * math.cos(phi34) - if B == 0 and C == 0: - raise ZeroDivisionError("B 和 C 均为零，无法计算右侧角度。") + if abs(B) < 1e-12 and abs(C) < 1e-12: + raise ValueError("右侧分母为零，机构处于奇异位形。") + + sqrt_BC = math.sqrt(B * B + C * 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("右侧三角函数计算失败，请检查输入是否合法。") + foai_t = math.acos(B / sqrt_BC) + inner_asin = A / sqrt_BC + if inner_asin < -1 or inner_asin > 1: + raise ValueError("asin 参数越界") + foai_40 = foai_t - math.asin(inner_asin) + except (ValueError, ZeroDivisionError) as e: + raise ValueError(f"右侧三角计算失败: {e}") - # 转换为角度再转回弧度 - theta1_deg = math.degrees(foai_01) - theta2_deg = 180 - math.degrees(foai_40) + # 关键：直接输出弧度，不再转角度 + # 原 C 代码中：theta2 = 180 - foai_40_deg，对应 u4 = pi - foai_40 + theta2 = math.pi - foai_40 - theta1 = math.radians(theta1_deg) - theta2 = math.radians(theta2_deg) + # ---------------- 可选：验证解的合理性 ---------------- + # 可添加工作空间边界检查、关节限位等 return theta1, theta2 \ No newline at end of file diff --git a/calculate/trajectory.py b/calculate/trajectory.py index 160c21c..11b59ba 100644 --- a/calculate/trajectory.py +++ b/calculate/trajectory.py @@ -16,6 +16,43 @@ def line_trajectory(start=(40, 0), end=(120, 0), num_points=20): y_list = start[1] + t * (end[1] - start[1]) return x_list, y_list + +def line_trajectory_fix(start=(40, 0), end=(120, 100), vx=0.1, vy=0.1, num_points=20): + """ + 生成带速度分量的匀速斜线轨迹 + 参数： + start: 起始点 (x, y) + end: 终点 (x, y) —— 仅用于估算运行时间（可选） + vx: x方向速度（单位/秒） + vy: y方向速度（单位/秒） + num_points: 生成的轨迹点数 + 返回： + x_list, y_list: 轨迹坐标数组 + """ + # 速度大小 + speed = np.sqrt(vx**2 + vy**2) + if speed == 0: + raise ValueError("速度不能为零") + + # 估算从 start 到 end 的距离（用于估算总时间） + if end is not None: + dx = end[0] - start[0] + dy = end[1] - start[1] + distance = np.sqrt(dx**2 + dy**2) + total_time = distance / speed # 理论到达时间 + print(total_time) + else: + total_time = 10.0 # 默认运行10秒 + + # 时间序列：从 0 到 total_time，均匀分布 num_points 个点 + t = np.linspace(0, total_time, num_points) + + # 位置 = 起点 + 速度 × 时间 + x_list = start[0] + vx * t + y_list = start[1] + vy * t + + return x_list, y_list + def ellipse_trajectory(center=(80, 0), rx=50, ry=25, num_points=60): """ 椭圆轨迹 """ angles = np.linspace(0, 2 * np.pi, num_points) diff --git a/jicheng1.py b/jicheng1.py new file mode 100644 index 0000000..a1f3985 --- /dev/null +++ b/jicheng1.py @@ -0,0 +1,457 @@ +# ==================== 五连杆机械臂轨迹控制集成版（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 new file mode 100644 index 0000000..15e49d4 --- /dev/null +++ b/main.py @@ -0,0 +1,378 @@ +# ==================== 五连杆机械臂轨迹控制（直接角度发送版）==================== +# 功能：轨迹生成 + 逆解 + 直接控制 + 动画显示 + 关节角度可视化 + 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 = 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 + +# ------------------------ MIT 控制函数 ------------------------ +def control_two_motors_mit(theta1_rad, theta4_rad): + """ + 发送 MIT 控制指令（角度单位：弧度） + 参数: theta1_rad, theta4_rad —— 目标角度（弧度） + """ + global current_theta1, current_theta4 + + # ✅ 直接使用弧度值，不再转为角度 + 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 + + # 定义90度对应于弧度的偏移量 + 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: + 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 + else: + current_theta1 = current_theta4 = -ANGLE_OFFSET_RAD # 考虑到偏移 + + 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 = current_theta1 + target_theta4 = current_theta4 + else: + current_theta1, current_theta4 = target_theta1, target_theta4 + + # === 直接发送角度到电机（假设motor api接受弧度值）=== + 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 Exception as fk_e: + 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 + ANGLE_OFFSET_RAD)) # 将弧度转回角度以便记录 + theta4_log.append(np.degrees(target_theta4 + ANGLE_OFFSET_RAD)) + + # 固定延时，控制发送频率 + 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(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("开始执行轨迹（直接发送角度）...") + time_log, theta1_log, theta4_log, x_fk_log, y_fk_log = execute_direct_trajectory( + x_list, y_list, dt=DT # 每个点发送后延时 1ms，这个地方控制电机延时 + ) + + # --- 绘制关节角度 --- + 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/new_main.py b/new_main.py new file mode 100644 index 0000000..206a9f9 --- /dev/null +++ b/new_main.py @@ -0,0 +1,301 @@ +import time +import numpy as np +import matplotlib.pyplot as plt +from matplotlib.animation import FuncAnimation + + +# 设置中文字体和解决负号显示问题 +plt.rcParams['font.sans-serif'] = ['SimHei', 'WenQuanYi Zen Hei', 'FangSong'] # 按优先级选择字体 +plt.rcParams['axes.unicode_minus'] = False # 显示负号 - +# ------------------------ 调试开关 ------------------------ +DEBUG_MODE = True # <<< True: 只播动画；False: 控制真实电机 + +# 导入运动学和轨迹函数 +from calculate.ik import inverseF +from calculate.trajectory import ( + circle_trajectory, + line_trajectory, + line_trajectory_fix, + ellipse_trajectory, + square_trajectory, + triangle_trajectory +) + +# -------------------- 只在非 Debug 模式导入 DM_CAN -------------------- +if not DEBUG_MODE: + from DM_CAN.DM_CAN import Motor, MotorControl, DM_Motor_Type, Control_Type, DM_variable + import serial + from can_test_motor import motor_control_can_all +else: + print("【DEBUG MODE】: 已启用调试模式，不连接真实硬件。") + +# ------------------------ 机械臂参数 ------------------------ +L1 = 250 +L2 = 300 +L3 = 300 +L4 = 250 +L0 = 250 # 右侧基座偏移 + +# ------------------------ DM4310 配置 ------------------------ +MOTOR1_ID = 0x01 +MOTOR2_ID = 0x04 +CAN_SERIAL_PORT = '/dev/ttyACM0' +BAUD_RATE = 921600 +KP = 5.0 +KD = 1.0 + +# ------------------------ 轮趣科技电机配置 ------------------------ +MOTOR3_ID = 0x05 + +# 全局变量 +motor1 = motor2 = motor3 = motor_control = None +current_theta1 = 0.0 +current_theta4 = 0.0 +x_list = y_list = [] # 全局轨迹点 +line = None # 动画线对象 +ani = None # 动画对象 + + +# ------------------------ 初始化电机 ------------------------ +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 + })() + 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): + global current_theta1, current_theta4 + + pos1 = np.degrees(theta1) + pos4 = np.degrees(theta4) + vel = 0.0 + + if not DEBUG_MODE: + motor_control.controlMIT(motor1, KP, KD, pos1, vel, 0.0) + motor_control.controlMIT(motor2, KP, KD, pos4, vel, 0.0) + else: + print(f"[DEBUG] 模拟控制 -> Motor1: {pos1:.2f}°, Motor2: {pos4:.2f}°") + + current_theta1 = theta1 + current_theta4 = theta4 + + +# ------------------------ 动画帧绘制函数 ------------------------ +def draw_frame(i): + global line, x_list, y_list + + 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) + # 右臂第一杆末端（注意 L0 偏移） + 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) + + # 发送控制指令（在循环中同步） + control_two_motors_mit(theta1, theta4) + + except Exception as e: + print(f"第 {i} 帧逆解失败: {e}") + line.set_data([], []) + + return line, + + +# ------------------------ 运行轨迹 + 动画 ------------------------ +# ------------------------ 运行轨迹 + 动画 + 关节轨迹可视化 ------------------------ +def run_trajectory_with_animation(trajectory_func, **params): + """ + 生成轨迹并播放动画，同时发送控制指令，并可视化关节角度变化 + """ + global x_list, y_list, line, ani + + print(f"生成轨迹: {trajectory_func.__name__}") + x_list, y_list = trajectory_func(**params) + print(f"轨迹点数: {len(x_list)}") + + # --- 存储关节角度用于后续绘图 --- + theta1_list = [] + theta4_list = [] + + # --- 设置结构动画 --- + fig, ax = plt.subplots(figsize=(10, 8)) + ax.set_xlim(-200, L0 + 200) + ax.set_ylim(0, 500) + ax.set_aspect('equal') + ax.grid(True, linestyle='--', alpha=0.6) + ax.set_title(f"五连杆机械臂运动仿真 - 轨迹: {trajectory_func.__name__}", fontsize=14) + + # 绘制目标轨迹（虚线） + ax.plot(x_list, y_list, 'b--', alpha=0.5, label='目标轨迹') + line, = ax.plot([], [], 'r-o', linewidth=3, markersize=8, label='机械臂结构') + ax.legend() + + # --- 动画更新函数（修改：记录角度）--- + def draw_frame(i): + x = x_list[i] + y = y_list[i] + + try: + theta1, theta4 = inverseF(x, y, L1, L2, L3, L4, L0) + + # 记录角度（弧度 → 角度） + theta1_deg = np.degrees(theta1) + theta4_deg = np.degrees(theta4) + theta1_list.append(theta1_deg) + theta4_list.append(theta4_deg) + + # 左臂第一杆末端 + 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) + + # 发送控制指令 + control_two_motors_mit(theta1, theta4) + + except Exception as e: + print(f"第 {i} 帧逆解失败: {e}") + line.set_data([], []) + theta1_list.append(np.nan) + theta4_list.append(np.nan) + + return line, + + # --- 创建动画 --- + ani = FuncAnimation( + fig, draw_frame, + frames=len(x_list), + interval=50, + repeat=False, + blit=True + ) + + plt.show() # 显示机械臂动画 + print("动画结束，正在绘制关节角度轨迹...") + + # --- 绘制关节角度曲线 --- + if len(theta1_list) == 0: + print("【警告】未记录到任何关节角度数据。") + return + + time_axis = np.linspace(0, len(theta1_list) * 0.05, len(theta1_list)) # 假设每帧 ~50ms + + fig2, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 6), sharex=True) + + # 电机1 关节角 θ1 + ax1.plot(time_axis, theta1_list, 'b-o', markersize=4, linewidth=2) + ax1.set_ylabel('电机1 关节角 θ₁ (°)') + ax1.set_title('电机关节角度轨迹') + ax1.grid(True, alpha=0.5) + ax1.axhline(y=0, color='k', linestyle='--', alpha=0.3) + + # 电机2 关节角 θ4 + ax2.plot(time_axis, theta4_list, 'r-o', markersize=4, linewidth=2) + ax2.set_xlabel('时间 (秒)') + ax2.set_ylabel('电机2 关节角 θ₄ (°)') + ax2.grid(True, alpha=0.5) + ax2.axhline(y=0, color='k', linestyle='--', alpha=0.3) + + plt.tight_layout() + plt.show() + + # --- 打印统计信息 --- + print("\n=== 关节角度统计 ===") + print(f"θ₁ 范围: {min(theta1_list):.2f}° ~ {max(theta1_list):.2f}°") + print(f"θ₄ 范围: {min(theta4_list):.2f}° ~ {max(theta4_list):.2f}°") + print(f"θ₁ 峰峰值: {max(theta1_list) - min(theta1_list):.2f}°") + print(f"θ₄ 峰峰值: {max(theta4_list) - min(theta4_list):.2f}°") + +# ------------------------ 主函数 ------------------------ +if __name__ == "__main__": + # === 在这里设置轨迹类型和参数 === + + '''trajectory_config = { + 'func': ellipse_trajectory, + 'params': { + 'center': (100, 200), + 'rx': 50, + 'ry': 25, + 'num_points': 100 + } + }''' + + + # 可选其他轨迹： + trajectory_config = { + 'func': line_trajectory_fix, + 'params': {'start': (25, 300), 'end': (200, 300), 'vx':3.0, 'vy':5.0, 'num_points':20} + } + #trajectory_config = { + # 'func': circle_trajectory, + # 'params': {'center': (100, 300), 'radius': 40} + # } + + # 初始化电机 + try: + motor1, motor2, motor_control = init_motors() + + # 运行带动画的轨迹 + 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