Huangjiabao_backup/kelaifen/Kelaifen_V6.0/Example_0_5.py

import time
import serial  
from serial import Serial  
import threading  
from ETController import connectETController,sendCMD,send_Point,wait_stop
from gpio import init_gpio, turn_off_relay, turn_on_relay
from program_config import load_process_config, program_switch,sequence_1, sequence_2, sequence_3, sequence_4
from serial_init import serial_init
from serial_handler import handle_special_commands, read_cmd_from_shared, serial_receive_data

if __name__ == "__main__":

    # 加载配置文件并提取参数
    process_config = load_process_config()
    if process_config:
        # 提取预设参数列表（现在PRESET_PARAMS本身包含所有子配置）
        PRESET_PARAMS = process_config["PRESET_PARAMS"]

    init_gpio()
    ser = serial_init()
    # 检查串口初始化结果
    if not ser or not isinstance(ser, serial.Serial) or not ser.is_open:
        print("串口初始化失败，无法继续运行")
        # 确保在函数内部返回
        exit()

    time.sleep(20)
    # 默认关闭喷枪
    turn_off_relay()  

    # 机器人IP地址
    robot_ip = "192.168.1.100"
    result = connectETController(robot_ip)
    sock = None
    if len(result) == 2:
        conSuc, sock = result
        if conSuc:
            print("成功连接到机器人")
            # 1.上电设置
            ret, result, id = sendCMD(sock, "set_robot_power_status", {"status": 1})
            print("开始上电")
            time.sleep(20)
            print("开始检测上电状态")
            ret, result, id = sendCMD(sock, "get_robot_power_status")

            if result == 0:
                ret, result, id = sendCMD(sock, "set_robot_power_status", {"status": 1})
                time.sleep(20)
            else :
                print("已上电")
            # 2. 清除报警
            print("清除报警")
            ret, result, id = sendCMD(sock,"clearAlarm")
            print("清除报警返回值")
            print(result)
            time.sleep(5)
            print("报警清除完成")

            # 获取同步状态
            suc, result , id = sendCMD(sock, "getMotorStatus")
            if result == 0:
                # 同步伺服编码器数据
                suc,result,id = sendCMD(sock, "syncMotorStatus")
                if result:
                    print("同步伺服编码器数据成功")
                else:
                    print("同步伺服编码器数据失败")
                time. sleep (2)

            # 3. 编码器零位校准
            suc, result, id = sendCMD(sock, "getServoStatus")
            print("获取伺服状态返回值")
            print(result)
            time.sleep(2)
            if result == 0:
                # 设置机械臂伺服状态为“开”（status=1）
                suc, result, id = sendCMD(sock, "set_servo_status", {"status": 1})
                print("设置机械臂伺服状态返回值")
                print(result)
                time.sleep(5)
                suc, result, id = sendCMD(sock, "calibrate_encoder_zero_position")
                print("编码器校准返回值")
                print(result)
                time.sleep(5)
                if result == 1:
                    print("编码器校准成功")
                else:
                    print("编码器校准失败")

            # 初始化透传服务
            suc, result, id = sendCMD(sock, "get_transparent_transmission_state")
            if id == 1:
                print(suc, result, id)
                if result == 1:
                    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
                    print("已清空透传缓存")
                    time.sleep(0.5)
                else:
                    print("无缓存")

            # 在此处做检测等相关操作
            suc, result, id = sendCMD(sock, "getRobotState")
            if (result == 4):
                # 清除报警
                suc, result, id = sendCMD(sock, "clearAlarm")
                time.sleep(0.5)

            # 获取同步状态
            suc, result, id = sendCMD(sock, "getMotorStatus")
            if (result == 0):
                # 同步伺服编码器数据
                suc, result, id = sendCMD(sock, "syncMotorStatus")
                time.sleep(0.5)
            else:
                print("同步状态获取失败")

            # 获取机械臂伺服状态
            suc, result, id = sendCMD(sock, "getServoStatus")
            if (result == 0):
                # 设置机械臂伺服状态ON
                suc, result, id = sendCMD(sock, "set_servo_status", {"status": 1})
                time.sleep(1)
            else:
                print("机械臂伺服取失败")

            # 获取当前机器人是否处于透传状态
            suc, result, id = sendCMD(sock, "get_transparent_transmission_state")
            print(result)
            if (result == 1):
                # 清空透传缓存
                suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
                time.sleep(0.5)
            else:
                print("非缓存状态")

            # 启动串口接收线程（daemon=True：主程序退出时线程自动终止）
            receive_thread = threading.Thread(target=serial_receive_data, args=(ser,), daemon=True)
            receive_thread.start()
            print("串口接收线程已启动")

            delay_time_10_13579_30 = 2

            # 外层主循环
            while True:
                try:
                    # 等待主指令（从共享缓冲区读取）
                    print("\n等待串口指令...")
                    main_cmd_bytes = None
                    while main_cmd_bytes is None:
                        # 喷漆工艺参数配置选择（速度、角度、搭接时间）
                        selected_delay_map, selected_relay_config = handle_special_commands(PRESET_PARAMS)
                        if selected_delay_map is not None and selected_relay_config is not None:
                            # 更新delay配置
                            program_delay_map = selected_delay_map
                            # 更新继电器参数
                            turn_on_relay_start = selected_relay_config["turn_on_relay_start"]
                            turn_off_relay_start = selected_relay_config["turn_off_relay_start"]
                            ex_turn_on_relay_start = selected_relay_config["ex_turn_on_relay_start"]
                            ex_turn_off_relay_start = selected_relay_config["ex_turn_off_relay_start"]

                            print(f"\n✅ 参数已更新为选中配置：")
                            print(f"program_delay_map: {program_delay_map}")
                            print(f"轨迹1开枪时间：{turn_on_relay_start} ~ {turn_off_relay_start}")
                            print(f"轨迹2开枪时间：{ex_turn_on_relay_start} ~ {ex_turn_off_relay_start}")
                        # 喷漆开始命令接收函数
                        main_cmd_bytes = read_cmd_from_shared(timeout=1.0)
                    # 解析主指令（帧头后第一字节为指令值）
                    cmd = main_cmd_bytes[1]
                    print(f"收到串口指令: 0x{cmd:02X}")

                    # region 情况1
                    # 情况1: 接收到1
                    if cmd == 1:
                        print("触发情况1: 执行program1")
                        program_switch[1](sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start)
                        time.sleep(delay_time_10_13579_30)

                        for i in range(3):
                            ser.write(b'\x80\x80')
                            ser.flush()
                            time.sleep(0.01)
                        print("执行program1结束,已发送响应0x80 0x80")

                        # 等待子指令接收
                        sub_cmd_bytes = None
                        while sub_cmd_bytes is None:
                            sub_cmd_bytes = read_cmd_from_shared(timeout=1.0)
                            time.sleep(0.01)
                        sub_cmd = sub_cmd_bytes[0]  # 应为0xAA
                        sub_cmd1 = sub_cmd_bytes[1]  # 子指令值（如0x20、0x0a）
                        print(f"收到子指令: 0x{sub_cmd:02X} 0x{sub_cmd1:02X}")

                        if sub_cmd == 0xAA:
                            if sub_cmd1 == 0x20:
                                print("开始情况1循环序列: program2->300->4->100...")
                                current_index = 0
                                running = True
                                while running:
                                    # 执行当前序列中的程序
                                    current_program = sequence_1[current_index]
                                    #print(f"执行序列程序: {current_program}")

                                    # 读取可能的停止指令（超时0.1秒，不阻塞循环）
                                    stop_cmd_bytes = read_cmd_from_shared(timeout=0.1)
                                    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:
                                        program_switch[current_program](sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start)
                                        # print(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_1)  # 注意这里要换成当前使用的 sequence
                                    # time.sleep(0.05)
                            # 回到洗枪位置指令
                            elif sub_cmd1 == 0x0a:
                                print(f"接收到指令0xAA 0x{sub_cmd1:02X}，切换执行对应程序")
                                program_switch[10](sock)
                                time.sleep(delay_time_10_13579_30)
                                for i in range(3):
                                    ser.write(b'\x80\x80')
                                    ser.flush()
                                    time.sleep(0.01)
                                print("执行program10结束,已发送响应0x80 0x80")
                                print("返回最外层等待串口数据")

                            else:
                                print(f"收到未知子指令: 0x{sub_cmd:02X}，继续等待")
                    #endregion
                    # region 情况3
                    # 情况3: 接收到5
                    elif cmd == 5:
                        print("触发情况3: 执行program5")
                        program_switch[5](sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start)

                        time.sleep(delay_time_10_13579_30)
                        for i in range(3):
                            ser.write(b'\x80\x80')
                            ser.flush()
                            time.sleep(0.01)
                        print("执行program5结束,已发送响应0x80 0x80")

                        # 子指令接收（调用read_cmd_from_shared）
                        print("等待0x20开始循环或其他指令...")
                        sub_cmd_bytes = None
                        while sub_cmd_bytes is None:
                            sub_cmd_bytes = read_cmd_from_shared(timeout=1.0)
                            time.sleep(0.01)
                        sub_cmd = sub_cmd_bytes[0]
                        sub_cmd1 = sub_cmd_bytes[1]
                        print(f"收到子指令: 0x{sub_cmd:02X}")

                        if sub_cmd == 0xAA:
                            if sub_cmd1 == 0x20:
                                print("开始情况3循环序列: program6->700->8->500...")
                                current_index = 0
                                running = True
                                while running:
                                    current_program = sequence_3[current_index]
                                    print(f"执行序列程序: {current_program}")

                                    # 读取可能的停止指令（超时0.1秒，不阻塞循环）
                                    delay = program_delay_map.get(current_program, 0.0)
                                    if delay <= 0:
                                        delay = 0                                 
                                    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)
                                    # 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
                                    # time.sleep(0.05)
                            #回到洗枪位置指令
                            elif sub_cmd1 == 0x0a:
                                print(f"接收到指令0xAA 0x{sub_cmd1:02X}，切换执行对应程序")
                                program_switch[10](sock)
                                time.sleep(delay_time_10_13579_30)
                                for i in range(3):
                                    ser.write(b'\x80\x80')
                                    ser.flush()
                                    time.sleep(0.01)
                                print("执行program10结束,已发送响应0x80 0x80")
                                print("返回最外层等待串口数据")
                            else:
                                print(f"收到未知子指令: 0x{sub_cmd:02X}，继续等待")
                    #endregion

                    # region 程序9，返回停机位置
                    elif cmd == 9:
                        print("执行program9,返回停机位置")
                        program_switch[9](sock)
                        time.sleep(delay_time_10_13579_30)
                        for i in range(3):
                            ser.write(b'\x80\x80')
                            ser.flush()
                            time.sleep(0.1)        
                        print("执行program9结束,已发送响应0x80 0x80")
                        print("返回最外层等待串口数据")
                    #endregion
                    
                    # region 程序10，前往初始位置
                    elif cmd == 10:
                        print("触发情况6: 执行program10")
                        program_switch[10](sock)
                        time.sleep(delay_time_10_13579_30)
                        for i in range(3):
                            ser.write(b'\x80\x80')
                            ser.flush()
                            time.sleep(0.1)
                        print("执行program10结束,已发送响应0x80 0x80")
                        print("返回最外层等待串口数据")
                    # endregion

                    # region 程序11，前往洗枪位置
                    elif cmd == 11:
                        print("执行program11,前往洗枪位置")
                        program_switch[11](sock)
                        time.sleep(delay_time_10_13579_30)
                        for i in range(3):
                            ser.write(b'\x80\x80')
                            ser.flush()
                            time.sleep(0.1)
                        print("执行program11结束,已发送响应0x80 0x80")
                        print("返回最外层等待串口数据")
                    #endregion
                        
                    # region 程序12，前往试枪位置
                    elif cmd == 12:
                        print("执行program12，前往试枪位置")
                        program_switch[12](sock)
                        time.sleep(delay_time_10_13579_30)
                        for i in range(3):
                            ser.write(b'\x80\x80')
                            ser.flush()
                            time.sleep(0.1)                      
                        print("执行program12结束,已发送响应0x80 0x80")
                        print("返回最外层等待串口数据")
                    #endregion

                    # 退出指令
                    elif cmd == 0x00:
                        print("收到退出指令，程序终止")
                        break
                    # 开喷枪           
                    elif cmd == 16: 
                        turn_on_relay()
                        print("打开喷枪")
                    # 关喷枪
                    elif cmd == 17: 
                        turn_off_relay()
                        print("关闭喷枪")
                    # 未知指令
                    else:
                        print(f"未知指令: 0x{cmd:02X}，等待下一条指令...")
                    time.sleep(0.01)

                except KeyboardInterrupt:
                    print("程序被手动中断")
                    break
                except Exception as e:
                    print(f"主循环错误: {e}")
                    time.sleep(0.5)
        else:
            print("连接机器人失败")
    else:
        print("无法连接到机器人")    

  

    # 资源清理
    if sock:
       sock.close()
    if ser and isinstance(ser, serial.Serial) and ser.is_open:
       ser.close()
       print("串口已关闭")