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

【调通打标】斜率和开关枪时间生效

master
Lizongdi 3 weeks ago
parent
ba55149ac0
commit
250d5e510f
4 changed files with 106 additions and 101 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. 2
      kelaifen/Kelaifen_V6.0/Example_0_5.py
  2. 76
      kelaifen/Kelaifen_V6.0/config.json
  3. 24
      kelaifen/Kelaifen_V6.0/trajectory_data.py
  4. 105
      kelaifen/Kelaifen_V6.0/trajectory_program.py

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

@ -292,7 +292,7 @@ if __name__ == "__main__":
running = False running = False
time.sleep(0.5) time.sleep(0.5)
else: 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) # delay = program_delay_map.get(current_program, 0.0)

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

@ -11,20 +11,16 @@
"4": 0.1, "4": 0.1,
"5": 1.0, "5": 1.0,
"500": 0.1, "500": 0.1,
"6": 0.5, "6": 0.1,
"7": 1.0, "7": 1.0,
"700": 0.1, "700": 0.1,
"8": 0.5 "8": 0.1
}, },
"relay_config": { "relay_config": {
"turn_on_relay_start": 20, "turn_on_relay_start": 20,
"turn_on_relay_end": 25,
"turn_off_relay_start": 135, "turn_off_relay_start": 135,
"turn_off_relay_end": 140,
"ex_turn_on_relay_start": 20, "ex_turn_on_relay_start": 20,
"ex_turn_on_relay_end": 25, "ex_turn_off_relay_start": 135
"ex_turn_off_relay_start": 135,
"ex_turn_off_relay_end": 140
} }
}, },
{ {
@ -45,13 +41,9 @@
}, },
"relay_config": { "relay_config": {
"turn_on_relay_start": 20, "turn_on_relay_start": 20,
"turn_on_relay_end": 25,
"turn_off_relay_start": 135, "turn_off_relay_start": 135,
"turn_off_relay_end": 140,
"ex_turn_on_relay_start": 20, "ex_turn_on_relay_start": 20,
"ex_turn_on_relay_end": 25, "ex_turn_off_relay_start": 135
"ex_turn_off_relay_start": 135,
"ex_turn_off_relay_end": 140
} }
}, },
{ {
@ -72,13 +64,9 @@
}, },
"relay_config": { "relay_config": {
"turn_on_relay_start": 20, "turn_on_relay_start": 20,
"turn_on_relay_end": 25,
"turn_off_relay_start": 135, "turn_off_relay_start": 135,
"turn_off_relay_end": 140,
"ex_turn_on_relay_start": 20, "ex_turn_on_relay_start": 20,
"ex_turn_on_relay_end": 25, "ex_turn_off_relay_start": 135
"ex_turn_off_relay_start": 135,
"ex_turn_off_relay_end": 140
} }
}, },
{ {
@ -99,13 +87,9 @@
}, },
"relay_config": { "relay_config": {
"turn_on_relay_start": 20, "turn_on_relay_start": 20,
"turn_on_relay_end": 25,
"turn_off_relay_start": 135, "turn_off_relay_start": 135,
"turn_off_relay_end": 140,
"ex_turn_on_relay_start": 20, "ex_turn_on_relay_start": 20,
"ex_turn_on_relay_end": 25, "ex_turn_off_relay_start": 135
"ex_turn_off_relay_start": 135,
"ex_turn_off_relay_end": 140
} }
}, },
{ {
@ -126,13 +110,9 @@
}, },
"relay_config": { "relay_config": {
"turn_on_relay_start": 20, "turn_on_relay_start": 20,
"turn_on_relay_end": 25,
"turn_off_relay_start": 135, "turn_off_relay_start": 135,
"turn_off_relay_end": 140,
"ex_turn_on_relay_start": 20, "ex_turn_on_relay_start": 20,
"ex_turn_on_relay_end": 25, "ex_turn_off_relay_start": 135
"ex_turn_off_relay_start": 135,
"ex_turn_off_relay_end": 140
} }
}, },
{ {
@ -153,13 +133,9 @@
}, },
"relay_config": { "relay_config": {
"turn_on_relay_start": 20, "turn_on_relay_start": 20,
"turn_on_relay_end": 25,
"turn_off_relay_start": 135, "turn_off_relay_start": 135,
"turn_off_relay_end": 140,
"ex_turn_on_relay_start": 20, "ex_turn_on_relay_start": 20,
"ex_turn_on_relay_end": 25, "ex_turn_off_relay_start": 135
"ex_turn_off_relay_start": 135,
"ex_turn_off_relay_end": 140
} }
}, },
{ {
@ -180,13 +156,9 @@
}, },
"relay_config": { "relay_config": {
"turn_on_relay_start": 20, "turn_on_relay_start": 20,
"turn_on_relay_end": 25,
"turn_off_relay_start": 135, "turn_off_relay_start": 135,
"turn_off_relay_end": 140,
"ex_turn_on_relay_start": 20, "ex_turn_on_relay_start": 20,
"ex_turn_on_relay_end": 25, "ex_turn_off_relay_start": 135
"ex_turn_off_relay_start": 135,
"ex_turn_off_relay_end": 140
} }
}, },
{ {
@ -207,13 +179,9 @@
}, },
"relay_config": { "relay_config": {
"turn_on_relay_start": 20, "turn_on_relay_start": 20,
"turn_on_relay_end": 25,
"turn_off_relay_start": 135, "turn_off_relay_start": 135,
"turn_off_relay_end": 140,
"ex_turn_on_relay_start": 20, "ex_turn_on_relay_start": 20,
"ex_turn_on_relay_end": 25, "ex_turn_off_relay_start": 135
"ex_turn_off_relay_start": 135,
"ex_turn_off_relay_end": 140
} }
}, },
{ {
@ -234,13 +202,9 @@
}, },
"relay_config": { "relay_config": {
"turn_on_relay_start": 20, "turn_on_relay_start": 20,
"turn_on_relay_end": 25,
"turn_off_relay_start": 135, "turn_off_relay_start": 135,
"turn_off_relay_end": 140,
"ex_turn_on_relay_start": 20, "ex_turn_on_relay_start": 20,
"ex_turn_on_relay_end": 25, "ex_turn_off_relay_start": 135
"ex_turn_off_relay_start": 135,
"ex_turn_off_relay_end": 140
} }
}, },
{ {
@ -261,13 +225,9 @@
}, },
"relay_config": { "relay_config": {
"turn_on_relay_start": 20, "turn_on_relay_start": 20,
"turn_on_relay_end": 25,
"turn_off_relay_start": 135, "turn_off_relay_start": 135,
"turn_off_relay_end": 140,
"ex_turn_on_relay_start": 20, "ex_turn_on_relay_start": 20,
"ex_turn_on_relay_end": 25, "ex_turn_off_relay_start": 135
"ex_turn_off_relay_start": 135,
"ex_turn_off_relay_end": 140
} }
}, },
{ {
@ -288,13 +248,9 @@
}, },
"relay_config": { "relay_config": {
"turn_on_relay_start": 20, "turn_on_relay_start": 20,
"turn_on_relay_end": 25,
"turn_off_relay_start": 135, "turn_off_relay_start": 135,
"turn_off_relay_end": 140,
"ex_turn_on_relay_start": 20, "ex_turn_on_relay_start": 20,
"ex_turn_on_relay_end": 25, "ex_turn_off_relay_start": 135
"ex_turn_off_relay_start": 135,
"ex_turn_off_relay_end": 140
} }
}, },
{ {
@ -315,13 +271,9 @@
}, },
"relay_config": { "relay_config": {
"turn_on_relay_start": 20, "turn_on_relay_start": 20,
"turn_on_relay_end": 25,
"turn_off_relay_start": 135, "turn_off_relay_start": 135,
"turn_off_relay_end": 140,
"ex_turn_on_relay_start": 20, "ex_turn_on_relay_start": 20,
"ex_turn_on_relay_end": 25, "ex_turn_off_relay_start": 135
"ex_turn_off_relay_start": 135,
"ex_turn_off_relay_end": 140
} }
} }
] ]

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

