【跑通未验证】停止功能

kelaifen/Kelaifen_V6.0/Example_0_5.py

@@ -283,16 +283,8 @@ if __name__ == "__main__":
                                     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):
-                                            ser.write(b'\x80\x80')
-                                            ser.flush()
-                                            time.sleep(0.01)
-                                        running = False
-                                        time.sleep(0.5)
                                     else:                                    
-                                        running = program_switch[current_program](sock, 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, stop_cmd_bytes, 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

@@ -1,3 +1,4 @@
+import sys
 import copy
 from queue import Queue
 from ETController import send_Point, sendCMD, wait_stop
@@ -977,7 +978,7 @@ def program12(sock):
     print("第十二段程序执行完")
 
 
-def program100(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start, delay):
+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):
     global sample_time, lookahead_time
     sample_time = 20          # 采样时间 (ms)
     lookahead_time = 300      # 前瞻时间 (ms)
@@ -1084,6 +1085,14 @@ def program100(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay
                 for point in traj:
                     send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point})
                     time.sleep(sleep_time)
+                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