From ce727fe42464700b8d242d5cc493893a89315ac1 Mon Sep 17 00:00:00 2001 From: Lizongdi <1210855344@qq.com> Date: Thu, 28 May 2026 14:59:16 +0800 Subject: [PATCH] =?UTF-8?q?=E3=80=90=E5=88=9D=E7=89=88=E6=89=93=E6=A0=87?= =?UTF-8?q?=E3=80=91=E6=96=B0=E7=BB=93=E6=9E=84=E8=B0=83=E9=80=9A=EF=BC=8C?= =?UTF-8?q?=E5=BD=93=E5=89=8D=E5=BB=B6=E6=97=B6=E9=97=AE=E9=A2=98=E5=B7=B2?= =?UTF-8?q?=E7=BB=8F=E8=A7=A3=E5=86=B3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- kelaifen/Kelaifen_V6.0/Example_0_5.py | 8 ++--- kelaifen/Kelaifen_V6.0/trajectory_program.py | 32 +++++++++++++++++++- 2 files changed, 35 insertions(+), 5 deletions(-) diff --git a/kelaifen/Kelaifen_V6.0/Example_0_5.py b/kelaifen/Kelaifen_V6.0/Example_0_5.py index 0defd1f..50374ac 100644 --- a/kelaifen/Kelaifen_V6.0/Example_0_5.py +++ b/kelaifen/Kelaifen_V6.0/Example_0_5.py @@ -217,7 +217,7 @@ if __name__ == "__main__": running = False time.sleep(0.5) 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) + 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) # print(current_program) # 按程序编号从字典获取延时，精准执行 @@ -229,7 +229,7 @@ if __name__ == "__main__": print(f"当前执行程序是: {current_program}（无指定延时）") # 移动到下一个程序索引 - current_index = (current_index + 1) % len(sequence_1) # 注意这里要换成当前使用的 sequence + #current_index = (current_index + 1) % len(sequence_1) # 注意这里要换成当前使用的 sequence # time.sleep(0.05) # 回到洗枪位置指令 elif sub_cmd1 == 0x0a: @@ -289,7 +289,7 @@ if __name__ == "__main__": running = False time.sleep(0.5) 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) + 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) # 统一字典映射延时 delay = program_delay_map.get(current_program, 0.0) @@ -300,7 +300,7 @@ if __name__ == "__main__": print(f"当前执行程序是: {current_program}（无指定延时）") # 移动到下一个程序索引 - current_index = (current_index + 1) % len(sequence_3) # 注意这里要换成当前使用的 sequence + #current_index = (current_index + 1) % len(sequence_3) # 注意这里要换成当前使用的 sequence # time.sleep(0.05) #回到洗枪位置指令 elif sub_cmd1 == 0x0a: diff --git a/kelaifen/Kelaifen_V6.0/trajectory_program.py b/kelaifen/Kelaifen_V6.0/trajectory_program.py index 942f2c5..0291596 100644 --- a/kelaifen/Kelaifen_V6.0/trajectory_program.py +++ b/kelaifen/Kelaifen_V6.0/trajectory_program.py @@ -7,7 +7,7 @@ from gpio import turn_off_relay, turn_on_relay # ) from serial_init import SerialSharedData from trajectory_data import ( - Pose_1, Pose_2, Pose_3, Pose_4, + 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 ) @@ -974,3 +974,33 @@ def program12(sock): suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") end_time = time.time() 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): + global sample_time, lookahead_time + sample_time = 20 # 采样时间 (ms) + lookahead_time = 400 # 前瞻时间 (ms) + sleep_time = sample_time * 0.001 # 休眠时间 (秒) + # ----- 一次性清理和初始化透传服务 ----- + + sendCMD(sock, "tt_clear_servo_joint_buf") + time.sleep(0.05) + trajectories = [Pose_3, Pose_3_4, Pose_4, Pose_4_3] + sendCMD(sock, "transparent_transmission_init", {"lookahead": lookahead_time,"t": sample_time,"smoothness": 1,"response_enable": 0 }) + cycles = 10 # 循环5次 + for _ in range(cycles): + for traj in trajectories: + print("开始执行轨迹...") + for point in traj: + if not compensate_queue.empty(): + increments_x, increments_y, increments_z = compensate_queue.get_nowait() + # point[0] += increments_x + # point[1] += increments_y + # point[2] += increments_z + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point}) + time.sleep(sleep_time) + print("轨迹执行完成") + + return False + +