diff --git a/kelaifen/Kelaifen_V6.0/Example_0_5.py b/kelaifen/Kelaifen_V6.0/Example_0_5.py index d30fb38..a2c297e 100644 --- a/kelaifen/Kelaifen_V6.0/Example_0_5.py +++ b/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) diff --git a/kelaifen/Kelaifen_V6.0/trajectory_program.py b/kelaifen/Kelaifen_V6.0/trajectory_program.py index f5c4b0b..5333774 100644 --- a/kelaifen/Kelaifen_V6.0/trajectory_program.py +++ b/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