From 250d5e510f6f01456b77c8ac2eb1fcff4511c36e Mon Sep 17 00:00:00 2001 From: Lizongdi <1210855344@qq.com> Date: Fri, 29 May 2026 10:21:10 +0800 Subject: [PATCH] =?UTF-8?q?=E3=80=90=E8=B0=83=E9=80=9A=E6=89=93=E6=A0=87?= =?UTF-8?q?=E3=80=91=E6=96=9C=E7=8E=87=E5=92=8C=E5=BC=80=E5=85=B3=E6=9E=AA?= =?UTF-8?q?=E6=97=B6=E9=97=B4=E7=94=9F=E6=95=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- kelaifen/Kelaifen_V6.0/Example_0_5.py | 2 +- kelaifen/Kelaifen_V6.0/config.json | 76 +++----------- kelaifen/Kelaifen_V6.0/trajectory_data.py | 24 ----- kelaifen/Kelaifen_V6.0/trajectory_program.py | 105 ++++++++++++++++--- 4 files changed, 106 insertions(+), 101 deletions(-) diff --git a/kelaifen/Kelaifen_V6.0/Example_0_5.py b/kelaifen/Kelaifen_V6.0/Example_0_5.py index 15a0256..d30fb38 100644 --- a/kelaifen/Kelaifen_V6.0/Example_0_5.py +++ b/kelaifen/Kelaifen_V6.0/Example_0_5.py @@ -292,7 +292,7 @@ if __name__ == "__main__": running = False time.sleep(0.5) else: - running = program_switch[current_program](sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, turn_off_relay_end, ex_turn_on_relay_start, ex_turn_on_relay_end, ex_turn_off_relay_start, ex_turn_off_relay_end) + running = program_switch[current_program](sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start, delay) # 统一字典映射延时 # delay = program_delay_map.get(current_program, 0.0) diff --git a/kelaifen/Kelaifen_V6.0/config.json b/kelaifen/Kelaifen_V6.0/config.json index 171ebdc..b85ff5c 100644 --- a/kelaifen/Kelaifen_V6.0/config.json +++ b/kelaifen/Kelaifen_V6.0/config.json @@ -11,20 +11,16 @@ "4": 0.1, "5": 1.0, "500": 0.1, - "6": 0.5, + "6": 0.1, "7": 1.0, "700": 0.1, - "8": 0.5 + "8": 0.1 }, "relay_config": { "turn_on_relay_start": 20, - "turn_on_relay_end": 25, "turn_off_relay_start": 135, - "turn_off_relay_end": 140, "ex_turn_on_relay_start": 20, - "ex_turn_on_relay_end": 25, - "ex_turn_off_relay_start": 135, - "ex_turn_off_relay_end": 140 + "ex_turn_off_relay_start": 135 } }, { @@ -45,13 +41,9 @@ }, "relay_config": { "turn_on_relay_start": 20, - "turn_on_relay_end": 25, "turn_off_relay_start": 135, - "turn_off_relay_end": 140, "ex_turn_on_relay_start": 20, - "ex_turn_on_relay_end": 25, - "ex_turn_off_relay_start": 135, - "ex_turn_off_relay_end": 140 + "ex_turn_off_relay_start": 135 } }, { @@ -72,13 +64,9 @@ }, "relay_config": { "turn_on_relay_start": 20, - "turn_on_relay_end": 25, "turn_off_relay_start": 135, - "turn_off_relay_end": 140, "ex_turn_on_relay_start": 20, - "ex_turn_on_relay_end": 25, - "ex_turn_off_relay_start": 135, - "ex_turn_off_relay_end": 140 + "ex_turn_off_relay_start": 135 } }, { @@ -99,13 +87,9 @@ }, "relay_config": { "turn_on_relay_start": 20, - "turn_on_relay_end": 25, "turn_off_relay_start": 135, - "turn_off_relay_end": 140, "ex_turn_on_relay_start": 20, - "ex_turn_on_relay_end": 25, - "ex_turn_off_relay_start": 135, - "ex_turn_off_relay_end": 140 + "ex_turn_off_relay_start": 135 } }, { @@ -126,13 +110,9 @@ }, "relay_config": { "turn_on_relay_start": 20, - "turn_on_relay_end": 25, "turn_off_relay_start": 135, - "turn_off_relay_end": 140, "ex_turn_on_relay_start": 20, - "ex_turn_on_relay_end": 25, - "ex_turn_off_relay_start": 135, - "ex_turn_off_relay_end": 140 + "ex_turn_off_relay_start": 135 } }, { @@ -153,13 +133,9 @@ }, "relay_config": { "turn_on_relay_start": 20, - "turn_on_relay_end": 25, "turn_off_relay_start": 135, - "turn_off_relay_end": 140, "ex_turn_on_relay_start": 20, - "ex_turn_on_relay_end": 25, - "ex_turn_off_relay_start": 135, - "ex_turn_off_relay_end": 140 + "ex_turn_off_relay_start": 135 } }, { @@ -180,13 +156,9 @@ }, "relay_config": { "turn_on_relay_start": 20, - "turn_on_relay_end": 25, "turn_off_relay_start": 135, - "turn_off_relay_end": 140, "ex_turn_on_relay_start": 20, - "ex_turn_on_relay_end": 25, - "ex_turn_off_relay_start": 135, - "ex_turn_off_relay_end": 140 + "ex_turn_off_relay_start": 135 } }, { @@ -207,13 +179,9 @@ }, "relay_config": { "turn_on_relay_start": 20, - "turn_on_relay_end": 25, "turn_off_relay_start": 135, - "turn_off_relay_end": 140, "ex_turn_on_relay_start": 20, - "ex_turn_on_relay_end": 25, - "ex_turn_off_relay_start": 135, - "ex_turn_off_relay_end": 140 + "ex_turn_off_relay_start": 135 } }, { @@ -234,13 +202,9 @@ }, "relay_config": { "turn_on_relay_start": 20, - "turn_on_relay_end": 25, "turn_off_relay_start": 135, - "turn_off_relay_end": 140, "ex_turn_on_relay_start": 20, - "ex_turn_on_relay_end": 25, - "ex_turn_off_relay_start": 135, - "ex_turn_off_relay_end": 140 + "ex_turn_off_relay_start": 135 } }, { @@ -261,13 +225,9 @@ }, "relay_config": { "turn_on_relay_start": 20, - "turn_on_relay_end": 25, "turn_off_relay_start": 135, - "turn_off_relay_end": 140, "ex_turn_on_relay_start": 20, - "ex_turn_on_relay_end": 25, - "ex_turn_off_relay_start": 135, - "ex_turn_off_relay_end": 140 + "ex_turn_off_relay_start": 135 } }, { @@ -288,13 +248,9 @@ }, "relay_config": { "turn_on_relay_start": 20, - "turn_on_relay_end": 25, "turn_off_relay_start": 135, - "turn_off_relay_end": 140, "ex_turn_on_relay_start": 20, - "ex_turn_on_relay_end": 25, - "ex_turn_off_relay_start": 135, - "ex_turn_off_relay_end": 140 + "ex_turn_off_relay_start": 135 } }, { @@ -315,13 +271,9 @@ }, "relay_config": { "turn_on_relay_start": 20, - "turn_on_relay_end": 25, "turn_off_relay_start": 135, - "turn_off_relay_end": 140, "ex_turn_on_relay_start": 20, - "ex_turn_on_relay_end": 25, - "ex_turn_off_relay_start": 135, - "ex_turn_off_relay_end": 140 + "ex_turn_off_relay_start": 135 } } ] diff --git a/kelaifen/Kelaifen_V6.0/trajectory_data.py b/kelaifen/Kelaifen_V6.0/trajectory_data.py index 704d836..7257751 100644 --- a/kelaifen/Kelaifen_V6.0/trajectory_data.py +++ b/kelaifen/Kelaifen_V6.0/trajectory_data.py @@ -634,11 +634,6 @@ Pose_4 = [ joint_positions_assembled_4 = [[92.553,-45.178,53.196,-97.284,97.995,15.177]] Pose_3_4 = [ - [-228.1, 1631.7, 370.4375, -3.1075, 0.0078, -0.1622], - [-228.1, 1631.7, 370.4375, -3.1075, 0.0078, -0.1622], - [-228.1, 1631.7, 370.4375, -3.1075, 0.0078, -0.1622], - [-228.1, 1631.7, 370.4375, -3.1075, 0.0078, -0.1622], - [-228.1, 1631.7, 370.4375, -3.1075, 0.0078, -0.1622], [-228.1, 1631.7, 370.4375, -3.1075, 0.0078, -0.1622], [-228.1, 1631.7, 380.6750, -3.1075, 0.0078, -0.1622], [-228.1, 1631.7, 390.9125, -3.1075, 0.0078, -0.1622], @@ -659,19 +654,9 @@ Pose_3_4 = [ [-228.1, 1631.7, 544.4750, -3.1075, 0.0078, -0.1622], [-228.1, 1631.7, 554.7125, -3.1075, 0.0078, -0.1622], [-228.1, 1631.7, 564.9500, -3.1075, 0.0078, -0.1622], - [-228.1, 1631.7, 564.9500, -3.1075, 0.0078, -0.1622], - [-228.1, 1631.7, 564.9500, -3.1075, 0.0078, -0.1622], - [-228.1, 1631.7, 564.9500, -3.1075, 0.0078, -0.1622], - [-228.1, 1631.7, 564.9500, -3.1075, 0.0078, -0.1622], - [-228.1, 1631.7, 564.9500, -3.1075, 0.0078, -0.1622] ] Pose_4_3 = [ - [1549.9, -66.7470, 370.4375, 3.1398, 0.0349, -1.5584], - [1549.9, -66.7470, 370.4375, 3.1398, 0.0349, -1.5584], - [1549.9, -66.7470, 370.4375, 3.1398, 0.0349, -1.5584], - [1549.9, -66.7470, 370.4375, 3.1398, 0.0349, -1.5584], - [1549.9, -66.7470, 370.4375, 3.1398, 0.0349, -1.5584], [1549.9, -66.7470, 370.4375, 3.1398, 0.0349, -1.5584], [1549.9, -66.7470, 380.6750, 3.1398, 0.0349, -1.5584], [1549.9, -66.7470, 390.9125, 3.1398, 0.0349, -1.5584], @@ -692,15 +677,6 @@ Pose_4_3 = [ [1549.9, -66.7470, 544.4750, 3.1398, 0.0349, -1.5584], [1549.9, -66.7470, 554.7125, 3.1398, 0.0349, -1.5584], [1549.9, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584], - [1549.9, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584], - [1549.9, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584], - [1549.9, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584], - [1549.9, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584], - [1549.9, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584], - [1549.9, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584], - [1549.9, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584], - [1549.9, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584], - [1549.9, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584] ] diff --git a/kelaifen/Kelaifen_V6.0/trajectory_program.py b/kelaifen/Kelaifen_V6.0/trajectory_program.py index 1405ddc..f5c4b0b 100644 --- a/kelaifen/Kelaifen_V6.0/trajectory_program.py +++ b/kelaifen/Kelaifen_V6.0/trajectory_program.py @@ -1,3 +1,4 @@ +import copy from queue import Queue from ETController import send_Point, sendCMD, wait_stop from gpio import turn_off_relay, turn_on_relay @@ -976,37 +977,113 @@ def program12(sock): print("第十二段程序执行完") -def program100(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, turn_off_relay_end, ex_turn_on_relay_start, ex_turn_on_relay_end, ex_turn_off_relay_start, ex_turn_off_relay_end, delay): +def program100(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start, delay): global sample_time, lookahead_time sample_time = 20 # 采样时间 (ms) lookahead_time = 300 # 前瞻时间 (ms) sleep_time = sample_time * 0.001 # 休眠时间 (秒) + + #0.点位平移操作 + # 1.假设 Off_Set_Value 已由外部传入（示例值） + Off_Set_Value = serial_handler.Trajectory_angle # 示例值，实际由外部传入 + # 2.深拷贝原始轨迹（避免修改原数据） + Pose_3_M = copy.deepcopy(Pose_3) + Pose_4_M = copy.deepcopy(Pose_4) + # 3.修改 Pose_3_M 和 Pose_4_M 的第12到144行（MATLAB索引12~144 => Python索引11~143） + # 注意：共 133 行（144-12+1=133），但范围是 12:144 共133个点 + for i in range(11, 144): # i = 11 ~ 143 对应 MATLAB 第12~144行 + factor = (i - 11) / 133.0 # MATLAB中 (i-12)/133，i从12开始，对应Python i=11时因子0 + Pose_3_M[i][2] = Pose_3[i][2] + Off_Set_Value * factor + Pose_4_M[i][2] = Pose_4[i][2] + Off_Set_Value * factor + # 修改第145行到末尾（MATLAB索引145:end => Python索引144:） + for i in range(144, len(Pose_3_M)): + Pose_3_M[i][2] = Pose_3[i][2] + Off_Set_Value + Pose_4_M[i][2] = Pose_4[i][2] + Off_Set_Value + # 4.处理 Pose_3_4 和 Pose_4_3 + num = len(Pose_3_4) # 假设两者长度相同 + Pose_3_4_M = copy.deepcopy(Pose_3_4) + Pose_4_3_M = copy.deepcopy(Pose_4_3) + for i in range(num): + # MATLAB 中 i 从 1 开始，公式： -Off_Set_Value/num*i + Off_Set_Value + # Python 中 i 从 0 开始，所以因子为 (i+1) + factor = (i + 1) / num + Pose_3_4_M[i][2] = Pose_3_4[i][2] - Off_Set_Value * factor + Off_Set_Value + Pose_4_3_M[i][2] = Pose_4_3[i][2] - Off_Set_Value * factor + Off_Set_Value + + #0.延时操作，点位延时 #1.添加点位延时 NUM = int(delay * 1000 / sample_time) # 2.第一道喷涂延时点位 - last_point_3_4 = Pose_3_4[-1].copy() # 先复制最后一个点位 + last_point_3_4 = Pose_3_4_M[-1].copy() # 先复制最后一个点位 Pose_3_4_Stop = [last_point_3_4.copy() for _ in range(NUM)] # 每个点位独立副本 # 3.第2道喷涂延时点位 - last_point_4_3 = Pose_4_3[-1].copy() + last_point_4_3 = Pose_4_3_M[-1].copy() Pose_4_3_Stop = [last_point_4_3.copy() for _ in range(NUM)] - # ----- 一次性清理和初始化透传服务 ----- + trajectories = [Pose_3_M, Pose_3_4_M,Pose_3_4_Stop, Pose_4_M, Pose_4_3_M,Pose_4_3_Stop] + # ----- 一次性清理和初始化透传服务 ----- sendCMD(sock, "tt_clear_servo_joint_buf") time.sleep(0.05) - trajectories = [Pose_3, Pose_3_4,Pose_3_4_Stop, Pose_4, Pose_4_3,Pose_4_3_Stop] + + #0.喷枪开关控制,开喷枪时间最小为0，最大为155；Pose_4_3_M及Pose_3_4_M中各有20组数据 + #每道喷涂指令有155个点， + # 第1道开枪时间在第10个点位（Pose_3_M的开枪点位 lookahead_time/sample_time+10），关枪时间在第130点位示例（lookahead_time/sample_time+135） + # 第2道开枪时间在第15个点位（Pose_4_M的开枪点位 lookahead_time/sample_time+15），，关枪时间在第150点位示例（lookahead_time/sample_time+135，，，，如果数据>155，那么关枪就在Pose_4_3_M中） + #队列，延时时间计算 + off_set_Time=int(lookahead_time / sample_time) + # 1.喷枪1开关时间计算 + OPEN_1=turn_on_relay_start + CLOSE_1=turn_off_relay_start + OPEN_1_M = off_set_Time+5 + CLOSE_1_M1S=off_set_Time+CLOSE_1 + CLOSE_1_M1E = off_set_Time + CLOSE_1-155 + #2.喷枪2开关时间计算 + OPEN_2=ex_turn_on_relay_start + CLOSE_2 = ex_turn_off_relay_start + OPEN_2_M = off_set_Time+OPEN_2 + CLOSE_2_M2S = off_set_Time+CLOSE_2 + CLOSE_2_M2E = off_set_Time + CLOSE_2- 155 + sendCMD(sock, "transparent_transmission_init", {"lookahead": lookahead_time,"t": sample_time,"smoothness": 1,"response_enable": 0 }) - cycles = 10 # 循环5次 + cycles = 20 # 循环5次 + for _ in range(cycles): for traj in trajectories: print("开始执行轨迹...") - for point in traj: - if not compensate_queue.empty(): - increments_x, increments_y, increments_z = compensate_queue.get_nowait() - # point[0] += increments_x - # point[1] += increments_y - # point[2] += increments_z - send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point}) - time.sleep(sleep_time) + # 针对不同轨迹进行计数 + if traj is Pose_3_M: + for idx, point in enumerate(traj, start=1): + if idx == OPEN_1_M: + turn_on_relay() + if idx == CLOSE_1_M1S: + turn_off_relay() + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point}) + 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}) + time.sleep(sleep_time) + elif traj is Pose_4_M: + for idx, point in enumerate(traj, start=1): + if idx == OPEN_2_M: + turn_on_relay() + if idx == CLOSE_2_M2S: + turn_off_relay() + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point}) + 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}) + time.sleep(sleep_time) + else: + # 其他轨迹（如 Stop 轨迹）无需喷枪控制 + for point in traj: + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point}) + time.sleep(sleep_time) print("轨迹执行完成") return False