@ -634,11 +634,6 @@ Pose_4 = [
joint_positions_assembled_4 = [[92.553,-45.178,53.196,-97.284,97.995,15.177]] joint_positions_assembled_4 = [[92.553,-45.178,53.196,-97.284,97.995,15.177]]
Pose_3_4 = [ 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, 370.4375, -3.1075, 0.0078, -0.1622],
[-228.1, 1631.7, 380.6750, -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], [-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, 544.4750, -3.1075, 0.0078, -0.1622],
[-228.1, 1631.7, 554.7125, -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],
[-228.1, 1631.7, 564.9500, -3.1075, 0.0078, -0.1622]
] ]
Pose_4_3 = [ 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, 370.4375, 3.1398, 0.0349, -1.5584],
[1549.9, -66.7470, 380.6750, 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], [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, 544.4750, 3.1398, 0.0349, -1.5584],
[1549.9, -66.7470, 554.7125, 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],
[1549.9, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584]
] ]

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

@ -1,3 +1,4 @@
import copy
from queue import Queue 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
@ -976,37 +977,113 @@ def program12(sock):
print("第十二段程序执行完") 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 global sample_time, lookahead_time
sample_time = 20 # 采样时间 (ms) sample_time = 20 # 采样时间 (ms)
lookahead_time = 300 # 前瞻时间 (ms) lookahead_time = 300 # 前瞻时间 (ms)
sleep_time = sample_time * 0.001 # 休眠时间 (秒) 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.添加点位延时 #1.添加点位延时
NUM = int(delay * 1000 / sample_time) NUM = int(delay * 1000 / sample_time)
# 2.第一道喷涂延时点位 # 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)] # 每个点位独立副本 Pose_3_4_Stop = [last_point_3_4.copy() for _ in range(NUM)] # 每个点位独立副本
# 3.第2道喷涂延时点位 # 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)] 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") sendCMD(sock, "tt_clear_servo_joint_buf")
time.sleep(0.05) 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 }) 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 _ in range(cycles):
for traj in trajectories: for traj in trajectories:
print("开始执行轨迹...") print("开始执行轨迹...")
for point in traj: # 针对不同轨迹进行计数
if not compensate_queue.empty(): if traj is Pose_3_M:
increments_x, increments_y, increments_z = compensate_queue.get_nowait() for idx, point in enumerate(traj, start=1):
# point[0] += increments_x if idx == OPEN_1_M:
# point[1] += increments_y turn_on_relay()
# point[2] += increments_z if idx == CLOSE_1_M1S:
send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point}) turn_off_relay()
time.sleep(sleep_time) 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("轨迹执行完成") print("轨迹执行完成")
return False return False

Write Preview
Loading…
Cancel
Save