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

更新点位并解决错误

master
Lizongdi 3 weeks ago
parent
dd3c95fedf
commit
4588d0a150
4 changed files with 852 additions and 805 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 21
      kelaifen/Kelaifen_V6.0/Example_0_5.py
  2. 168
      kelaifen/Kelaifen_V6.0/config.json
  3. 1388
      kelaifen/Kelaifen_V6.0/trajectory_data.py
  4. 80
      kelaifen/Kelaifen_V6.0/trajectory_program.py

21
kelaifen/Kelaifen_V6.0/Example_0_5.py
View File

@ -154,20 +154,14 @@ if __name__ == "__main__":
program_delay_map = selected_delay_map program_delay_map = selected_delay_map
# 更新继电器参数 # 更新继电器参数
turn_on_relay_start = selected_relay_config["turn_on_relay_start"] turn_on_relay_start = selected_relay_config["turn_on_relay_start"]
turn_on_relay_end = selected_relay_config["turn_on_relay_end"]
turn_off_relay_start = selected_relay_config["turn_off_relay_start"] turn_off_relay_start = selected_relay_config["turn_off_relay_start"]
turn_off_relay_end = selected_relay_config["turn_off_relay_end"]
ex_turn_on_relay_start = selected_relay_config["ex_turn_on_relay_start"] ex_turn_on_relay_start = selected_relay_config["ex_turn_on_relay_start"]
ex_turn_on_relay_end = selected_relay_config["ex_turn_on_relay_end"]
ex_turn_off_relay_start = selected_relay_config["ex_turn_off_relay_start"] ex_turn_off_relay_start = selected_relay_config["ex_turn_off_relay_start"]
ex_turn_off_relay_end = selected_relay_config["ex_turn_off_relay_end"]
print(f"\n✅ 参数已更新为选中配置：") print(f"\n✅ 参数已更新为选中配置：")
print(f"program_delay_map: {program_delay_map}") print(f"program_delay_map: {program_delay_map}")
print(f"继电器开启区间：{turn_on_relay_start} ~ {turn_on_relay_end}") print(f"轨迹1开枪时间：{turn_on_relay_start} ~ {turn_off_relay_start}")
print(f"继电器关闭区间：{turn_off_relay_start} ~ {turn_off_relay_end}") print(f"轨迹2开枪时间：{ex_turn_on_relay_start} ~ {ex_turn_off_relay_start}")
print(f"继电器开启区间2：{ex_turn_on_relay_start} ~ {ex_turn_on_relay_end}")
print(f"继电器关闭区间2：{ex_turn_off_relay_start} ~ {ex_turn_off_relay_end}")
# 喷漆开始命令接收函数 # 喷漆开始命令接收函数
main_cmd_bytes = read_cmd_from_shared(timeout=1.0) main_cmd_bytes = read_cmd_from_shared(timeout=1.0)
# 解析主指令（帧头后第一字节为指令值） # 解析主指令（帧头后第一字节为指令值）
@ -178,7 +172,7 @@ if __name__ == "__main__":
# 情况1: 接收到1 # 情况1: 接收到1
if cmd == 1: if cmd == 1:
print("触发情况1: 执行program1") print("触发情况1: 执行program1")
program_switch[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) program_switch[1](sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start)
time.sleep(delay_time_10_13579_30) time.sleep(delay_time_10_13579_30)
for i in range(3): for i in range(3):
@ -217,7 +211,7 @@ if __name__ == "__main__":
running = False running = False
time.sleep(0.5) time.sleep(0.5)
else: else:
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) program_switch[current_program](sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start)
# print(current_program) # print(current_program)
# 按程序编号从字典获取延时，精准执行 # 按程序编号从字典获取延时，精准执行
@ -250,7 +244,7 @@ if __name__ == "__main__":
# 情况3: 接收到5 # 情况3: 接收到5
elif cmd == 5: elif cmd == 5:
print("触发情况3: 执行program5") print("触发情况3: 执行program5")
program_switch[5](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) program_switch[5](sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start)
time.sleep(delay_time_10_13579_30) time.sleep(delay_time_10_13579_30)
for i in range(3): for i in range(3):
@ -282,9 +276,8 @@ if __name__ == "__main__":
stop_cmd_bytes = read_cmd_from_shared(timeout=0.1) stop_cmd_bytes = read_cmd_from_shared(timeout=0.1)
delay = program_delay_map.get(current_program, 0.0) delay = program_delay_map.get(current_program, 0.0)
if delay <= 0: if delay <= 0:
delay = 0 delay = 0
else: running = program_switch[current_program](sock, ser, stop_cmd_bytes, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start, delay)
running = program_switch[current_program](sock, ser, stop_cmd_bytes, 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) # delay = program_delay_map.get(current_program, 0.0)

