【调通打标】延时逻辑成功

kelaifen/Kelaifen_V6.0/Example_0_5.py

@@ -217,7 +217,7 @@ if __name__ == "__main__":
                                         running = False
                                         time.sleep(0.5)
                                     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)
+                                        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)
                                 
                                     # 按程序编号从字典获取延时，精准执行
@@ -280,6 +280,9 @@ if __name__ == "__main__":
 
                                     # 读取可能的停止指令（超时0.1秒，不阻塞循环）
                                     stop_cmd_bytes = read_cmd_from_shared(timeout=0.1)
+                                    delay = program_delay_map.get(current_program, 0.0)
+                                    if delay <= 0:
+                                        delay = 0
                                     if stop_cmd_bytes is not None and stop_cmd_bytes == b'\xAA\x30':
                                         print("收到0xAA 0x30,停止循环")
                                         for i in range(3):
@@ -292,12 +295,12 @@ if __name__ == "__main__":
                                         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)
-                                    if delay > 0:
-                                        print(f"program{current_program}执行完成，延时{delay}秒...")
-                                        time.sleep(delay)
-                                    else:
-                                        print(f"当前执行程序是: {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_3)  # 注意这里要换成当前使用的 sequence

kelaifen/Kelaifen_V6.0/trajectory_program.py

@@ -976,16 +976,24 @@ def program12(sock):
     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):
+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):
     global sample_time, lookahead_time
     sample_time = 20          # 采样时间 (ms)
-    lookahead_time = 400      # 前瞻时间 (ms)
+    lookahead_time = 300      # 前瞻时间 (ms)
     sleep_time = sample_time * 0.001   # 休眠时间 (秒)
+    #1.添加点位延时
+    NUM = int(delay * 1000 / sample_time)
+    # 2.第一道喷涂延时点位
+    last_point_3_4 = Pose_3_4[-1].copy()  # 先复制最后一个点位
+    Pose_3_4_Stop = [last_point_3_4.copy() for _ in range(NUM)]  # 每个点位独立副本
+    # 3.第2道喷涂延时点位
+    last_point_4_3 = Pose_4_3[-1].copy()
+    Pose_4_3_Stop = [last_point_4_3.copy() for _ in range(NUM)]
     # ----- 一次性清理和初始化透传服务 -----
 
     sendCMD(sock, "tt_clear_servo_joint_buf")
     time.sleep(0.05)
-    trajectories = [Pose_3, Pose_3_4, Pose_4, Pose_4_3]
+    trajectories = [Pose_3, Pose_3_4,Pose_3_4_Stop, Pose_4, Pose_4_3,Pose_4_3_Stop]
     sendCMD(sock, "transparent_transmission_init", {"lookahead": lookahead_time,"t": sample_time,"smoothness": 1,"response_enable": 0 })
     cycles = 10  # 循环5次
     for _ in range(cycles):