wire_controlsystem/conveyor_controller/conveyor_master_controller1.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
'''
# @Time    : 2025/12/22
# @Author  : reenrr
# @File    : conveyor_master_controller.py
# @Desc    : 传送带1和2协同控制  使用一个串口两个轴地址（485总线）  每隔4秒两个传送带同步走一个挡板的距离
'''
import logging
import threading
import time
from datetime import datetime
import serial
from EMV import RelayController

# 彻底屏蔽pymodbus所有日志
logging.getLogger("pymodbus").setLevel(logging.CRITICAL)
logging.basicConfig(level=logging.INFO)

# --- 全局参数配置 ---
SERIAL_PORT = '/dev/ttyUSB0'  # 单串口（485总线）
BAUD_RATE = 115200
ACTION_DELAY = 5
SLAVE_ID_1 = 1  # 传送带1轴地址
SLAVE_ID_2 = 2  # 传送带2轴地址
SERIAL_TIMEOUT = 0.05  # 串口超时50ms
SYNC_STOP_DURATION = 4.0  # 同步停止时间（4秒）
SENSOR_DEBOUNCE_TIME = 0.2  # 传感器防抖时间（200ms）
DETECTION_INTERVAL = 0.05  # 传感器检测间隔（50ms）
RELEASE_WAIT_TIMEOUT = 5.0  # 等待挡板离开超时（5秒）

# 全局串口锁（485总线必须单指令发送，避免冲突）
GLOBAL_SERIAL_LOCK = threading.Lock()


