From ea77c41d9a963d81d6aa482017e8ca62bf8696e6 Mon Sep 17 00:00:00 2001 From: Lizongdi <1210855344@qq.com> Date: Wed, 27 May 2026 15:10:51 +0800 Subject: [PATCH] =?UTF-8?q?=E7=A1=AE=E4=BF=9D=E5=8F=AA=E8=B0=83=E7=94=A8?= =?UTF-8?q?=E4=B8=80=E6=AC=A1=E6=B8=85=E7=A9=BA=E5=92=8C=E5=88=9D=E5=A7=8B?= =?UTF-8?q?=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- kelaifen/Kelaifen_V6.0/trajectory_program.py | 63 ++++++++++++-------- 1 file changed, 37 insertions(+), 26 deletions(-) diff --git a/kelaifen/Kelaifen_V6.0/trajectory_program.py b/kelaifen/Kelaifen_V6.0/trajectory_program.py index 942f2c5..8925e8e 100644 --- a/kelaifen/Kelaifen_V6.0/trajectory_program.py +++ b/kelaifen/Kelaifen_V6.0/trajectory_program.py @@ -47,6 +47,9 @@ serial_shared = SerialSharedData() # 轨迹插值点数 Interpolation_points = 20 +programinit_lock2 = 1 +programinit_lock6 = 1 + # 第一段程序封装为函数 # 第二段程序封装为函数(上升喷涂) # 第三段程序封装为函数 @@ -110,13 +113,13 @@ def program1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, time.sleep(sleep_time) i = i + 1 - suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + #suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") print("第一段程序执行完") def program1_1(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): print("第1_1段程序开始执行") i = 0 - suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + #suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") turn_off_relay() global increments_x, increments_y, increments_z global Coordinate_compensation @@ -164,7 +167,8 @@ def program1_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star # 遍历插值结果，逐个发送位姿（保留你原有发送逻辑） for modified_list in interpolated_poses: # 初始化透传 - if i == 0: + if (i == 0 and programinit_lock2 == 1): + programinit_lock2 = 0 suc, result, id = sendCMD(sock, "transparent_transmission_init", { "lookahead": lookahead_time, "t": sample_time, @@ -176,7 +180,7 @@ def program1_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star time.sleep(sleep_time) i = i + 1 - suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + #suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") print("第1_1段程序执行完") def program2(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): @@ -219,7 +223,8 @@ def program2(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, modified_list = [val + inc for val, inc in zip(line_list, increments)] Pose_after_adjustment = modified_list # print(Pose_after_adjustment) - if (i == 0): + if (i == 0 and programinit_lock2 == 1): + programinit_lock2 = 0 suc, result, id = sendCMD(sock, "transparent_transmission_init", { "lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0}) send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list}) @@ -233,7 +238,7 @@ def program2(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, print(f"关闭喷枪2：{turn_off_relay_start} ~ {turn_off_relay_end}") i = i + 1 - suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + #suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") print("第二段程序执行完") @@ -288,13 +293,13 @@ def program3(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, wait_stop(sock) time.sleep(sleep_time) i = i + 1 - suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + #suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") print("第三段程序执行完") def program3_1(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): print("第3_1段程序开始执行") i = 0 - suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + #suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") turn_off_relay() global increments_x, increments_y, increments_z global Coordinate_compensation @@ -343,7 +348,8 @@ def program3_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star # 遍历插值结果，逐个发送位姿（保留你原有发送逻辑） for modified_list in interpolated_poses: # 初始化透传 - if i == 0: + if (i == 0 and programinit_lock2 == 1): + programinit_lock2 = 0 suc, result, id = sendCMD(sock, "transparent_transmission_init", { "lookahead": lookahead_time, "t": sample_time, @@ -354,7 +360,7 @@ def program3_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list}) time.sleep(sleep_time) i = i + 1 - suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + #suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") print("第3_1段程序执行完") def program4(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): @@ -370,7 +376,7 @@ def program4(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, i = 0 sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment) sleep_time = sample_time*0.001 - suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + #suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") for line_idx, line_list in enumerate(Pose_2): if len(line_list) != 6: @@ -395,7 +401,8 @@ def program4(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, modified_list = [val + inc for val, inc in zip(line_list, increments)] Pose_after_adjustment = modified_list # print(Pose_after_adjustment) - if (i == 0): + if (i == 0 and programinit_lock2 == 1): + programinit_lock2 = 0 suc, result, id = sendCMD(sock, "transparent_transmission_init", { "lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0}) send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list}) @@ -408,7 +415,7 @@ def program4(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, print(f"关闭喷枪4：{ex_turn_off_relay_start} ~ {ex_turn_off_relay_end}") i = i + 1 - suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + #suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") print("第四段程序执行完") @@ -465,13 +472,13 @@ def program5(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, time.sleep(sleep_time) i = i + 1 - suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + #suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") print("第五段程序执行完") def program5_1(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): print("第5_1段程序开始执行") i = 0 - suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + #suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") turn_off_relay() global increments_x, increments_y, increments_z global Coordinate_compensation @@ -520,7 +527,8 @@ def program5_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star # 遍历插值结果，逐个发送位姿（保留你原有发送逻辑） for modified_list in interpolated_poses: # 初始化透传 - if i == 0: + if (i == 0 and programinit_lock6 == 1): + programinit_lock6 = 0 suc, result, id = sendCMD(sock, "transparent_transmission_init", { "lookahead": lookahead_time, "t": sample_time, @@ -531,7 +539,7 @@ def program5_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list}) time.sleep(sleep_time) i = i + 1 - suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + #suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") print("第5_1段程序执行完") def program6(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): @@ -570,7 +578,8 @@ def program6(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, modified_list = [val + inc for val, inc in zip(line_list, increments)] Pose_after_adjustment = modified_list # print(Pose_after_adjustment) - if (i == 0): + if (i == 0 and programinit_lock6 == 1): + programinit_lock6 = 0 suc, result, id = sendCMD(sock, "transparent_transmission_init", { "lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0}) send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list}) @@ -582,7 +591,7 @@ def program6(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, turn_off_relay() print(f"关闭喷枪6：{turn_off_relay_start} ~ {turn_off_relay_end}") i = i + 1 - suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + #suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") print("第六段程序执行完") @@ -637,13 +646,13 @@ def program7(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, time.sleep(sleep_time) i = i + 1 - suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + #suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") print("第七段程序执行完") def program7_1(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): print("第7_1段程序开始执行") i = 0 - suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + #suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") turn_off_relay() global increments_x, increments_y, increments_z global Coordinate_compensation @@ -690,7 +699,8 @@ def program7_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star # 遍历插值结果，逐个发送位姿（保留你原有发送逻辑） for modified_list in interpolated_poses: # 初始化透传 - if i == 0: + if (i == 0 and programinit_lock6 == 1): + programinit_lock6 = 0 suc, result, id = sendCMD(sock, "transparent_transmission_init", { "lookahead": lookahead_time, "t": sample_time, @@ -701,7 +711,7 @@ def program7_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list}) time.sleep(sleep_time) i = i + 1 - suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + #suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") print("第7_1段程序执行完") def program8(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): @@ -712,7 +722,7 @@ def program8(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, global lookahead_time global Pose_after_adjustment - suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + #suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") lookahead_time = 400 i = 0 sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment) @@ -742,7 +752,8 @@ def program8(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, modified_list = [val + inc for val, inc in zip(line_list, increments)] Pose_after_adjustment = modified_list # print(Pose_after_adjustment) - if (i == 0): + if (i == 0 and programinit_lock6 == 1): + programinit_lock6 = 0 suc, result, id = sendCMD(sock, "transparent_transmission_init", { "lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0}) send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list}) @@ -755,7 +766,7 @@ def program8(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, print(f"关闭喷枪8：{ex_turn_off_relay_start} ~ {ex_turn_off_relay_end}") i = i + 1 - suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + #suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") print("第八段程序执行完")