线条厂各设备控制代码V1.0

conveyor_controller/conveyor_master_controller2.py

@@ -0,0 +1,381 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+'''
+# @Time    : 2026/1/6 09:41
+# @Author  : reenrr
+# @File    : conveyor_master_controller2.py
+# @Desc    : 传送带1和2协同控制  两个传送带同步走一个挡板的距离
+'''
+import logging
+import threading
+import time
+from datetime import datetime
+import serial
+from EMV import RelayController
+
+logging.getLogger("pymodbus").setLevel(logging.CRITICAL)
+
+# --- 全局参数配置 ---
+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
+SENSOR_DEBOUNCE_TIME = 0.2  # 传感器防抖时间（200ms）
+DETECTION_INTERVAL = 0.05  # 传感器检测间隔（50ms）
+
+# 全局串口锁（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.is_running = False  # 电机是否已启动（一次性）
+        self.is_stopped = False  # 电机是否已停止（触发传感器后）
+        self.sensor_triggered = False  # 传感器触发标志
+        self.sensor_locked = False  # 传感器锁定（防抖）
+        self.last_sensor_trigger = 0  # 上次触发时间（防抖）
+
+        # 线程对象
+        self.monitor_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")
+
+        # 打印指令（调试用）
+        print(f"[传送带{self.conveyor_id}] 加载轴地址{self.slave_id}指令集：")
+        print(f"  启动指令: {self.start_command.hex(' ')}")
+        print(f"  停止指令: {self.stop_command.hex(' ')}")
+
+    def send_command_list(self, command_list, delay=0.05):
+        """
+        批量发送指令列表（加全局锁，指令间延时避免总线冲突）
+        :param command_list: 指令列表
+        :param delay: 指令间延时（默认0.05s）
+        """
+        if not (self.ser and self.ser.is_open):
+            print(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):
+        """
+        底层串口通信（核心：使用全局锁保证单指令发送）
+        :param command: 指令
+        :param description: 指令描述
+        :return: 返回响应
+        """
+        if not (self.ser and self.ser.is_open):
+            print(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
+            print(f"[传送带{self.conveyor_id}] [{datetime.now().strftime('%H:%M:%S.%f')[:-3]}]")
+            print(f"  发送 {description}: {command.hex(' ')} (耗时: {send_cost:.2f}ms)")
+            print(f"  接收响应: {valid_resp.hex(' ')} (长度: {len(valid_resp)}, 耗时: {recv_cost:.2f}ms)")
+
+            return valid_resp
+
+        except Exception as e:
+            print(f"传送带{self.conveyor_id}通信异常 ({description}): {e}")
+            return None
+
+    def start_motor_once(self):
+        """一次性启动电机"""
+        with self.state_lock:
+            if self.is_running or self.is_stopped:
+                print(f"[传送带{self.conveyor_id}] 电机已启动/停止，无需重复启动")
+                return
+
+        # 1. 发送速度模式配置指令
+        print(f"[传送带{self.conveyor_id}] 配置速度模式...")
+        self.send_command_list(self.speed_commands[:4])
+
+        # 2. 发送启动指令
+        print(f"[传送带{self.conveyor_id}] 发送启动指令")
+        self.send_and_receive_raw(self.start_command, "启动传送带")
+
+        with self.state_lock:
+            self.is_running = True
+
+    def stop_motor(self):
+        """停止电机"""
+        with self.state_lock:
+            if self.is_stopped:
+                return
+            self.is_stopped = True
+            self.is_running = False
+
+        # 发送停止指令
+        print(f"[传送带{self.conveyor_id}] 发送停止指令")
+        self.send_and_receive_raw(self.stop_command, "停止传送带")
+
+    def monitor_conveyors_sensor_status(self, master_controller):
+        """传感器检测线程（仅检测，触发后停止电机）"""
+        print(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.is_stopped:
+                        time.sleep(DETECTION_INTERVAL)
+                        continue
+
+                    # 2. 读取传感器状态
+                    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()
+
+                    # 3. 检测到挡板且满足防抖条件
+                    if (not sensor_status) and (not self.sensor_locked) and \
+                            (current_time - self.last_sensor_trigger) > SENSOR_DEBOUNCE_TIME:
+                        print(
+                            f"\n[传送带{self.conveyor_id}] [{datetime.now().strftime('%H:%M:%S')}] 检测到挡板！立即停止")
+                        self.last_sensor_trigger = current_time
+                        self.sensor_triggered = True
+                        self.sensor_locked = True
+
+                        # 立即停止电机
+                        self.stop_motor()
+
+                        # 通知主控制器（可选：用于同步两个传送带停止）
+                        threading.Thread(
+                            target=master_controller.on_sensor_triggered,
+                            args=(self.conveyor_id,),
+                            daemon=True
+                        ).start()
+
+                time.sleep(DETECTION_INTERVAL)
+
+            except Exception as e:
+                print(f"[传送带{self.conveyor_id}] 传感器检测异常: {e}")
+                time.sleep(0.1)
+
+        print(f"[传送带{self.conveyor_id}] 传感器检测线程已停止")
+
+    def start_sensor_thread(self, master_controller):
+        """启动传感器检测线程"""
+        if self.monitor_thread and self.monitor_thread.is_alive():
+            return
+
+        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()
+
+class MasterConveyorController:
+    """主控制器 - 单串口管理两个传送带（485总线）"""
+    def __init__(self):
+        self.global_stop_flag = False
+        self.both_stopped = 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()
+
+    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:
+                print(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):
+        """传感器触发回调（同步两个传送带停止）"""
+        with self.sync_lock:
+            # 检查是否两个传送带都已触发
+            if self.conveyor1.sensor_triggered and self.conveyor2.sensor_triggered:
+                self.both_stopped = True
+                print(f"\n[主控制器] 两个传送带都已检测到挡板并停止！任务完成")
+                self.global_stop_flag = True  # 标记全局停止
+
+    def start_all_conveyors(self):
+        """启动所有传送带"""
+        print("\n=== 主控制器：启动所有传送带===")
+
+        # 检查串口是否正常
+        if not (self.ser and self.ser.is_open):
+            print("[主控制器] 485串口未打开，无法启动")
+            return False
+        # 1. 一次性启动两个电机
+        self.conveyor1.start_motor_once()
+        self.conveyor2.start_motor_once()
+
+        # 2. 启动传感器检测线程
+        self.conveyor1.start_sensor_thread(self)
+        self.conveyor2.start_sensor_thread(self)
+
+        print("[主控制器] 所有传送带已一次性启动，传感器开始检测...")
+        return True
+
+    def stop_all_conveyors(self):
+        """停止所有传送带并关闭串口"""
+        print("\n=== 主控制器：停止所有传送带 ===")
+        self.global_stop_flag = True
+
+        # 强制停止两个电机
+        self.conveyor1.stop_motor()
+        self.conveyor2.stop_motor()
+
+        # 关闭串口
+        time.sleep(1)
+        with GLOBAL_SERIAL_LOCK:
+            if self.ser and self.ser.is_open:
+                self.ser.close()
+                print(f"485总线串口 {SERIAL_PORT} 已关闭")
+
+        print("[主控制器] 所有传送带已停止并关闭串口")
+
+    def run(self):
+        """主运行函数"""
+        try:
+            if not self.start_all_conveyors():
+                return
+
+            # 主线程等待：直到两个传送带都停止或手动退出
+            while not self.global_stop_flag:
+                if self.both_stopped:
+                    break
+                time.sleep(0.5)
+
+        except KeyboardInterrupt:
+            print("\n\n[主控制器] 检测到退出指令，正在停止系统...")
+        except Exception as e:
+            print(f"\n[主控制器] 程序异常: {e}")
+        finally:
+            self.stop_all_conveyors()
+            print("\n=== 主控制器：程序执行完毕 ===")
+
+# -----------传送带对外接口--------------
+def conveyor_control():
+    """主函数"""
+    try:
+        master = MasterConveyorController()
+        master.run()
+    except RuntimeError as e:
+        print(f"系统启动失败: {e}")
+    except Exception as e:
+        print(f"未知异常: {e}")
+
+
+# ------------测试接口--------------
+if __name__ == '__main__':
+    conveyor_control()