168
kelaifen/Kelaifen_V6.0/config.json
View File

@ -1,7 +1,7 @@
{ {
"PRESET_PARAMS": [ "PRESET_PARAMS": [
{ {
"params": [50, -100], "params": [80, -150],
"program_delay_map": { "program_delay_map": {
"1": 1.0, "1": 1.0,
"100": 0.1, "100": 0.1,
@ -11,20 +11,20 @@
"4": 0.1, "4": 0.1,
"5": 1.0, "5": 1.0,
"500": 0.1, "500": 0.1,
"6": 0.1, "6": 10,
"7": 1.0, "7": 1.0,
"700": 0.1, "700": 0.1,
"8": 0.1 "8": 0.2
}, },
"relay_config": { "relay_config": {
"turn_on_relay_start": 20, "turn_on_relay_start": 0,
"turn_off_relay_start": 135, "turn_off_relay_start": 134,
"ex_turn_on_relay_start": 20, "ex_turn_on_relay_start": 0,
"ex_turn_off_relay_start": 135 "ex_turn_off_relay_start": 134
} }
}, },
{ {
"params": [50, -50], "params": [80, -150],
"program_delay_map": { "program_delay_map": {
"1": 1.0, "1": 1.0,
"100": 0.1, "100": 0.1,
@ -34,20 +34,20 @@
"4": 0.1, "4": 0.1,
"5": 1.0, "5": 1.0,
"500": 0.1, "500": 0.1,
"6": 0.5, "6": 10,
"7": 1.0, "7": 1.0,
"700": 0.1, "700": 0.1,
"8": 0.5 "8": 0.2
}, },
"relay_config": { "relay_config": {
"turn_on_relay_start": 20, "turn_on_relay_start": 0,
"turn_off_relay_start": 135, "turn_off_relay_start": 134,
"ex_turn_on_relay_start": 20, "ex_turn_on_relay_start": 0,
"ex_turn_off_relay_start": 135 "ex_turn_off_relay_start": 134
} }
}, },
{ {
"params": [50, 0], "params": [80, -150],
"program_delay_map": { "program_delay_map": {
"1": 1.0, "1": 1.0,
"100": 0.1, "100": 0.1,
@ -57,20 +57,20 @@
"4": 0.1, "4": 0.1,
"5": 1.0, "5": 1.0,
"500": 0.1, "500": 0.1,
"6": 0.5, "6": 10,
"7": 1.0, "7": 1.0,
"700": 0.1, "700": 0.1,
"8": 0.5 "8": 0.2
}, },
"relay_config": { "relay_config": {
"turn_on_relay_start": 20, "turn_on_relay_start": 0,
"turn_off_relay_start": 135, "turn_off_relay_start": 134,
"ex_turn_on_relay_start": 20, "ex_turn_on_relay_start": 0,
"ex_turn_off_relay_start": 135 "ex_turn_off_relay_start": 134
} }
}, },
{ {
"params": [50, 50], "params": [80, -150],
"program_delay_map": { "program_delay_map": {
"1": 1.0, "1": 1.0,
"100": 0.1, "100": 0.1,
@ -80,20 +80,20 @@
"4": 0.1, "4": 0.1,
"5": 1.0, "5": 1.0,
"500": 0.1, "500": 0.1,
"6": 0.5, "6": 10,
"7": 1.0, "7": 1.0,
"700": 0.1, "700": 0.1,
"8": 0.5 "8": 0.2
}, },
"relay_config": { "relay_config": {
"turn_on_relay_start": 20, "turn_on_relay_start": 0,
"turn_off_relay_start": 135, "turn_off_relay_start": 134,
"ex_turn_on_relay_start": 20, "ex_turn_on_relay_start": 0,
"ex_turn_off_relay_start": 135 "ex_turn_off_relay_start": 134
} }
}, },
{ {
"params": [50, 100], "params": [80, -150],
"program_delay_map": { "program_delay_map": {
"1": 1.0, "1": 1.0,
"100": 0.1, "100": 0.1,
@ -103,20 +103,20 @@
"4": 0.1, "4": 0.1,
"5": 1.0, "5": 1.0,
"500": 0.1, "500": 0.1,
"6": 0.5, "6": 10,
"7": 1.0, "7": 1.0,
"700": 0.1, "700": 0.1,
"8": 0.5 "8": 0.2
}, },
"relay_config": { "relay_config": {
"turn_on_relay_start": 20, "turn_on_relay_start": 0,
"turn_off_relay_start": 135, "turn_off_relay_start": 134,
"ex_turn_on_relay_start": 20, "ex_turn_on_relay_start": 0,
"ex_turn_off_relay_start": 135 "ex_turn_off_relay_start": 134
} }
}, },
{ {
"params": [100, -100], "params": [80, -150],
"program_delay_map": { "program_delay_map": {
"1": 1.0, "1": 1.0,
"100": 0.1, "100": 0.1,
@ -126,20 +126,20 @@
"4": 0.1, "4": 0.1,
"5": 1.0, "5": 1.0,
"500": 0.1, "500": 0.1,
"6": 0.5, "6": 10,
"7": 1.0, "7": 1.0,
"700": 0.1, "700": 0.1,
"8": 0.5 "8": 0.2
}, },
"relay_config": { "relay_config": {
"turn_on_relay_start": 20, "turn_on_relay_start": 0,
"turn_off_relay_start": 135, "turn_off_relay_start": 134,
"ex_turn_on_relay_start": 20, "ex_turn_on_relay_start": 0,
"ex_turn_off_relay_start": 135 "ex_turn_off_relay_start": 134
} }
}, },
{ {
"params": [100, -50], "params": [80, -150],
"program_delay_map": { "program_delay_map": {
"1": 1.0, "1": 1.0,
"100": 0.1, "100": 0.1,
@ -149,20 +149,20 @@
"4": 0.1, "4": 0.1,
"5": 1.0, "5": 1.0,
"500": 0.1, "500": 0.1,
"6": 0.5, "6": 10,
"7": 1.0, "7": 1.0,
"700": 0.1, "700": 0.1,
"8": 0.5 "8": 0.2
}, },
"relay_config": { "relay_config": {
"turn_on_relay_start": 20, "turn_on_relay_start": 0,
"turn_off_relay_start": 135, "turn_off_relay_start": 134,
"ex_turn_on_relay_start": 20, "ex_turn_on_relay_start": 0,
"ex_turn_off_relay_start": 135 "ex_turn_off_relay_start": 134
} }
}, },
{ {
"params": [100, 0], "params": [80, -150],
"program_delay_map": { "program_delay_map": {
"1": 1.0, "1": 1.0,
"100": 0.1, "100": 0.1,
@ -172,20 +172,20 @@
"4": 0.1, "4": 0.1,
"5": 1.0, "5": 1.0,
"500": 0.1, "500": 0.1,
"6": 0.5, "6": 10,
"7": 1.0, "7": 1.0,
"700": 0.1, "700": 0.1,
"8": 0.5 "8": 0.2
}, },
"relay_config": { "relay_config": {
"turn_on_relay_start": 20, "turn_on_relay_start": 0,
"turn_off_relay_start": 135, "turn_off_relay_start": 134,
"ex_turn_on_relay_start": 20, "ex_turn_on_relay_start": 0,
"ex_turn_off_relay_start": 135 "ex_turn_off_relay_start": 134
} }
}, },
{ {
"params": [100, 50], "params": [80, -150],
"program_delay_map": { "program_delay_map": {
"1": 1.0, "1": 1.0,
"100": 0.1, "100": 0.1,
@ -195,20 +195,20 @@
"4": 0.1, "4": 0.1,
"5": 1.0, "5": 1.0,
"500": 0.1, "500": 0.1,
"6": 0.5, "6": 10,
"7": 1.0, "7": 1.0,
"700": 0.1, "700": 0.1,
"8": 0.5 "8": 0.2
}, },
"relay_config": { "relay_config": {
"turn_on_relay_start": 20, "turn_on_relay_start": 0,
"turn_off_relay_start": 135, "turn_off_relay_start": 134,
"ex_turn_on_relay_start": 20, "ex_turn_on_relay_start": 0,
"ex_turn_off_relay_start": 135 "ex_turn_off_relay_start": 134
} }
}, },
{ {
"params": [100, 100], "params": [80, -150],
"program_delay_map": { "program_delay_map": {
"1": 1.0, "1": 1.0,
"100": 0.1, "100": 0.1,
@ -218,20 +218,20 @@
"4": 0.1, "4": 0.1,
"5": 1.0, "5": 1.0,
"500": 0.1, "500": 0.1,
"6": 0.5, "6": 10,
"7": 1.0, "7": 1.0,
"700": 0.1, "700": 0.1,
"8": 0.5 "8": 0.2
}, },
"relay_config": { "relay_config": {
"turn_on_relay_start": 20, "turn_on_relay_start": 0,
"turn_off_relay_start": 135, "turn_off_relay_start": 134,
"ex_turn_on_relay_start": 20, "ex_turn_on_relay_start": 0,
"ex_turn_off_relay_start": 135 "ex_turn_off_relay_start": 134
} }
}, },
{ {
"params": [90, 50], "params": [80, -150],
"program_delay_map": { "program_delay_map": {
"1": 1.0, "1": 1.0,
"100": 0.1, "100": 0.1,
@ -241,20 +241,20 @@
"4": 0.1, "4": 0.1,
"5": 1.0, "5": 1.0,
"500": 0.1, "500": 0.1,
"6": 0.5, "6": 10,
"7": 1.0, "7": 1.0,
"700": 0.1, "700": 0.1,
"8": 0.5 "8": 0.2
}, },
"relay_config": { "relay_config": {
"turn_on_relay_start": 20, "turn_on_relay_start": 0,
"turn_off_relay_start": 135, "turn_off_relay_start": 134,
"ex_turn_on_relay_start": 20, "ex_turn_on_relay_start": 0,
"ex_turn_off_relay_start": 135 "ex_turn_off_relay_start": 134
} }
}, },
{ {
"params": [90, 150], "params": [80, -150],
"program_delay_map": { "program_delay_map": {
"1": 1.0, "1": 1.0,
"100": 0.1, "100": 0.1,
@ -264,16 +264,16 @@
"4": 0.1, "4": 0.1,
"5": 1.0, "5": 1.0,
"500": 0.1, "500": 0.1,
"6": 0.5, "6": 10,
"7": 1.0, "7": 1.0,
"700": 0.1, "700": 0.1,
"8": 0.5 "8": 0.2
}, },
"relay_config": { "relay_config": {
"turn_on_relay_start": 20, "turn_on_relay_start": 0,
"turn_off_relay_start": 135, "turn_off_relay_start": 134,
"ex_turn_on_relay_start": 20, "ex_turn_on_relay_start": 0,
"ex_turn_off_relay_start": 135 "ex_turn_off_relay_start": 134
} }
} }
] ]

1388
kelaifen/Kelaifen_V6.0/trajectory_data.py
View File

File diff suppressed because it is too large

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

@ -4,8 +4,8 @@ from queue import Queue
from ETController import send_Point, sendCMD, wait_stop from ETController import send_Point, sendCMD, wait_stop
from gpio import turn_off_relay, turn_on_relay from gpio import turn_off_relay, turn_on_relay
# from program_config import ( # from program_config import (
# PRESET_PARAMS, turn_off_relay_end, turn_off_relay_start, # PRESET_PARAMS, turn_off_relay_start,
# turn_on_relay_end, turn_on_relay_start # turn_on_relay_start
# ) # )
from serial_init import SerialSharedData from serial_init import SerialSharedData
from trajectory_data import ( from trajectory_data import (
@ -60,7 +60,7 @@ Interpolation_points = 20
# 第九段程序封装为函数 # 第九段程序封装为函数
# 第十二段程序封装为函数 # 第十二段程序封装为函数
def program1(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): def program1(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start):
i = 0 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() turn_off_relay()
@ -115,7 +115,7 @@ def program1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start,
suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
print("第一段程序执行完") 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): def program1_1(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start):
print("第1_1段程序开始执行") print("第1_1段程序开始执行")
i = 0 i = 0
suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
@ -181,7 +181,7 @@ def program1_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star
suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
print("第1_1段程序执行完") 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): def program2(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start):
global increments_x, increments_y, increments_z global increments_x, increments_y, increments_z
global Coordinate_compensation global Coordinate_compensation
global sample_time global sample_time
@ -227,19 +227,19 @@ def program2(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start,
send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list}) send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list})
time.sleep(sleep_time) time.sleep(sleep_time)
if(i >= turn_on_relay_start and i <= turn_on_relay_end): if(i >= turn_on_relay_start):
turn_on_relay() turn_on_relay()
print(f"打开喷枪2：{turn_on_relay_start} ~ {turn_on_relay_end}") print(f"打开喷枪2：{turn_on_relay_start}")
if(i >= turn_off_relay_start and i <= turn_off_relay_end): if(i >= turn_off_relay_start):
turn_off_relay() turn_off_relay()
print(f"关闭喷枪2：{turn_off_relay_start} ~ {turn_off_relay_end}") print(f"关闭喷枪2：{turn_off_relay_start}")
i = i + 1 i = i + 1
suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
print("第二段程序执行完") print("第二段程序执行完")
def program3(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): def program3(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start):
print("第三段程序开始执行") print("第三段程序开始执行")
i = 0 i = 0
suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
@ -293,7 +293,7 @@ def program3(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start,
suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
print("第三段程序执行完") 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): def program3_1(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start):
print("第3_1段程序开始执行") print("第3_1段程序开始执行")
i = 0 i = 0
suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
@ -359,7 +359,7 @@ def program3_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star
suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
print("第3_1段程序执行完") 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): def program4(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start):
print("第四段程序开始执行") print("第四段程序开始执行")
global increments_x, increments_y, increments_z global increments_x, increments_y, increments_z
global Coordinate_compensation global Coordinate_compensation
@ -402,19 +402,19 @@ def program4(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start,
"lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0}) "lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0})
send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list}) send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list})
time.sleep(sleep_time) time.sleep(sleep_time)
if(i >= ex_turn_on_relay_start and i <= ex_turn_on_relay_end): if(i >= ex_turn_on_relay_start):
turn_on_relay() turn_on_relay()
print(f"打开喷枪4：{ex_turn_on_relay_start} ~ {ex_turn_on_relay_end}") print(f"打开喷枪4：{ex_turn_on_relay_start}")
if(i >= ex_turn_off_relay_start and i <= ex_turn_off_relay_end): if(i >= ex_turn_off_relay_start):
turn_off_relay() turn_off_relay()
print(f"关闭喷枪4：{ex_turn_off_relay_start} ~ {ex_turn_off_relay_end}") print(f"关闭喷枪4：{ex_turn_off_relay_start}")
i = i + 1 i = i + 1
suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
print("第四段程序执行完") print("第四段程序执行完")
def program5(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): def program5(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start):
print("第五段程序开始执行") print("第五段程序开始执行")
i = 0 i = 0
suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
@ -470,7 +470,7 @@ def program5(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start,
suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
print("第五段程序执行完") 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): def program5_1(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start):
print("第5_1段程序开始执行") print("第5_1段程序开始执行")
i = 0 i = 0
suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
@ -536,7 +536,7 @@ def program5_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star
suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
print("第5_1段程序执行完") 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): def program6(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start):
global increments_x, increments_y, increments_z global increments_x, increments_y, increments_z
global Coordinate_compensation global Coordinate_compensation
global sample_time global sample_time
@ -577,18 +577,18 @@ def program6(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start,
"lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0}) "lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0})
send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list}) send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list})
time.sleep(sleep_time) time.sleep(sleep_time)
if(i >= turn_on_relay_start and i <= turn_on_relay_end): if(i >= turn_on_relay_start):
turn_on_relay() turn_on_relay()
print(f"打开喷枪6：{turn_on_relay_start} ~ {turn_on_relay_end}") print(f"打开喷枪6：{turn_on_relay_start}")
if(i >= turn_off_relay_start and i <= turn_off_relay_end): if(i >= turn_off_relay_start):
turn_off_relay() turn_off_relay()
print(f"关闭喷枪6：{turn_off_relay_start} ~ {turn_off_relay_end}") print(f"关闭喷枪6：{turn_off_relay_start}")
i = i + 1 i = i + 1
suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
print("第六段程序执行完") print("第六段程序执行完")
def program7(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): def program7(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start):
i = 0 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() turn_off_relay()
@ -642,7 +642,7 @@ def program7(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start,
suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
print("第七段程序执行完") 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): def program7_1(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start):
print("第7_1段程序开始执行") print("第7_1段程序开始执行")
i = 0 i = 0
suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
@ -706,7 +706,7 @@ def program7_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star
suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
print("第7_1段程序执行完") 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): def program8(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start):
global increments_x, increments_y, increments_z global increments_x, increments_y, increments_z
global Coordinate_compensation global Coordinate_compensation
global sample_time global sample_time
@ -749,12 +749,12 @@ def program8(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start,
"lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0}) "lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0})
send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list}) send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list})
time.sleep(sleep_time) time.sleep(sleep_time)
if(i >= ex_turn_on_relay_start and i <= ex_turn_on_relay_end): if(i >= ex_turn_on_relay_start):
turn_on_relay() turn_on_relay()
print(f"打开喷枪8：{ex_turn_on_relay_start} ~ {ex_turn_on_relay_end}") print(f"打开喷枪8：{ex_turn_on_relay_start}")
if(i >= ex_turn_off_relay_start and i <= ex_turn_off_relay_end): if(i >= ex_turn_off_relay_start):
turn_off_relay() turn_off_relay()
print(f"关闭喷枪8：{ex_turn_off_relay_start} ~ {ex_turn_off_relay_end}") print(f"关闭喷枪8：{ex_turn_off_relay_start}")
i = i + 1 i = i + 1
suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
@ -1033,10 +1033,11 @@ def program100(sock, ser, stop_cmd_bytes, turn_on_relay_start, turn_off_relay_st
# 第2道开枪时间在第15个点位（Pose_4_M的开枪点位 lookahead_time/sample_time+15），，关枪时间在第150点位示例（lookahead_time/sample_time+135，，，，如果数据>155，那么关枪就在Pose_4_3_M中） # 第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) off_set_Time=int(lookahead_time / sample_time)
off_set_Time=0
# 1.喷枪1开关时间计算 # 1.喷枪1开关时间计算
OPEN_1=turn_on_relay_start OPEN_1=turn_on_relay_start
CLOSE_1=turn_off_relay_start CLOSE_1=turn_off_relay_start
OPEN_1_M = off_set_Time+5 OPEN_1_M = off_set_Time+OPEN_1
CLOSE_1_M1S=off_set_Time+CLOSE_1 CLOSE_1_M1S=off_set_Time+CLOSE_1
CLOSE_1_M1E = off_set_Time + CLOSE_1-155 CLOSE_1_M1E = off_set_Time + CLOSE_1-155
#2.喷枪2开关时间计算 #2.喷枪2开关时间计算
@ -1086,14 +1087,15 @@ def program100(sock, ser, stop_cmd_bytes, turn_on_relay_start, turn_off_relay_st
for point in traj: for point in traj:
send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point}) send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point})
time.sleep(sleep_time) time.sleep(sleep_time)
if stop_cmd_bytes is not None and stop_cmd_bytes == b'\xAA\x30': print(f"停止轨迹，NUM = {NUM}, delay = {delay}")
print("收到0xAA 0x30,停止循环") # if stop_cmd_bytes is not None and stop_cmd_bytes == b'\xAA\x30':
for i in range(3): # print("收到0xAA 0x30,停止循环")
ser.write(b'\x80\x80') # for i in range(3):
ser.flush() # ser.write(b'\x80\x80')
time.sleep(0.01) # ser.flush()
time.sleep(0.5) # time.sleep(0.01)
return False # time.sleep(0.5)
# return False
print("轨迹执行完成") print("轨迹执行完成")
return False return False

Write Preview
Loading…
Cancel
Save