From 364a68302fb8b2dfc04a7ade43e6e0b6178837ac Mon Sep 17 00:00:00 2001 From: Lizongdi <1210855344@qq.com> Date: Tue, 2 Jun 2026 10:36:27 +0800 Subject: [PATCH] =?UTF-8?q?=E3=80=90=E8=B0=83=E9=80=9A=E6=89=93=E6=A0=87?= =?UTF-8?q?=E3=80=91=E6=96=B0=E5=A2=9E=E5=90=91=E4=B8=8B=E5=96=B7=E6=B6=82?= =?UTF-8?q?=E9=80=BB=E8=BE=91=E5=B9=B6=E5=8F=AF=E4=BB=A5=E8=B7=91=E8=B5=B7?= =?UTF-8?q?=E6=9D=A5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- kelaifen/Kelaifen_V6.0/Example_0_5.py | 44 +++--- kelaifen/Kelaifen_V6.0/config.json | 24 ++-- kelaifen/Kelaifen_V6.0/program_config.py | 4 +- kelaifen/Kelaifen_V6.0/trajectory_program.py | 139 ++++++++++++++++++- 4 files changed, 168 insertions(+), 43 deletions(-) diff --git a/kelaifen/Kelaifen_V6.0/Example_0_5.py b/kelaifen/Kelaifen_V6.0/Example_0_5.py index a9c768c..7d1c2ad 100644 --- a/kelaifen/Kelaifen_V6.0/Example_0_5.py +++ b/kelaifen/Kelaifen_V6.0/Example_0_5.py @@ -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}，切换执行对应程序") @@ -275,20 +265,18 @@ if __name__ == "__main__": # 读取可能的停止指令（超时0.1秒，不阻塞循环） delay = program_delay_map.get(current_program, 0.0) if delay <= 0: - delay = 0 - 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) + 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) #回到洗枪位置指令 elif sub_cmd1 == 0x0a: print(f"接收到指令0xAA 0x{sub_cmd1:02X}，切换执行对应程序") diff --git a/kelaifen/Kelaifen_V6.0/config.json b/kelaifen/Kelaifen_V6.0/config.json index 43d26a8..eff7e15 100644 --- a/kelaifen/Kelaifen_V6.0/config.json +++ b/kelaifen/Kelaifen_V6.0/config.json @@ -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, diff --git a/kelaifen/Kelaifen_V6.0/program_config.py b/kelaifen/Kelaifen_V6.0/program_config.py index d207089..02d664a 100644 --- a/kelaifen/Kelaifen_V6.0/program_config.py +++ b/kelaifen/Kelaifen_V6.0/program_config.py @@ -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, diff --git a/kelaifen/Kelaifen_V6.0/trajectory_program.py b/kelaifen/Kelaifen_V6.0/trajectory_program.py index e7c5e78..16244f5 100644 --- a/kelaifen/Kelaifen_V6.0/trajectory_program.py +++ b/kelaifen/Kelaifen_V6.0/trajectory_program.py @@ -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 + + + +