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

【调通打标】新增向下喷涂逻辑并可以跑起来

master
Lizongdi 2 weeks ago
parent
bd849fb0ce
commit
364a68302f
4 changed files with 168 additions and 43 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. 40
      kelaifen/Kelaifen_V6.0/Example_0_5.py
  2. 24
      kelaifen/Kelaifen_V6.0/config.json
  3. 4
      kelaifen/Kelaifen_V6.0/program_config.py
  4. 139
      kelaifen/Kelaifen_V6.0/trajectory_program.py

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

@ -201,6 +201,9 @@ if __name__ == "__main__":
#print(f"执行序列程序: {current_program}")
# 读取可能的停止指令（超时0.1秒，不阻塞循环）
delay = program_delay_map.get(current_program, 0.0)
if delay <= 0:
delay = 0
stop_cmd_bytes = read_cmd_from_shared(timeout=0.1)
if stop_cmd_bytes is not None and stop_cmd_bytes == b'\xAA\x30':
print("收到0xAA 0x30,停止循环")
@ -211,20 +214,7 @@ if __name__ == "__main__":
running = False
time.sleep(0.5)
else:
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)
# 按程序编号从字典获取延时，精准执行
delay = program_delay_map.get(current_program, 0.0)
if delay > 0:
print(f"program{current_program}执行完成，延时{delay}秒...")
time.sleep(delay)
else:
print(f"当前执行程序是: {current_program}（无指定延时）")
# 移动到下一个程序索引
#current_index = (current_index + 1) % len(sequence_1) # 注意这里要换成当前使用的 sequence
# time.sleep(0.05)
running = program_switch[current_program](sock, ser, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start, delay)
# 回到洗枪位置指令
elif sub_cmd1 == 0x0a:
print(f"接收到指令0xAA 0x{sub_cmd1:02X}，切换执行对应程序")
@ -276,19 +266,17 @@ if __name__ == "__main__":
delay = program_delay_map.get(current_program, 0.0)
if delay <= 0:
delay = 0
stop_cmd_bytes = read_cmd_from_shared(timeout=0.1)
if stop_cmd_bytes is not None and stop_cmd_bytes == b'\xAA\x30':
print("收到0xAA 0x30,停止循环")
for i in range(3):
ser.write(b'\x80\x80')
ser.flush()
time.sleep(0.01)
running = False
time.sleep(0.5)
else:
running = program_switch[current_program](sock, ser, 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)
# if delay > 0:
# print(f"program{current_program}执行完成，延时{delay}秒...")
# time.sleep(delay)
# else:
# print(f"当前执行程序是: {current_program}（无指定延时）")
# 移动到下一个程序索引
#current_index = (current_index + 1) % len(sequence_3) # 注意这里要换成当前使用的 sequence
# time.sleep(0.05)
#回到洗枪位置指令
elif sub_cmd1 == 0x0a:
print(f"接收到指令0xAA 0x{sub_cmd1:02X}，切换执行对应程序")

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

