增加电机控制时间延迟
This commit is contained in:
琉璃月光
2
.idea/5dof.iml
generated
2
.idea/5dof.iml
generated
@ -2,7 +2,7 @@
|
||||
<module type="PYTHON_MODULE" version="4">
|
||||
<component name="NewModuleRootManager">
|
||||
<content url="file://$MODULE_DIR$" />
|
||||
<orderEntry type="jdk" jdkName="robot" jdkType="Python SDK" />
|
||||
<orderEntry type="jdk" jdkName="drake" jdkType="Python SDK" />
|
||||
<orderEntry type="sourceFolder" forTests="false" />
|
||||
</component>
|
||||
</module>
|
||||
2
.idea/misc.xml
generated
2
.idea/misc.xml
generated
@ -3,5 +3,5 @@
|
||||
<component name="Black">
|
||||
<option name="sdkName" value="Python 3.12 (PythonProject)" />
|
||||
</component>
|
||||
<component name="ProjectRootManager" version="2" project-jdk-name="robot" project-jdk-type="Python SDK" />
|
||||
<component name="ProjectRootManager" version="2" project-jdk-name="drake" project-jdk-type="Python SDK" />
|
||||
</project>
|
||||
@ -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,
|
||||
|
||||
144
DM_control.py
Normal file
144
DM_control.py
Normal file
@ -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()
|
||||
@ -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
|
||||
@ -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
|
||||
111
calculate/ik.py
111
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
|
||||
@ -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)
|
||||
|
||||
457
jicheng1.py
Normal file
457
jicheng1.py
Normal file
@ -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】程序结束")
|
||||
378
main.py
Normal file
378
main.py
Normal file
@ -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】程序结束")
|
||||
301
new_main.py
Normal file
301
new_main.py
Normal file
@ -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】程序结束")
|
||||
Reference in New Issue
Block a user