#!/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()