【调通打标】机器人停止功能正常

kelaifen/Kelaifen_V6.0/Example_0_5.py

@@ -273,11 +273,10 @@ if __name__ == "__main__":
                                     print(f"执行序列程序: {current_program}")
 
                                     # 读取可能的停止指令（超时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                                 
-                                    running = program_switch[current_program](sock,  ser, stop_cmd_bytes, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start, delay)
+                                    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)

kelaifen/Kelaifen_V6.0/trajectory_program.py

@@ -3,6 +3,7 @@ import copy
 from queue import Queue
 from ETController import send_Point, sendCMD, wait_stop
 from gpio import turn_off_relay, turn_on_relay
+from serial_handler import read_cmd_from_shared
 # from program_config import ( 
 #     PRESET_PARAMS,  turn_off_relay_start, 
 #      turn_on_relay_start
@@ -978,7 +979,7 @@ def program12(sock):
     print("第十二段程序执行完")
 
 
-def program100(sock, ser, stop_cmd_bytes, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start, delay):
+def program100(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)
 
@@ -1088,14 +1089,15 @@ def program100(sock, ser, stop_cmd_bytes, turn_on_relay_start, turn_off_relay_st
                     send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point})
                     time.sleep(sleep_time)
                     print(f"停止轨迹，NUM = {NUM}, delay = {delay}")
-                # if stop_cmd_bytes is not None and stop_cmd_bytes == b'\xAA\x30':
+                stop_cmd_bytes = read_cmd_from_shared(timeout=0.1)
-                #     print("收到0xAA 0x30,停止循环")
+                if stop_cmd_bytes is not None and stop_cmd_bytes == b'\xAA\x30':
-                #     for i in range(3):
+                    print("收到0xAA 0x30,停止循环")
-                #         ser.write(b'\x80\x80')
+                    for i in range(3):
-                #         ser.flush()
+                        ser.write(b'\x80\x80')
-                #         time.sleep(0.01)
+                        ser.flush()
-                #     time.sleep(0.5)
+                        time.sleep(0.01)
-                #     return False
+                    time.sleep(0.5)
+                    return False
             print("轨迹执行完成")
 
     return False