#!/usr/bin/env python # -*- coding: utf-8 -*- ''' # @Time : 2026/1/6 10:41 # @Author : reenrr # @File : conveyor_master_controller2_test.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） WAIT_INIT_RELEASE_TIMEOUT = 3.0 # 等待初始挡板离开超时（10秒） # 全局串口锁（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.init_release_done = False # 初始挡板是否已离开 # 线程对象 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(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 wait_init_sensor_release(self): """等待初始挡板离开传感器（传感器从无信号→有信号）""" print(f"[传送带{self.conveyor_id}] 等待初始挡板离开传感器（超时{WAIT_INIT_RELEASE_TIMEOUT}秒）") start_time = time.time() while time.time() - start_time < WAIT_INIT_RELEASE_TIMEOUT: # 读取传感器状态：True=有信号（无遮挡），False=无信号（遮挡） sensor_status = self.get_sensor_status() if sensor_status: # 挡板离开，传感器有信号 print(f"[传送带{self.conveyor_id}] 初始挡板已离开传感器") with self.state_lock: self.init_release_done = True return True time.sleep(DETECTION_INTERVAL) # 超时处理 print(f"[传送带{self.conveyor_id}] 等待初始挡板离开超时！强制标记为已离开") with self.state_lock: self.init_release_done = True return False def get_sensor_status(self): """读取传感器状态""" if self.conveyor_id == 1: return self.relay_controller.get_device_status('conveyor1_sensor', 'sensors') else: return self.relay_controller.get_device_status('conveyor2_sensor', 'sensors') def monitor_conveyors_sensor_status(self, master_controller): """ 传感器检测线程（仅检测运行中挡板遮挡） :param 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 not self.init_release_done: time.sleep(DETECTION_INTERVAL) continue # 3. 读取传感器状态 sensor_status = self.get_sensor_status() current_time = time.time() # 4. 检测到挡板遮挡（无信号）且满足防抖条件 → 触发停止 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() # 通知主控制器同步状态 master_controller.on_sensor_triggered() 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): """ 启动传感器检测线程 :param 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: # 停止未停止的传送带 if not self.conveyor1.is_stopped: self.conveyor1.stop_motor() if not self.conveyor2.is_stopped: self.conveyor2.stop_motor() 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. 第一步：同步启动两个电机 print("[主控制器] 同步启动两个传送带（初始挡板遮挡传感器）") self.conveyor1.start_motor() self.conveyor2.start_motor() # 2. 第二步：等待两个传送带的初始挡板都离开传感器 print("[主控制器] 等待两个传送带的初始挡板离开传感器...") self.conveyor1.wait_init_sensor_release() self.conveyor2.wait_init_sensor_release() # 3. 第三步：初始挡板都离开后，启动传感器检测线程 print("[主控制器] 初始挡板已全部离开，启动传感器检测线程") 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()