Feeding_control_system/Fedding.py

import socket
import binascii
import time
import threading
import struct
import cv2
import os
from pathlib import Path
from pymodbus.client import ModbusTcpClient
from pymodbus.exceptions import ModbusException
# 添加视觉模块路径
import sys

sys.path.append(os.path.join(os.path.dirname(__file__), 'src', 'vision'))
# 导入视觉处理模块
from src.vision.anger_caculate import predict_obb_best_angle
from src.vision.resize_tuili_image_main import classify_image_weighted, load_global_rois, crop_and_resize, YOLO


class FeedingControlSystem:
    def __init__(self, relay_host='192.168.0.18', relay_port=50000):
        # 网络继电器配置
        self.relay_host = relay_host
        self.relay_port = relay_port
        self.relay_modbus_client = ModbusTcpClient(relay_host, port=relay_port)

        # 继电器映射
        self.DOOR_UPPER = 'door_upper'  # DO0 - 上料斗滑动
        self.DOOR_LOWER_1 = 'door_lower_1'  # DO1 - 上料斗出砼门
        self.DOOR_LOWER_2 = 'door_lower_2'  # DO2 - 下料斗出砼门
        self.BREAK_ARCH_UPPER = 'break_arch_upper'  # DO3 - 上料斗破拱
        self.BREAK_ARCH_LOWER = 'break_arch_lower'  # DO4 - 下料斗破拱

        # 继电器命令（原始Socket）mudbus TCP模式
        self.relay_commands = {
            self.DOOR_UPPER: {'open': '00000000000601050000FF00', 'close': '000000000006010500000000'},
            self.DOOR_LOWER_1: {'open': '00000000000601050001FF00', 'close': '000000000006010500010000'},
            self.DOOR_LOWER_2: {'open': '00000000000601050002FF00', 'close': '000000000006010500020000'},
            self.BREAK_ARCH_UPPER: {'open': '00000000000601050003FF00', 'close': '000000000006010500030000'},
            self.BREAK_ARCH_LOWER: {'open': '00000000000601050004FF00', 'close': '000000000006010500040000'}
        }

        # 读取状态命令
        self.read_status_command = '000000000006010100000008'

        # 设备位映射
        self.device_bit_map = {
            self.DOOR_UPPER: 0,
            self.DOOR_LOWER_1: 1,
            self.DOOR_LOWER_2: 2,
            self.BREAK_ARCH_UPPER: 3,
            self.BREAK_ARCH_LOWER: 4
        }

        # 变频器配置（Modbus RTU 协议）
        self.inverter_config = {
            'slave_id': 1,
            'frequency_register': 0x01,  # 2001H
            'start_register': 0x00,  # 2000H
            'stop_register': 0x00,  # 2000H（用于停机）
            'start_command': 0x0013,  # 正转点动运行
            'stop_command': 0x0001  # 停机
        }

        # 变送器配置（Modbus RTU）
        self.transmitter_config = {
            1: {  # 上料斗
                'slave_id': 1,
                'weight_register': 0x01,
                'register_count': 2
            },#发出去的内容01 03 00 01 00 02
            2: {  # 下料斗
                'slave_id': 2,
                'weight_register': 0x01,
                'register_count': 2
            }#发出去的内容02 03 00 01 00 02
        }

        # 系统状态
        self._running = False
        self._monitor_thread = None
        self._visual_control_thread = None
        self._alignment_check_thread = None

        # 下料控制相关
        self.min_required_weight = 500  # 模具车最小需要重量(kg)
        self.upper_door_position = 'default'  # default(在搅拌楼下接料), over_lower(在下料斗上方), returning(返回中)
        self.lower_feeding_stage = 0  # 0:未下料, 1:第一阶段, 2:第二阶段, 3:等待上料, 4:等待模具车对齐
        self.lower_feeding_cycle = 0  # 下料斗下料循环次数 (1, 2, 3)
        self.upper_feeding_count = 0  # 上料斗已下料次数 (0, 1, 2)
        self.last_upper_weight = 0
        self.last_lower_weight = 0
        self.last_weight_time = time.time()
        self.target_vehicle_weight = 5000  # 目标模具车重量(kg)
        self.upper_buffer_weight = 500  # 上料斗缓冲重量(kg)，每次下料多下这么多
        self.single_batch_weight = 2500  # 单次下料重量(kg)


        #夹角状态
        self.angle_control_mode = "normal"  # 角度控制模式: normal, reducing, maintaining, recovery

        # 错误计数
        self.upper_weight_error_count = 0
        self.lower_weight_error_count = 0
        self.max_error_count = 3

        # 下料阶段频率（Hz）
        self.inverter_max_frequency = 400.0#频率最大值
        self.frequencies = [220.0, 230.0, 240.0]

        # 视觉系统接口
        self.overflow_detected = False  # 堆料检测
        self.door_opening_large = False  # 夹角
        self.vehicle_aligned = False  # 模具车是否对齐

        # 视觉控制参数
        self.angle_threshold = 60.0  # 角度阈值，超过此值认为开口过大
        self.target_angle = 20.0  # 目标角度
        self.min_angle = 10.0  # 最小角度
        self.max_angle = 80.0  # 最大角度
        self.angle_tolerance = 5.0  # 角度容差
        self.visual_control_enabled = True  # 视觉控制使能
        self.last_angle = None  # 上次检测角度
        self.visual_check_interval = 1.0  # 视觉检查间隔(秒)
        self.alignment_check_interval = 0.5  # 对齐检查间隔(秒)

        # 模型路径配置
        self.angle_model_path = "src/vision/angle.pt"
        self.overflow_model_path = "src/vision/overflow.pt"
        self.alignment_model_path = "src/vision/alig.pt"  # 模具车对齐检测模型
        self.roi_file_path = "src/vision/roi_coordinates/1_rois.txt"

        # 模型实例
        self.angle_model = None  # 夹角检测模型实例
        self.overflow_model = None  # 堆料检测模型实例
        self.alignment_model = None  # 对齐检测模型实例

        # 摄像头相关配置
        self.camera = None
        self.camera_type = "ip"
        self.camera_ip = "192.168.1.51"
        self.camera_port = 554
        self.camera_username = "admin"
        self.camera_password = "XJ123456"
        self.camera_channel = 1
        self.current_image_path = "current_frame.jpg"

    def set_camera_config(self, camera_type="ip", ip=None, port=None, username=None, password=None, channel=1):
        """
        设置摄像头配置
        :param camera_type: 摄像头类型 "usb" 或 "ip"
        :param ip: 网络摄像头IP地址
        :param port: 网络摄像头端口
        :param username: 网络摄像头用户名
        :param password: 网络摄像头密码
        :param channel: 摄像头通道号
        """
        self.camera_type = camera_type
        if ip:
            self.camera_ip = ip
        if port:
            self.camera_port = port
        if username:
            self.camera_username = username
        if password:
            self.camera_password = password
        self.camera_channel = channel

    def set_angle_parameters(self, target_angle=20.0, min_angle=10.0, max_angle=80.0, threshold=60.0):
        """
        设置角度控制参数
        """
        self.target_angle = target_angle
        self.min_angle = min_angle
        self.max_angle = max_angle
        self.angle_threshold = threshold

    def set_feeding_parameters(self, target_vehicle_weight=5000, upper_buffer_weight=500, single_batch_weight=2500):
        """
        设置下料参数
        :param target_vehicle_weight: 目标模具车重量(kg)
        :param upper_buffer_weight: 上料斗缓冲重量(kg)
        :param single_batch_weight: 单次下料重量(kg)
        """
        self.target_vehicle_weight = target_vehicle_weight
        self.upper_buffer_weight = upper_buffer_weight
        self.single_batch_weight = single_batch_weight

    def load_all_models(self):
        """
        加载所有视觉检测模型
        """
        success = True

        # 加载夹角检测模型
        try:
            if not os.path.exists(self.angle_model_path):
                print(f"夹角检测模型不存在: {self.angle_model_path}")
                success = False
            else:
                # 注意：angle.pt模型通过predict_obb_best_angle函数使用，不需要预加载
                print(f"夹角检测模型路径: {self.angle_model_path}")
        except Exception as e:
            print(f"检查夹角检测模型失败: {e}")
            success = False

        # 加载堆料检测模型
        try:
            if not os.path.exists(self.overflow_model_path):
                print(f"堆料检测模型不存在: {self.overflow_model_path}")
                success = False
            else:
                self.overflow_model = YOLO(self.overflow_model_path)
                print(f"成功加载堆料检测模型: {self.overflow_model_path}")
        except Exception as e:
            print(f"加载堆料检测模型失败: {e}")
            success = False

        # 加载对齐检测模型
        try:
            if not os.path.exists(self.alignment_model_path):
                print(f"对齐检测模型不存在: {self.alignment_model_path}")
                success = False
            else:
                self.alignment_model = YOLO(self.alignment_model_path)
                print(f"成功加载对齐检测模型: {self.alignment_model_path}")
        except Exception as e:
            print(f"加载对齐检测模型失败: {e}")
            success = False

        return success

    def send_relay_command(self, command_hex):
        """发送原始Socket命令"""
        try:
            byte_data = binascii.unhexlify(command_hex)
            with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock:
                sock.connect((self.relay_host, self.relay_port))
                sock.send(byte_data)
                response = sock.recv(1024)
                print(f"收到继电器响应: {binascii.hexlify(response)}")
                return response
        except Exception as e:
            print(f"继电器通信错误: {e}")
            return None

    def get_relay_status(self):
        """获取继电器状态"""
        response = self.send_relay_command(self.read_status_command)
        status_dict = {}

        if response and len(response) >= 10:
            status_byte = response[9]
            status_bin = f"{status_byte:08b}"[::-1]
            for key, bit_index in self.device_bit_map.items():
                status_dict[key] = status_bin[bit_index] == '1'
        else:
            print("读取继电器状态失败")

        return status_dict

    def control_relay(self, device, action):
        """控制继电器"""
        if device in self.relay_commands and action in self.relay_commands[device]:
            print(f"控制继电器 {device} {action}")
            self.send_relay_command(self.relay_commands[device][action])
            time.sleep(0.1)
        else:
            print(f"无效设备或动作: {device}, {action}")

    def read_transmitter_data_via_relay(self, transmitter_id):
        """读取变送器数据（Modbus TCP 转 RS485）"""
        try:
            if transmitter_id not in self.transmitter_config:
                print(f"无效变送器ID: {transmitter_id}")
                return None

            config = self.transmitter_config[transmitter_id]

            if not self.relay_modbus_client.connect():
                print("无法连接网络继电器Modbus服务")
                return None

            result = self.relay_modbus_client.read_holding_registers(
                address=config['weight_register'],
                count=config['register_count'],
                slave=config['slave_id']#转发给哪台变送器
            )

            if isinstance(result, Exception):
                print(f"读取变送器 {transmitter_id} 失败: {result}")
                return None

            # 根据图片示例，正确解析数据
            if config['register_count'] == 2:#读两个寄存器
                # 获取原始字节数组
                raw_data = result.registers
                # 组合成32位整数
                weight = (raw_data[0] << 16) + raw_data[1]
                weight = weight / 1000.0  # 单位转换为千克
            elif config['register_count'] == 1:
                weight = float(result.registers[0])
            else:
                print(f"不支持的寄存器数量: {config['register_count']}")
                return None

            print(f"变送器 {transmitter_id} 读取重量: {weight}kg")
            return weight

        except ModbusException as e:
            print(f"Modbus通信错误: {e}")
            return None
        except Exception as e:
            print(f"数据解析错误: {e}")
            return None
        finally:
            self.relay_modbus_client.close()

    def set_inverter_frequency_via_relay(self, frequency):
        """设置变频器频率"""
        try:
            if not self.relay_modbus_client.connect():
                print("无法连接网络继电器Modbus服务")
                return False

            # 使用最大频率变量计算百分比
            percentage = frequency / self.inverter_max_frequency  # 得到 0~1 的比例
            value = int(percentage * 10000)  # 转换为 -10000 ~ 10000 的整数

            # 限制范围
            value = max(-10000, min(10000, value))

            result = self.relay_modbus_client.write_register(
                self.inverter_config['frequency_register'],
                value,
                slave=self.inverter_config['slave_id']
            )

            if isinstance(result, Exception):
                print(f"设置频率失败: {result}")
                return False

            print(f"设置变频器频率为 {frequency}Hz")
            return True
        except ModbusException as e:
            print(f"变频器Modbus通信错误: {e}")
            return False
        finally:
            self.relay_modbus_client.close()

    def control_inverter_via_relay(self, action):
        try:
            if not self.relay_modbus_client.connect():
                print("无法连接网络继电器Modbus服务")
                return False

            if action == 'start':
                result = self.relay_modbus_client.write_register(
                    address=self.inverter_config['start_register'],
                    value=self.inverter_config['start_command'],
                    slave=self.inverter_config['slave_id']
                )
                print("启动变频器")
            elif action == 'stop':
                result = self.relay_modbus_client.write_register(
                    address=self.inverter_config['start_register'],
                    value=self.inverter_config['stop_command'],
                    slave=self.inverter_config['slave_id']
                )
                print("停止变频器")
            else:
                print(f"无效操作: {action}")
                return False

            if isinstance(result, Exception):
                print(f"控制失败: {result}")
                return False

            return True
        except ModbusException as e:
            print(f"变频器控制错误: {e}")
            return False
        finally:
            self.relay_modbus_client.close()

    def check_upper_material_request(self):
        """检查是否需要要料"""
        current_weight = self.read_transmitter_data_via_relay(1)

        if current_weight is None:
            self.upper_weight_error_count += 1
            print(f"上料斗重量读取失败，错误计数: {self.upper_weight_error_count}")
            if self.upper_weight_error_count >= self.max_error_count:
                print("警告：上料斗传感器连续读取失败，请检查连接")
            return False

        self.upper_weight_error_count = 0
        # 判断是否需要要料：当前重量 < 目标重量 + 缓冲重量
        if current_weight < (self.single_batch_weight + self.min_required_weight):
            print("上料斗重量不足，通知搅拌楼要料")
            self.request_material_from_mixing_building()  # 请求搅拌楼下料
            return True
        return False

    def request_material_from_mixing_building(self):
        """
        请求搅拌楼下料（待完善）
        TODO: 与同事对接通信协议
        """
        print("发送要料请求至搅拌楼...")
        self.return_upper_door_to_default()
        # 这里需要与同事对接具体的通信方式
        # 可能是Modbus写寄存器、TCP通信、HTTP请求等
        pass

    def wait_for_mixing_building_material(self):
        """
        等待搅拌楼下料完成（待完善）
        TODO: 与同事对接信号接收
        """
        print("等待搅拌楼下料完成...")
        # 这里需要与同事对接具体的信号接收方式
        # 可能是Modbus读寄存器、TCP通信、HTTP请求等
        # 模拟等待
        time.sleep(5)
        print("搅拌楼下料完成")
        self.move_upper_door_over_lower()
        return True

    def move_upper_door_over_lower(self):
        """移动上料斗到下料斗上方"""
        print("移动上料斗到下料斗上方")
        self.control_relay(self.DOOR_UPPER, 'open')
        self.upper_door_position = 'over_lower'

    def return_upper_door(self):
        """返回上料斗到搅拌楼"""
        print("上料斗返回搅拌楼")
        self.control_relay(self.DOOR_UPPER, 'close')
        self.upper_door_position = 'returning'

    def return_upper_door_to_default(self):
        """上料斗回到默认位置（搅拌楼下接料位置）"""
        print("上料斗回到默认位置")
        self.control_relay(self.DOOR_UPPER, 'close')
        self.upper_door_position = 'default'

    def start_lower_feeding(self):
        """开始分步下料"""
        if self.lower_feeding_stage != 0:
            print("下料已在进行中")
            return

        # 检查关键设备是否可连接
        if not self._check_device_connectivity():
            print("关键设备连接失败，无法开始下料")
            return
            # 先进行上料斗向下料斗下料

        print("开始分步下料过程")
        self.transfer_material_from_upper_to_lower()
        self.lower_feeding_stage = 4  # 从等待模具车对齐开始
        self.upper_feeding_count = 0
        self.wait_for_vehicle_alignment()

    def _check_device_connectivity(self):
        """检查关键设备连接状态"""
        try:
            # 检查网络继电器连接
            test_response = self.send_relay_command(self.read_status_command)
            if not test_response:
                print("网络继电器连接失败")
                return False

            # 检查变频器连接
            if not self.relay_modbus_client.connect():
                print("无法连接到网络继电器Modbus服务")
                return False

            # 尝试读取变频器一个寄存器（测试连接）
            test_result = self.relay_modbus_client.read_holding_registers(
                address=0x00,
                count=1,
                slave=self.inverter_config['slave_id']
            )

            if isinstance(test_result, Exception):
                print("变频器连接测试失败")
                return False

            # 检查下料斗变送器连接
            test_weight = self.read_transmitter_data_via_relay(2)
            if test_weight is None:
                print("下料斗变送器连接失败")
                return False

            self.relay_modbus_client.close()
            return True
        except Exception as e:
            print(f"设备连接检查失败: {e}")
            return False

    def transfer_material_from_upper_to_lower(self):
        """上料斗向下料斗下料"""
        print(f"上料斗向下料斗下料 (第 {self.upper_feeding_count + 1} 次)")

        # 确保下料斗出砼门关闭
        self.control_relay(self.DOOR_LOWER_2, 'close')
        # 打开上料斗出砼门
        self.control_relay(self.DOOR_LOWER_1, 'open')

        # 等待物料流入下料斗（根据实际情况调整时间）
        time.sleep(3)

        # 关闭上料斗出砼门
        self.control_relay(self.DOOR_LOWER_1, 'close')

        # 增加上料计数
        self.upper_feeding_count += 1
        print("上料斗下料完成")

    def wait_for_vehicle_alignment(self):
        """等待模具车对齐"""
        print("等待模具车对齐...")
        self.lower_feeding_stage = 4

        while self.lower_feeding_stage == 4 and self._running:
            if self.vehicle_aligned:
                print("模具车已对齐，开始下料")
                self.lower_feeding_stage = 1
                self.feeding_stage_one()
                break
            time.sleep(self.alignment_check_interval)


    def feeding_stage_one(self):
        """第一阶段下料：下料斗向模具车下料（低速）"""
        print("开始第一阶段下料：下料斗低速下料")
        self.lower_feeding_cycle = 1
        self.set_inverter_frequency_via_relay(self.frequencies[0])
        self.control_inverter_via_relay('start')

        # 确保上料斗出砼门关闭
        self.control_relay(self.DOOR_LOWER_1, 'close')
        # 打开下料斗出砼门
        self.control_relay(self.DOOR_LOWER_2, 'open')

        start_time = time.time()
        initial_weight = self.read_transmitter_data_via_relay(2)
        if initial_weight is None:
            print("无法获取初始重量，取消下料")
            self.finish_current_batch()
            return

        target_weight = initial_weight + self.single_batch_weight

        while self.lower_feeding_stage == 1:
            current_weight = self.read_transmitter_data_via_relay(2)
            if current_weight is None:
                self.lower_weight_error_count += 1
                if self.lower_weight_error_count >= self.max_error_count:
                    print("下料斗传感器连续读取失败，停止下料")
                    self.finish_current_batch()
                    return
            else:
                self.lower_weight_error_count = 0

            if (current_weight is not None and current_weight >= target_weight) or (time.time() - start_time) > 30:
                self.lower_feeding_stage = 2
                self.feeding_stage_two()
                break
            time.sleep(2)

    def feeding_stage_two(self):
        """第二阶段下料：下料斗向模具车下料（中速）"""
        print("开始第二阶段下料：下料斗中速下料")
        self.lower_feeding_cycle = 2
        self.set_inverter_frequency_via_relay(self.frequencies[1])

        # 保持下料斗出砼门打开
        self.control_relay(self.DOOR_LOWER_2, 'open')
        # 确保上料斗出砼门关闭
        self.control_relay(self.DOOR_LOWER_1, 'close')

        start_time = time.time()
        initial_weight = self.read_transmitter_data_via_relay(2)
        if initial_weight is None:
            print("无法获取初始重量，取消下料")
            self.finish_current_batch()
            return

        target_weight = initial_weight + self.single_batch_weight

        while self.lower_feeding_stage == 2:
            current_weight = self.read_transmitter_data_via_relay(2)
            if current_weight is None:
                self.lower_weight_error_count += 1
                if self.lower_weight_error_count >= self.max_error_count:
                    print("下料斗传感器连续读取失败，停止下料")
                    self.finish_current_batch()
                    return
            else:
                self.lower_weight_error_count = 0

            if (current_weight is not None and current_weight >= target_weight) or (time.time() - start_time) > 30:
                self.lower_feeding_stage = 3
                self.feeding_stage_three()
                break
            time.sleep(2)

    def feeding_stage_three(self):
        """第三阶段下料：下料斗向模具车下料（高速）"""
        print("开始第三阶段下料：下料斗高速下料")
        self.lower_feeding_cycle = 3
        self.set_inverter_frequency_via_relay(self.frequencies[2])

        # 保持下料斗出砼门打开
        self.control_relay(self.DOOR_LOWER_2, 'open')
        # 确保上料斗出砼门关闭
        self.control_relay(self.DOOR_LOWER_1, 'close')

        start_time = time.time()
        initial_weight = self.read_transmitter_data_via_relay(2)
        if initial_weight is None:
            print("无法获取初始重量，取消下料")
            self.finish_current_batch()
            return

        target_weight = initial_weight + self.single_batch_weight

        while self.lower_feeding_stage == 3:
            current_weight = self.read_transmitter_data_via_relay(2)
            if current_weight is None:
                self.lower_weight_error_count += 1
                if self.lower_weight_error_count >= self.max_error_count:
                    print("下料斗传感器连续读取失败，停止下料")
                    self.finish_current_batch()
                    return
            else:
                self.lower_weight_error_count = 0

            if (current_weight is not None and current_weight >= target_weight) or (time.time() - start_time) > 30:
                self.lower_feeding_stage = 4
                self.finish_current_batch()
                break
            time.sleep(2)

    def finish_current_batch(self):
        """完成当前批次下料"""
        print("当前批次下料完成，关闭出砼门")
        self.control_inverter_via_relay('stop')
        self.control_relay(self.DOOR_LOWER_1, 'close')
        self.control_relay(self.DOOR_LOWER_2, 'close')
        self.upper_feeding_count = 0

        # 检查上料斗剩余重量
        upper_weight = self.read_transmitter_data_via_relay(1)
        if upper_weight is not None and upper_weight < (self.single_batch_weight + self.min_required_weight):
            print("上料斗剩余重量不足，需要返回搅拌楼要料")
            self.lower_feeding_stage = 3  # 等待上料
            self.return_upper_door()  # 返回上料斗
            self.request_material_from_mixing_building()  # 请求要料
        else:
            print("上料斗仍有足够物料，等待下一个模具车")
            self.lower_feeding_stage = 4  # 等待模具车对齐
            self.wait_for_vehicle_alignment()

    def finish_feeding_process(self):
        """完成整个下料流程"""
        print("整个下料流程完成")
        self.lower_feeding_stage = 0
        self.upper_feeding_count = 0
        self.return_upper_door_to_default()  # 上料斗回到默认位置

    def handle_overflow_control(self, overflow_detected, door_opening_large):
        """处理溢料控制"""
        if overflow_detected and door_opening_large:
            print("检测到溢料且出砼门开口较大，调小出砼门")
            self.control_relay(self.DOOR_LOWER_1, 'close')
            time.sleep(0.1)
            self.control_relay(self.DOOR_LOWER_1, 'open')
            time.sleep(0.1)

    def is_lower_door_open(self):
        """检查出砼门是否打开"""
        return self.lower_feeding_stage in [1, 2]  # 只有在下料阶段才认为门是打开的

    def check_arch_blocking(self):
        """检查是否需要破拱"""
        current_time = time.time()

        # 检查下料斗破拱（只有在下料过程中才检查）
        if self.lower_feeding_stage in [1, 2]:
            lower_weight = self.read_transmitter_data_via_relay(2)
            if lower_weight is not None:
                # 检查重量变化是否过慢（小于0.1kg变化且时间超过10秒）
                if (abs(lower_weight - self.last_lower_weight) < 0.1) and \
                        (current_time - self.last_weight_time) > 10:
                    print("下料斗可能堵塞，启动破拱")
                    self.control_relay(self.BREAK_ARCH_LOWER, 'open')
                    time.sleep(2)
                    self.control_relay(self.BREAK_ARCH_LOWER, 'close')

                self.last_lower_weight = lower_weight

        # 检查上料斗破拱（只有在上料斗给下料斗下料时才检查）
        if (self.upper_door_position == 'over_lower' and
                self.lower_feeding_stage in [1, 2] and
                self.is_lower_door_open()):
            upper_weight = self.read_transmitter_data_via_relay(1)
            if upper_weight is not None:
                # 检查重量变化是否过慢（小于0.1kg变化且时间超过10秒）
                if (abs(upper_weight - self.last_upper_weight) < 0.1) and \
                        (current_time - self.last_weight_time) > 10:
                    print("上料斗可能堵塞，启动破拱")
                    self.control_relay(self.BREAK_ARCH_UPPER, 'open')
                    time.sleep(2)
                    self.control_relay(self.BREAK_ARCH_UPPER, 'close')

                self.last_upper_weight = upper_weight

        # 更新最后读取时间
        if (self.read_transmitter_data_via_relay(1) is not None or
                self.read_transmitter_data_via_relay(2) is not None):
            self.last_weight_time = current_time

    def monitor_system(self):
        """监控系统状态"""
        while self._running:
            try:
                self.check_upper_material_request()
                self.check_arch_blocking()
                time.sleep(1)
            except Exception as e:
                print(f"监控线程错误: {e}")

    def setup_camera_capture(self, camera_index=0):
        """
        设置摄像头捕获
        :param camera_index: USB摄像头索引或IP摄像头配置
        """
        try:
            rtsp_url = f"rtsp://{self.camera_username}:{self.camera_password}@{self.camera_ip}:{self.camera_port}/streaming/channels/{self.camera_channel}01"
            self.camera = cv2.VideoCapture(rtsp_url)

            if not self.camera.isOpened():
                print(f"无法打开网络摄像头: {rtsp_url}")
                return False
            print(f"网络摄像头初始化成功，地址: {rtsp_url}")
            return True
        except Exception as e:
            print(f"摄像头设置失败: {e}")
            return False

    def capture_current_frame(self):
        """捕获当前帧并返回numpy数组"""
        try:
            if self.camera is None:
                print("摄像头未初始化")
                return None

            ret, frame = self.camera.read()
            if ret:
                return frame
            else:
                print("无法捕获图像帧")
                return None
        except Exception as e:
            print(f"图像捕获失败: {e}")
            return None

    def detect_overflow_from_image(self, image_array):
        """通过图像检测是否溢料（接受numpy数组）"""
        try:
            # 检查模型是否已加载
            if self.overflow_model is None:
                print("堆料检测模型未加载")
                return False

            rois = load_global_rois(self.roi_file_path)

            if not rois:
                print("没有有效的ROI配置")
                return False

            if image_array is None:
                print("输入图像为空")
                return False

            crops = crop_and_resize(image_array, rois, 640)
            for roi_resized, _ in crops:
                final_class, _, _, _ = classify_image_weighted(roi_resized, self.overflow_model, threshold=0.4)
                if "大堆料" in final_class or "小堆料" in final_class:
                    return True

            return False
        except Exception as e:
            print(f"溢料检测失败: {e}")
            return False

    def detect_vehicle_alignment(self, image_array):
        """通过图像检测模具车是否对齐（接受numpy数组）"""
        try:
            # 检查模型是否已加载
            if self.alignment_model is None:
                print("对齐检测模型未加载")
                return False

            if image_array is None:
                print("输入图像为空")
                return False

            # 直接使用模型进行推理
            results = self.alignment_model(image_array)
            pared_probs = results[0].probs.data.cpu().numpy().flatten()

            # 类别0: 未对齐, 类别1: 对齐
            class_id = int(pared_probs.argmax())
            confidence = float(pared_probs[class_id])

            # 只有当对齐且置信度>95%时才认为对齐
            if class_id == 1 and confidence > 0.95:
                return True
            return False
        except Exception as e:
            print(f"对齐检测失败: {e}")
            return False

    def get_current_door_angle(self, image=None, image_path=None):
        """
        通过视觉系统获取当前出砼门角度
        :param image: 图像数组（numpy array）
        :param image_path: 图片路径
        """
        try:
            # 检查模型是否已加载
            if self.angle_model is None:
                print("夹角检测模型未加载")
                return None

            angle_deg, _ = predict_obb_best_angle(
                model=self.angle_model,  # 传递预加载的模型实例
                image=image,  # 传递图像数组
                image_path=image_path  # 或传递图像路径
            )
            return angle_deg
        except Exception as e:
            print(f"角度检测失败: {e}")
            return None

    def alignment_check_loop(self):
        """
        模具车对齐检查循环
        """
        while self._running:
            try:
                # 只在需要检查对齐时才检查
                if self.lower_feeding_stage == 4:
                    current_frame = self.capture_current_frame()
                    if current_frame is not None:
                        self.vehicle_aligned = self.detect_vehicle_alignment(current_frame)
                        if self.vehicle_aligned:
                            print("检测到模具车对齐")
                        else:
                            print("模具车未对齐")
                time.sleep(self.alignment_check_interval)
            except Exception as e:
                print(f"对齐检查循环错误: {e}")
                time.sleep(self.alignment_check_interval)

    def visual_control_loop(self):
        """
        视觉控制主循环
        """

        while self._running and self.visual_control_enabled:
            try:
                current_frame = self.capture_current_frame()
                if current_frame is None:
                    print("无法获取当前图像，跳过本次调整")
                    time.sleep(self.visual_check_interval)
                    continue

                # 检测是否溢料
                overflow = self.detect_overflow_from_image(current_frame)

                # 获取当前角度
                current_angle = self.get_current_door_angle(image=current_frame)

                if current_angle is None:
                    print("无法获取当前角度，跳过本次调整")
                    time.sleep(self.visual_check_interval)
                    continue

                print(f"当前角度: {current_angle:.2f}°, 溢料状态: {overflow}, 控制模式: {self.angle_control_mode}")

                # 状态机控制逻辑
                if self.angle_control_mode == "normal":
                    # 正常模式
                    if overflow and current_angle > self.angle_threshold:
                        # 检测到堆料且角度过大，进入角度减小模式
                        print("检测到堆料且角度过大，关闭出砼门开始减小角度")
                        self.control_relay(self.DOOR_LOWER_2, 'close')
                        self.angle_control_mode = "reducing"
                    else:
                        # 保持正常开门
                        self.control_relay(self.DOOR_LOWER_2, 'open')

                elif self.angle_control_mode == "reducing":
                    # 角度减小模式
                    if current_angle <= self.target_angle + self.angle_tolerance:
                        # 角度已达到目标范围
                        if overflow:
                            # 仍有堆料，进入维持模式
                            print(f"角度已降至{current_angle:.2f}°，仍有堆料，进入维持模式")
                            self.angle_control_mode = "maintaining"
                            self.control_relay(self.DOOR_LOWER_2, 'open')  # 先打开门
                        else:
                            # 无堆料，恢复正常模式
                            print(f"角度已降至{current_angle:.2f}°，无堆料，恢复正常模式")
                            self.control_relay(self.DOOR_LOWER_2, 'open')
                            self.angle_control_mode = "recovery"

                elif self.angle_control_mode == "maintaining":
                    # 维持模式 - 使用脉冲控制
                    if not overflow:
                        # 堆料已消除，恢复正常模式
                        print("堆料已消除，恢复正常模式")
                        self.control_relay(self.DOOR_LOWER_2, 'open')
                        self.angle_control_mode = "recovery"
                    else:
                        # 继续维持角度控制
                        self.pulse_control_door_for_maintaining()

                elif self.angle_control_mode == "recovery":#打开夹爪的过程中又堆料了
                    # 恢复模式 - 逐步打开门
                    if overflow:
                        # 又出现堆料，回到角度减小模式
                        print("恢复过程中又检测到堆料，回到角度减小模式")
                        self.angle_control_mode = "maintaining"
                    else:
                        # 堆料已消除，恢复正常模式
                        print("堆料已消除，恢复正常模式")
                        self.control_relay(self.DOOR_LOWER_2, 'open')
                        self.angle_control_mode = "normal"

                self.last_angle = current_angle
                time.sleep(self.visual_check_interval)

            except Exception as e:
                print(f"视觉控制循环错误: {e}")
                time.sleep(self.visual_check_interval)

    def pulse_control_door_for_maintaining(self):
        """
        用于维持模式的脉冲控制
        保持角度在目标范围内
        """
        print("执行维持脉冲控制")
        # 关门1秒
        self.control_relay(self.DOOR_LOWER_2, 'close')
        time.sleep(1.0)
        # 开门1秒
        self.control_relay(self.DOOR_LOWER_2, 'open')
        time.sleep(1.0)

    def start_visual_control(self):
        """
        启动视觉控制线程
        """
        if not self.visual_control_enabled:
            print("视觉控制未启用")
            return

        print("启动视觉控制线程")
        self._visual_control_thread = threading.Thread(
            target=self.visual_control_loop,
            daemon=True
        )
        self._visual_control_thread.start()
        return self._visual_control_thread

    def start_alignment_check(self):
        """
        启动模具车对齐检查线程
        """
        print("启动模具车对齐检查线程")
        self._alignment_check_thread = threading.Thread(
            target=self.alignment_check_loop,
            daemon=True
        )
        self._alignment_check_thread.start()
        return self._alignment_check_thread

    def start(self):
        """启动系统"""
        if self._running:
            print("系统已在运行")
            return
        print("启动控制系统")
        self._running = True
        self._monitor_thread = threading.Thread(target=self.monitor_system, daemon=True)
        self._monitor_thread.start()

    def stop(self):
        """停止系统"""
        if not self._running:
            print("系统未在运行")
            return
        print("停止控制系统")
        self._running = False
        if self._monitor_thread is not None:
            self._monitor_thread.join()
        if self._visual_control_thread is not None:
            self._visual_control_thread.join()
        if self._alignment_check_thread is not None:
            self._alignment_check_thread.join()
        if self.camera is not None:
            self.camera.release()
        print("控制系统已停止")


