pengqi/wire_controlsystem
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
pengqi
2026-01-06 16:01:15 +08:00
parent a6e9d0d734
commit cc4bdbb965
14 changed files with 2117 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

431
conveyor_controller/conveyor_master_controller2_test.py Normal file
View File

@ -0,0 +1,431 @@
#!/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()