diff --git a/kelaifen/Kelaifen_V6.0/trajectory_program.py b/kelaifen/Kelaifen_V6.0/trajectory_program.py index 8925e8e..942f2c5 100644 --- a/kelaifen/Kelaifen_V6.0/trajectory_program.py +++ b/kelaifen/Kelaifen_V6.0/trajectory_program.py @@ -47,9 +47,6 @@ serial_shared = SerialSharedData() # 轨迹插值点数 Interpolation_points = 20 -programinit_lock2 = 1 -programinit_lock6 = 1 - # 第一段程序封装为函数 # 第二段程序封装为函数(上升喷涂) # 第三段程序封装为函数 @@ -113,13 +110,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 @@ -167,8 +164,7 @@ 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 and programinit_lock2 == 1): - programinit_lock2 = 0 + if i == 0: suc, result, id = sendCMD(sock, "transparent_transmission_init", { "lookahead": lookahead_time, "t": sample_time, @@ -180,7 +176,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): @@ -223,8 +219,7 @@ 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 and programinit_lock2 == 1): - programinit_lock2 = 0 + if (i == 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}) @@ -238,7 +233,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("第二段程序执行完") @@ -293,13 +288,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 @@ -348,8 +343,7 @@ 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 and programinit_lock2 == 1): - programinit_lock2 = 0 + if i == 0: suc, result, id = sendCMD(sock, "transparent_transmission_init", { "lookahead": lookahead_time, "t": sample_time, @@ -360,7 +354,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): @@ -376,7 +370,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: @@ -401,8 +395,7 @@ 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 and programinit_lock2 == 1): - programinit_lock2 = 0 + if (i == 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}) @@ -415,7 +408,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("第四段程序执行完") @@ -472,13 +465,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 @@ -527,8 +520,7 @@ 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 and programinit_lock6 == 1): - programinit_lock6 = 0 + if i == 0: suc, result, id = sendCMD(sock, "transparent_transmission_init", { "lookahead": lookahead_time, "t": sample_time, @@ -539,7 +531,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): @@ -578,8 +570,7 @@ 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 and programinit_lock6 == 1): - programinit_lock6 = 0 + if (i == 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}) @@ -591,7 +582,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("第六段程序执行完") @@ -646,13 +637,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 @@ -699,8 +690,7 @@ 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 and programinit_lock6 == 1): - programinit_lock6 = 0 + if i == 0: suc, result, id = sendCMD(sock, "transparent_transmission_init", { "lookahead": lookahead_time, "t": sample_time, @@ -711,7 +701,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): @@ -722,7 +712,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) @@ -752,8 +742,7 @@ 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 and programinit_lock6 == 1): - programinit_lock6 = 0 + if (i == 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}) @@ -766,7 +755,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("第八段程序执行完")