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("串口已关闭")