【初版打标】新结构调通，当前延时问题已经解决

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:

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
+
+