# 使用示例
if __name__ == "__main__":
    system = FeedingControlSystem(relay_host='192.168.0.18', relay_port=50000)

    # 设置角度控制参数
    system.set_angle_parameters(
        target_angle=20.0,
        min_angle=10.0,
        max_angle=80.0,
        threshold=60.0
    )

    # 设置下料参数
    system.set_feeding_parameters(
        target_vehicle_weight=5000,  # 5吨
        upper_buffer_weight=500,  # 0.5吨缓冲
        single_batch_weight=2500  # 每次下2.5吨
    )

    # 设置摄像头配置
    system.set_camera_config(
        camera_type="ip",
        ip="192.168.1.51",
        port=554,
        username="admin",
        password="XJ123456",
        channel=1
    )

    # 初始化摄像头
    if not system.setup_camera_capture():
        print("摄像头初始化失败")
        exit(1)

    # 加载所有模型
    if not system.load_all_models():
        print("模型加载失败")
        exit(1)

    # 启动系统监控
    system.start()

    # 启动视觉控制
    system.start_visual_control()

    # 启动对齐检查
    system.start_alignment_check()

    print("系统准备就绪，5秒后开始下料...")
    time.sleep(5)
    system.start_lower_feeding()  # 启动下料流程

    try:
        while True:
            time.sleep(1)
    except KeyboardInterrupt:
        print("收到停止信号")
    except Exception as e:
        print(f"系统错误: {e}")
    finally:
        system.stop()