class SingleMotorController:
    """单个电机控制器（按轴地址自动匹配指令集，复用全局串口）"""

    def __init__(self, slave_id, conveyor_id, action_delay, serial_obj, global_serial_lock):
        self.slave_id = slave_id  # 轴地址（1/2）
        self.conveyor_id = conveyor_id  # 传送带编号（1/2）
        self.action_delay = action_delay
        self.ser = serial_obj  # 复用主控制器的串口实例
        self.global_serial_lock = global_serial_lock  # 全局串口锁

        # 初始化指令
        self._init_commands()

        # 核心状态标志
        self.status_thread_is_running = False
        self.conveyor_thread_is_running = False
        self.sensor_triggered = False  # 传感器触发标志
        self.sensor_locked = False  # 传感器锁定（防抖）
        self.stop_flag = False  # 本地停止标志
        self.wait_sensor_release = False  # 重启后等待挡板离开标志
        self.last_sensor_trigger = 0  # 上次触发时间（防抖）

        # 线程对象
        self.monitor_thread = None
        self.run_speed_thread = None
        self.relay_controller = RelayController()

        # 锁
        self.sensor_lock = threading.Lock()
        self.state_lock = threading.Lock()

    def _init_commands(self):
        """根据轴地址初始化指令集"""
        if self.slave_id == 1:
            # --------传送带1（轴地址1）指令集--------
            self.start_command = bytes([0x01, 0x06, 0x60, 0x02, 0x00, 0x10, 0x37, 0xC6])  # 启动指令
            self.stop_command = bytes([0x01, 0x06, 0x60, 0x02, 0x00, 0x40, 0x37, 0xFA])  # 停止指令
            self.speed_commands = [
                bytes([0x01, 0x06, 0x62, 0x00, 0x00, 0x02, 0x17, 0xB3]),  # 设定PR0为速度模式
                bytes([0x01, 0x06, 0x62, 0x03, 0xFF, 0xE2, 0xA7, 0xCB]),  # 设定PR0速度  -30
                bytes([0x01, 0x06, 0x62, 0x04, 0x00, 0x32, 0x56, 0x66]),  # 设定PR0加速度
                bytes([0x01, 0x06, 0x62, 0x05, 0x00, 0x32, 0x07, 0xA6]),  # 设定PR0减速度
            ]
        elif self.slave_id == 2:
            # --------传送带2（轴地址2）指令集--------
            self.start_command = bytes([0x02, 0x06, 0x60, 0x02, 0x00, 0x10, 0x37, 0xF5])  # 启动指令
            self.stop_command = bytes([0x02, 0x06, 0x60, 0x02, 0x00, 0x40, 0x37, 0xC9])  # 停止指令
            self.speed_commands = [
                bytes([0x02, 0x06, 0x62, 0x00, 0x00, 0x02, 0x17, 0x80]),  # 设定PR0为速度模式
                bytes([0x02, 0x06, 0x62, 0x03, 0xFF, 0xE2, 0xA7, 0xF8]),  # 设定PR0速度  -30
                bytes([0x02, 0x06, 0x62, 0x04, 0x00, 0x32, 0x56, 0x55]),  # 设定PR0加速度
                bytes([0x02, 0x06, 0x62, 0x05, 0x00, 0x32, 0x07, 0x95]),  # 设定PR0减速度
            ]
        else:
            raise ValueError(f"不支持的轴地址：{self.slave_id}，仅支持1/2")

        # 打印指令（调试用）
        logging.info(f"[传送带{self.conveyor_id}] 加载轴地址{self.slave_id}指令集：")
        logging.info(f"  启动指令: {self.start_command.hex(' ')}")
        logging.info(f"  停止指令: {self.stop_command.hex(' ')}")

    def send_command_list(self, command_list, delay=0.05):
        """批量发送指令列表（加全局锁，指令间延时避免总线冲突）"""
        if not (self.ser and self.ser.is_open):
            logging.info(f"传送带{self.conveyor_id}串口未打开，跳过指令列表发送")
            return
        for idx, cmd in enumerate(command_list):
            self.send_and_receive_raw(cmd, f"指令{idx + 1}")
            time.sleep(delay)  # 485总线指令间必须加延时

    def clear_buffer(self):
        """清空串口缓冲区（加全局锁）"""
        if self.ser and self.ser.is_open:
            with self.global_serial_lock:
                self.ser.reset_input_buffer()
                self.ser.reset_output_buffer()
                while self.ser.in_waiting > 0:
                    self.ser.read(self.ser.in_waiting)
                    time.sleep(0.001)

    def send_and_receive_raw(self, command, description):
        """底层串口通信（核心：使用全局锁保证单指令发送）"""
        if not (self.ser and self.ser.is_open):
            logging.info(f"传送带{self.conveyor_id}串口未打开，跳过发送")
            return None

        try:
            self.clear_buffer()
            # 全局锁：同一时间仅一个设备发送指令
            with self.global_serial_lock:
                send_start = time.perf_counter()
                self.ser.write(command)
                self.ser.flush()
                send_cost = (time.perf_counter() - send_start) * 1000

                # 接收响应（仅对应轴地址的设备会回复）
                recv_start = time.perf_counter()
                response = b""
                while (time.perf_counter() - recv_start) < SERIAL_TIMEOUT:
                    if self.ser.in_waiting > 0:
                        chunk = self.ser.read(8)
                        response += chunk
                        if len(response) >= 8:
                            break
                    time.sleep(0.001)
                recv_cost = (time.perf_counter() - recv_start) * 1000

            # 处理响应
            valid_resp = response[:8] if len(response) >= 8 else response
            logging.info(f"[传送带{self.conveyor_id}] [{datetime.now().strftime('%H:%M:%S.%f')[:-3]}]")
            logging.info(f"  发送 {description}: {command.hex(' ')} (耗时: {send_cost:.2f}ms)")
            logging.info(f"  接收响应: {valid_resp.hex(' ')} (长度: {len(valid_resp)}, 耗时: {recv_cost:.2f}ms)")

            return valid_resp

        except Exception as e:
            logging.info(f"传送带{self.conveyor_id}通信异常 ({description}): {e}")
            return None

    def emergency_stop(self):
        """紧急停止（调用对应轴地址的停止指令）"""
        with self.state_lock:
            if self.stop_flag:
                return
            self.stop_flag = True

        self.send_and_receive_raw(self.stop_command, "停止寄存器")
        logging.info(f"[传送带{self.conveyor_id}] 电机（轴地址{self.slave_id}）已紧急停止")

    def resume_motor(self):
        """恢复电机运行（调用对应轴地址的启动指令）"""
        with self.state_lock:
            self.stop_flag = False
            self.sensor_triggered = False
            self.sensor_locked = False
            self.last_sensor_trigger = 0
            self.wait_sensor_release = True  # 标记需要等待挡板离开

        # 主动发送运行指令
        self.send_and_receive_raw(self.start_command, "重启寄存器")
        logging.info(f"[传送带{self.conveyor_id}] 电机（轴地址{self.slave_id}）已重启，等待挡板离开传感器...")

    def wait_for_sensor_release(self):
        """等待挡板离开传感器"""
        start_time = time.time()
        logging.info(f"[传送带{self.conveyor_id}] 开始等待挡板离开（超时{RELEASE_WAIT_TIMEOUT}秒）")

        while time.time() - start_time < RELEASE_WAIT_TIMEOUT:
            # 读取传感器状态（True=无遮挡，False=有遮挡）
            if self.conveyor_id == 1:
                sensor_status = self.relay_controller.get_device_status('conveyor1_sensor', 'sensors')
            else:
                sensor_status = self.relay_controller.get_device_status('conveyor2_sensor', 'sensors')

            # 传感器恢复为无遮挡（True），说明挡板已离开
            if sensor_status:
                logging.info(f"[传送带{self.conveyor_id}] 挡板已离开传感器，恢复正常检测")
                with self.state_lock:
                    self.wait_sensor_release = False
                return True

            time.sleep(DETECTION_INTERVAL)

        # 超时处理
        logging.info(f"[传送带{self.conveyor_id}] 等待挡板离开超时！强制恢复检测")
        with self.state_lock:
            self.wait_sensor_release = False
        return False

    def monitor_conveyors_sensor_status(self, master_controller):
        """传感器检测线程"""
        logging.info(f"[传送带{self.conveyor_id}] 传感器检测线程已启动（检测间隔：{DETECTION_INTERVAL}s）")

        while self.status_thread_is_running and not master_controller.global_stop_flag:
            try:
                with self.sensor_lock:
                    # 1. 全局停止/电机停止时跳过检测
                    if master_controller.global_stop_flag or self.stop_flag:
                        time.sleep(DETECTION_INTERVAL)
                        continue

                    # 2. 重启后先等待挡板离开
                    if self.wait_sensor_release:
                        self.wait_for_sensor_release()
                        continue

                    # 3. 读取传感器状态
                    if self.conveyor_id == 1:
                        sensor_status = self.relay_controller.get_device_status('conveyor1_sensor', 'sensors')
                    else:
                        sensor_status = self.relay_controller.get_device_status('conveyor2_sensor', 'sensors')

                    current_time = time.time()

                    # 4. 检测到挡板且满足防抖条件
                    if sensor_status and (not self.sensor_locked) and \
                            (current_time - self.last_sensor_trigger) > SENSOR_DEBOUNCE_TIME:
                        logging.info(f"\n[传送带{self.conveyor_id}] [{datetime.now().strftime('%H:%M:%S')}] 检测到挡板！立即响应")
                        self.last_sensor_trigger = current_time

                        with self.state_lock:
                            self.sensor_triggered = True
                            self.sensor_locked = True

                        # 立即通知主控制器
                        threading.Thread(
                            target=master_controller.on_sensor_triggered,
                            args=(self.conveyor_id,),
                            daemon=True
                        ).start()

                time.sleep(DETECTION_INTERVAL)

            except Exception as e:
                logging.info(f"[传送带{self.conveyor_id}] 传感器检测异常: {e}")
                time.sleep(0.1)

        logging.info(f"[传送带{self.conveyor_id}] 传感器检测线程已停止")

    def run_speed_mode(self, master_controller):
        """电机速度模式线程"""
        logging.info(f"[传送带{self.conveyor_id}] 电机速度模式线程已启动（轴地址{self.slave_id}）")
        self.conveyor_thread_is_running = True

        while self.conveyor_thread_is_running and not master_controller.global_stop_flag:
            try:
                # 串口未打开则退出循环（由主控制器管理串口连接）
                if not (self.ser and self.ser.is_open):
                    logging.info(f"[传送带{self.conveyor_id}] 串口已关闭，停止发送运行指令")
                    time.sleep(1)
                    continue

                # 仅当未停止时发送运行指令
                if not self.stop_flag:
                    self.send_and_receive_raw(self.start_command, "持续运行指令")

                # 启动传感器线程（仅一次）
                if not self.status_thread_is_running and (
                        self.monitor_thread is None or not self.monitor_thread.is_alive()):
                    self.status_thread_is_running = True
                    self.monitor_thread = threading.Thread(
                        target=self.monitor_conveyors_sensor_status,
                        args=(master_controller,),
                        daemon=True
                    )
                    self.monitor_thread.start()

                time.sleep(0.5)

            except Exception as e:
                logging.info(f"[传送带{self.conveyor_id}] 电机运行异常: {e}")
                time.sleep(1)

        self.emergency_stop()
        self.conveyor_thread_is_running = False
        logging.info(f"[传送带{self.conveyor_id}] 电机速度模式线程已停止")

    def start_run_speed_thread(self, master_controller):
        """启动电机线程"""
        if self.run_speed_thread and self.run_speed_thread.is_alive():
            with self.state_lock:
                self.conveyor_thread_is_running = False
            self.run_speed_thread.join(timeout=2)

        with self.state_lock:
            self.stop_flag = False
            self.sensor_triggered = False
            self.sensor_locked = False
            self.wait_sensor_release = True
            self.last_sensor_trigger = 0

        self.run_speed_thread = threading.Thread(
            target=self.run_speed_mode,
            args=(master_controller,),
            daemon=True
        )
        self.run_speed_thread.start()