@ -1,7 +1,7 @@
{
"PRESET_PARAMS": [
{
"params": [80, -150],
"params": [30, -150],
"program_delay_map": {
"1": 1.0,
"100": 0.1,
@ -24,7 +24,7 @@
}
},
{
"params": [80, -150],
"params": [30, -150],
"program_delay_map": {
"1": 1.0,
"100": 0.1,
@ -47,7 +47,7 @@
}
},
{
"params": [80, -150],
"params": [30, -150],
"program_delay_map": {
"1": 1.0,
"100": 0.1,
@ -70,7 +70,7 @@
}
},
{
"params": [80, -150],
"params": [30, -150],
"program_delay_map": {
"1": 1.0,
"100": 0.1,
@ -93,7 +93,7 @@
}
},
{
"params": [80, -150],
"params": [30, -150],
"program_delay_map": {
"1": 1.0,
"100": 0.1,
@ -116,7 +116,7 @@
}
},
{
"params": [80, -150],
"params": [30, -150],
"program_delay_map": {
"1": 1.0,
"100": 0.1,
@ -139,7 +139,7 @@
}
},
{
"params": [80, -150],
"params": [30, -150],
"program_delay_map": {
"1": 1.0,
"100": 0.1,
@ -162,7 +162,7 @@
}
},
{
"params": [80, -150],
"params": [30, -150],
"program_delay_map": {
"1": 1.0,
"100": 0.1,
@ -185,7 +185,7 @@
}
},
{
"params": [80, -150],
"params": [30, -150],
"program_delay_map": {
"1": 1.0,
"100": 0.1,
@ -208,7 +208,7 @@
}
},
{
"params": [80, -150],
"params": [30, -150],
"program_delay_map": {
"1": 1.0,
"100": 0.1,
@ -231,7 +231,7 @@
}
},
{
"params": [80, -150],
"params": [30, -150],
"program_delay_map": {
"1": 1.0,
"100": 0.1,
@ -254,7 +254,7 @@
}
},
{
"params": [80, -150],
"params": [30, -150],
"program_delay_map": {
"1": 1.0,
"100": 0.1,

4
kelaifen/Kelaifen_V6.0/program_config.py
View File

@ -2,13 +2,13 @@ import json
from trajectory_program import (
program1, program1_1, program2, program3, program3_1, program4,
program5, program5_1, program6, program7, program7_1, program8,
program9, program10, program11, program12, program100
program9, program10, program11, program12, program100, program101
)
program_switch = {
1: program1,
100: program1_1,
2: program2,
2: program101,
3: program3,
300: program3_1,
4: program4,

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

@ -12,7 +12,8 @@ from serial_init import SerialSharedData
from trajectory_data import (
Pose_1, Pose_2, Pose_3, Pose_4, Pose_3_4, Pose_4_3,
joint_positions_assembled_1, joint_positions_assembled_2,
joint_positions_assembled_3, joint_positions_assembled_4
joint_positions_assembled_3, joint_positions_assembled_4,
Pose_1_2, Pose_2_1,
)
import time
# 导入串口处理模块
@ -1013,6 +1014,11 @@ def program100(sock, ser, turn_on_relay_start, turn_off_relay_start, ex_turn_on_
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
Pose_3_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_3_M]
Pose_4_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_4_M]
Pose_3_4_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_3_4_M]
Pose_4_3_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_4_3_M]
#0.延时操作，点位延时
#1.添加点位延时
NUM = int(delay * 1000 / sample_time)
@ -1102,4 +1108,135 @@ def program100(sock, ser, turn_on_relay_start, turn_off_relay_start, ex_turn_on_
return False
def program101(sock, ser, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start, delay):
global sample_time, lookahead_time
lookahead_time = 300 # 前瞻时间 (ms)
sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment)
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_1)
Pose_4_M = copy.deepcopy(Pose_2)
# 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_1[i][2] + Off_Set_Value * factor
Pose_4_M[i][2] = Pose_2[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_1[i][2] + Off_Set_Value
Pose_4_M[i][2] = Pose_2[i][2] + Off_Set_Value
# 4.处理 Pose_1_2 和 Pose_2_1
num = len(Pose_1_2) # 假设两者长度相同
Pose_3_4_M = copy.deepcopy(Pose_1_2)
Pose_4_3_M = copy.deepcopy(Pose_2_1)
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_1_2[i][2] - Off_Set_Value * factor + Off_Set_Value
Pose_4_3_M[i][2] = Pose_2_1[i][2] - Off_Set_Value * factor + Off_Set_Value
Pose_3_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_3_M]
Pose_4_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_4_M]
Pose_3_4_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_3_4_M]
Pose_4_3_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_4_3_M]
#0.延时操作，点位延时
#1.添加点位延时
NUM = int(delay * 1000 / sample_time)
# 2.第一道喷涂延时点位
last_point_3_4 = Pose_3_4_M[-1].copy() # 先复制最后一个点位
Pose_3_4_Stop = [last_point_3_4.copy() for _ in range(NUM)] # 每个点位独立副本
# 3.第2道喷涂延时点位
last_point_4_3 = Pose_4_3_M[-1].copy()
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")
time.sleep(0.05)
#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)
off_set_Time=0
# 1.喷枪1开关时间计算
OPEN_1=turn_on_relay_start
CLOSE_1=turn_off_relay_start
OPEN_1_M = off_set_Time+OPEN_1
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 })
cycles = 100 # 循环5次s
for _ in range(cycles):
for traj in trajectories:
print("开始执行轨迹...")
# 针对不同轨迹进行计数
if traj is Pose_3_M:
for idx, point in enumerate(traj, start=1):
if idx == OPEN_1_M:
turn_on_relay()
if idx == CLOSE_1_M1S:
turn_off_relay()
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(f"停止轨迹，NUM = {NUM}, delay = {delay}")
stop_cmd_bytes = read_cmd_from_shared(timeout=0.1)
if stop_cmd_bytes is not None and stop_cmd_bytes == b'\xAA\x30':
print("收到0xAA 0x30,停止循环")
for i in range(3):
ser.write(b'\x80\x80')
ser.flush()
time.sleep(0.01)
time.sleep(0.5)
return False
print("轨迹执行完成")
return False

Write Preview
Loading…
Cancel
Save