LiZongdi
/
Huangjiabao_backup
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

确保只调用一次清空和初始化

master
Lizongdi 3 weeks ago
parent
64d2d0c6ed
commit
ea77c41d9a
1 changed files with 37 additions and 26 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 63
      kelaifen/Kelaifen_V6.0/trajectory_program.py

63
kelaifen/Kelaifen_V6.0/trajectory_program.py
View File

@ -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("第八段程序执行完")

Write Preview
Loading…
Cancel
Save