class MasterConveyorController:
    """主控制器 - 单串口管理两个传送带（485总线）"""

    def __init__(self):
        self.global_stop_flag = False
        self.sync_running = False

        # 1. 初始化单串口（485总线）
        self.ser = None
        self._init_serial()

        # 2. 初始化两个电机控制器（复用同一个串口）
        self.conveyor1 = SingleMotorController(
            slave_id=SLAVE_ID_1,
            conveyor_id=1,
            action_delay=ACTION_DELAY,
            serial_obj=self.ser,
            global_serial_lock=GLOBAL_SERIAL_LOCK
        )
        self.conveyor2 = SingleMotorController(
            slave_id=SLAVE_ID_2,
            conveyor_id=2,
            action_delay=ACTION_DELAY,
            serial_obj=self.ser,
            global_serial_lock=GLOBAL_SERIAL_LOCK
        )

        # 同步锁
        self.sync_lock = threading.Lock()
        self.sync_condition = threading.Condition(self.sync_lock)

    def _init_serial(self):
        """初始化485总线串口（主控制器统一管理）"""
        try:
            self.ser = serial.Serial(
                port=SERIAL_PORT,
                baudrate=BAUD_RATE,
                bytesize=serial.EIGHTBITS,
                parity=serial.PARITY_NONE,
                stopbits=serial.STOPBITS_ONE,
                timeout=SERIAL_TIMEOUT,
                write_timeout=SERIAL_TIMEOUT,
                xonxoff=False,
                rtscts=False,
                dsrdtr=False
            )
            if self.ser.is_open:
                logging.info(f"成功初始化485总线串口 {SERIAL_PORT}（波特率{BAUD_RATE}）")
                # 初始化时清空缓冲区
                with GLOBAL_SERIAL_LOCK:
                    self.ser.reset_input_buffer()
                    self.ser.reset_output_buffer()
            else:
                raise RuntimeError("串口初始化失败：无法打开串口")
        except Exception as e:
            raise RuntimeError(f"485串口初始化失败: {e}")

    def on_sensor_triggered(self, conveyor_id):
        """传感器触发回调"""
        with self.sync_condition:
            if self.sync_running:
                return

            if conveyor_id == 1:
                self.conveyor1.sensor_triggered = True
                logging.info(f"\n[主控制器] 传送带1检测到挡板，等待传送带2...")
            else:
                self.conveyor2.sensor_triggered = True
                logging.info(f"\n[主控制器] 传送带2检测到挡板，等待传送带1...")

            # 立即停止当前触发的传送带
            if conveyor_id == 1:
                self.conveyor1.emergency_stop()
            else:
                self.conveyor2.emergency_stop()

            # 两个都触发后同步停止
            if self.conveyor1.sensor_triggered and self.conveyor2.sensor_triggered:
                self.sync_running = True
                logging.info(f"\n[主控制器] 两个传送带都检测到挡板！开始同步停止 {SYNC_STOP_DURATION} 秒")

                self.conveyor1.emergency_stop()
                self.conveyor2.emergency_stop()

                # 同步停止+重启逻辑
                def sync_stop_and_resume():
                    try:
                        time.sleep(SYNC_STOP_DURATION)

                        with self.sync_condition:
                            logging.info(f"\n[主控制器] 同步停止结束，重启所有传送带")
                            # 重启两个电机
                            self.conveyor1.resume_motor()
                            self.conveyor2.resume_motor()

                            # 重置所有状态
                            self.conveyor1.sensor_triggered = False
                            self.conveyor2.sensor_triggered = False
                            self.conveyor1.sensor_locked = False
                            self.conveyor2.sensor_locked = False
                            self.conveyor1.last_sensor_trigger = 0
                            self.conveyor2.last_sensor_trigger = 0

                            # 强制重启电机线程
                            self.conveyor1.start_run_speed_thread(self)
                            self.conveyor2.start_run_speed_thread(self)

                            self.sync_running = False
                            self.sync_condition.notify_all()
                    except Exception as e:
                        logging.info(f"[主控制器] 同步恢复异常: {e}")
                        self.sync_running = False

                threading.Thread(target=sync_stop_and_resume, daemon=True).start()

    def start_all_conveyors(self):
        """启动所有传送带"""
        logging.info("\n=== 主控制器：启动所有传送带（485总线）===")

        # 检查串口是否正常
        if not (self.ser and self.ser.is_open):
            logging.info("[主控制器] 485串口未打开，无法启动")
            return False

        # 发送速度模式指令（按轴地址发送）
        self.conveyor1.send_command_list(self.conveyor1.speed_commands[:4])
        self.conveyor2.send_command_list(self.conveyor2.speed_commands[:4])

        # 启动电机线程
        self.conveyor1.start_run_speed_thread(self)
        self.conveyor2.start_run_speed_thread(self)

        logging.info("[主控制器] 所有传送带已启动，等待挡板离开后开始检测...")
        return True

    def stop_all_conveyors(self):
        """停止所有传送带并关闭串口"""
        logging.info("\n=== 主控制器：停止所有传送带 ===")
        self.global_stop_flag = True

        with self.sync_condition:
            self.sync_running = False
            self.conveyor1.emergency_stop()
            self.conveyor2.emergency_stop()

        # 关闭串口
        time.sleep(1)
        with GLOBAL_SERIAL_LOCK:
            if self.ser and self.ser.is_open:
                self.ser.close()
                logging.info(f"485总线串口 {SERIAL_PORT} 已关闭")

        logging.info("[主控制器] 所有传送带已停止并关闭串口")

    def run(self):
        """主运行函数"""
        try:
            if not self.start_all_conveyors():
                return

            # 主线程保持运行
            while not self.global_stop_flag:
                time.sleep(0.5)

        except KeyboardInterrupt:
            logging.info("\n\n[主控制器] 检测到退出指令，正在停止系统...")
        except Exception as e:
            logging.info(f"\n[主控制器] 程序异常: {e}")
        finally:
            self.stop_all_conveyors()
            logging.info("\n=== 主控制器：程序执行完毕 ===")


def main():
    """主函数"""
    try:
        master = MasterConveyorController()
        master.run()
    except RuntimeError as e:
        logging.info(f"系统启动失败: {e}")
    except Exception as e:
        logging.info(f"未知异常: {e}")


if __name__ == '__main__':
    main()