From 320dcfe6d75557d0fbae81866a41384dd179a200 Mon Sep 17 00:00:00 2001 From: Lizongdi <1210855344@qq.com> Date: Fri, 5 Jun 2026 14:22:03 +0800 Subject: [PATCH] =?UTF-8?q?=E3=80=906=E6=9C=885=E6=97=A5=E5=8F=91=E5=BE=80?= =?UTF-8?q?=E7=8E=B0=E5=9C=BA=E3=80=91=E5=A2=9E=E5=8A=A0=E5=81=8F=E7=A7=BB?= =?UTF-8?q?=E9=87=8F=E8=A1=A5=E5=81=BF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- kelaifen/Kelaifen_V6.0/trajectory_program.py | 134 +++++++++++++++++-- 1 file changed, 124 insertions(+), 10 deletions(-) diff --git a/kelaifen/Kelaifen_V6.0/trajectory_program.py b/kelaifen/Kelaifen_V6.0/trajectory_program.py index e4d8233..9787044 100644 --- a/kelaifen/Kelaifen_V6.0/trajectory_program.py +++ b/kelaifen/Kelaifen_V6.0/trajectory_program.py @@ -16,10 +16,14 @@ from trajectory_data import ( Pose_1_2, Pose_2_1, ) import time +import copy +from collections import deque +import queue # 导入串口处理模块 import serial_handler # 采样时间、延时时间和衔接机器人速度 +ENABLE_INCREMENTS = 1 # 表示是否支持偏移 SAMP_TIME = 20 SLP_TIME = 0.02 # 补偿值更新队列（解耦线程通信，避免锁竞争） @@ -31,6 +35,17 @@ increments_x = 0 increments_y = 0 increments_z = 0 Coordinate_compensation = [0, 0, 0, 0, 0, 0] + +FILTER_N = 10 + +buf_x = deque([0.0] * FILTER_N, maxlen=FILTER_N) +buf_y = deque([0.0] * FILTER_N, maxlen=FILTER_N) +buf_z = deque([0.0] * FILTER_N, maxlen=FILTER_N) + +X_M = 0.0 +Y_M = 0.0 +Z_M = 0.0 + # 预期的帧长度，可根据实际情况修改 EXPECTED_LENGTH = 7 global sample_time @@ -51,6 +66,87 @@ serial_shared = SerialSharedData() # 轨迹插值点数 Interpolation_points = 20 +def reset_filter(): + + global buf_x + global buf_y + global buf_z + + global X_M + global Y_M + global Z_M + + buf_x = deque([0.0] * FILTER_N, maxlen=FILTER_N) + buf_y = deque([0.0] * FILTER_N, maxlen=FILTER_N) + buf_z = deque([0.0] * FILTER_N, maxlen=FILTER_N) + + X_M = 0.0 + Y_M = 0.0 + Z_M = 0.0 + while True: + try: + compensate_queue.get_nowait() + except: + break + + print("Filter Reset") + + +def update_filter(x, y, z): + + buf_x.append(x) + buf_y.append(y) + buf_z.append(z) + + x_m = sum(buf_x) / len(buf_x) + y_m = sum(buf_y) / len(buf_y) + z_m = sum(buf_z) / len(buf_z) + + MAX_COMP = 200 + + x_m = max(min(x_m, MAX_COMP), -MAX_COMP) + y_m = max(min(y_m, MAX_COMP), -MAX_COMP) + z_m = max(min(z_m, MAX_COMP), -MAX_COMP) + + + return x_m, y_m, z_m + +def get_compensated_point(point): + + global X_M + global Y_M + global Z_M + + try: + + while True: + + increments_x, increments_y, increments_z = \ + compensate_queue.get_nowait() + + increments_x = max(min(increments_x, 200), -200) + increments_y = max(min(increments_y, 200), -200) + increments_z = max(min(increments_z, 200), -200) + + X_M, Y_M, Z_M = update_filter( + increments_x, + increments_y, + increments_z + ) + + except queue.Empty: + + pass + + point_M = point.copy() + + if 1 == ENABLE_INCREMENTS: + point_M[0] += X_M + point_M[1] += Y_M + point_M[2] += Z_M + + return point_M + # 第一段程序封装为函数 # 第二段程序封装为函数(上升喷涂) # 第三段程序封装为函数 @@ -982,6 +1078,10 @@ def program12(sock): def program100(sock, ser, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start, delay, delay1): global sample_time, lookahead_time + + if 1 == ENABLE_INCREMENTS: + reset_filter() + lookahead_time = 400 # 前瞻时间 (ms) sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment) @@ -1069,13 +1169,15 @@ def program100(sock, ser, turn_on_relay_start, turn_off_relay_start, ex_turn_on_ turn_on_relay() if idx == CLOSE_1_M1S: turn_off_relay() - send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point}) + point_M = get_compensated_point(point) + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point_M}) time.sleep(sleep_time) elif traj is Pose_3_4_M: for idx, point in enumerate(traj, start=1): if idx == CLOSE_1_M1E: turn_off_relay() - send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point}) + point_M = get_compensated_point(point) + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point_M}) time.sleep(sleep_time) elif traj is Pose_4_M: for idx, point in enumerate(traj, start=1): @@ -1083,18 +1185,21 @@ def program100(sock, ser, turn_on_relay_start, turn_off_relay_start, ex_turn_on_ turn_on_relay() if idx == CLOSE_2_M2S: turn_off_relay() - send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point}) + point_M = get_compensated_point(point) + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point_M}) time.sleep(sleep_time) elif traj is Pose_4_3_M: for idx, point in enumerate(traj, start=1): if idx == CLOSE_2_M2E: turn_off_relay() - send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point}) + point_M = get_compensated_point(point) + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point_M}) time.sleep(sleep_time) else: # 其他轨迹（如 Stop 轨迹）无需喷枪控制 for point in traj: - send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point}) + point_M = get_compensated_point(point) + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point_M}) time.sleep(sleep_time) print(f"停止轨迹，NUM = {NUM}, delay = {delay}") stop_cmd_bytes = read_cmd_from_shared(timeout=0.1) @@ -1112,6 +1217,10 @@ def program100(sock, ser, turn_on_relay_start, turn_off_relay_start, ex_turn_on_ def program101(sock, ser, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start, delay, delay1): global sample_time, lookahead_time + + if 1 == ENABLE_INCREMENTS: + reset_filter() + lookahead_time = 400 # 前瞻时间 (ms) sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment) @@ -1201,13 +1310,15 @@ def program101(sock, ser, turn_on_relay_start, turn_off_relay_start, ex_turn_on_ turn_on_relay() if idx == CLOSE_1_M1S: turn_off_relay() - send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point}) + point_M = get_compensated_point(point) + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point_M}) time.sleep(sleep_time) elif traj is Pose_3_4_M: for idx, point in enumerate(traj, start=1): if idx == CLOSE_1_M1E: turn_off_relay() - send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point}) + point_M = get_compensated_point(point) + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point_M}) time.sleep(sleep_time) elif traj is Pose_4_M: for idx, point in enumerate(traj, start=1): @@ -1215,18 +1326,21 @@ def program101(sock, ser, turn_on_relay_start, turn_off_relay_start, ex_turn_on_ turn_on_relay() if idx == CLOSE_2_M2S: turn_off_relay() - send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point}) + point_M = get_compensated_point(point) + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point_M}) time.sleep(sleep_time) elif traj is Pose_4_3_M: for idx, point in enumerate(traj, start=1): if idx == CLOSE_2_M2E: turn_off_relay() - send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point}) + point_M = get_compensated_point(point) + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point_M}) time.sleep(sleep_time) else: # 其他轨迹（如 Stop 轨迹）无需喷枪控制 for point in traj: - send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point}) + point_M = get_compensated_point(point) + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point_M}) time.sleep(sleep_time) print(f"停止轨迹，NUM = {NUM}, delay = {delay}") stop_cmd_bytes = read_cmd_from_shared(timeout=0.1)