432 lines
18 KiB
Python
432 lines
18 KiB
Python
#!/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.EMV_test 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()
|
||
|
||
|