first commit
This commit is contained in:
rangang
@@ -0,0 +1,7 @@
|
||||
import os
|
||||
# 示例 3: 末尾路径为空
|
||||
path = os.path.join("/home/user", "documents", "")
|
||||
print(path) # 输出: /home/user/documents/
|
||||
|
||||
path = os.path.join("/home/user", "documents")
|
||||
print(path) # 输出: /home/user/documents
|
||||
+824
@@ -0,0 +1,824 @@
|
||||
import time
|
||||
import os
|
||||
import sys
|
||||
from pymodbus.client import ModbusTcpClient, ModbusSerialClient
|
||||
from pymodbus.exceptions import ModbusException
|
||||
from pymodbus.payload import BinaryPayloadDecoder, BinaryPayloadBuilder
|
||||
from pymodbus.constants import Endian
|
||||
|
||||
|
||||
def _motor_log(msg: str):
|
||||
"""电机操作日志，直接写文件 + 刷盘"""
|
||||
try:
|
||||
log_dir = os.path.join(os.path.dirname(sys.executable), "logs")
|
||||
os.makedirs(log_dir, exist_ok=True)
|
||||
log_file = os.path.join(log_dir, "control_debug.log")
|
||||
with open(log_file, "a", encoding="utf-8") as f:
|
||||
f.write(f"[{time.strftime('%H:%M:%S.%f')[:-3]}] [MOTOR] {msg}\n")
|
||||
f.flush()
|
||||
os.fsync(f.fileno())
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
volthege_min = 819 # 模拟量映射最小值（对应 0V/4mA）
|
||||
volthege_max = 4095 # 模拟量映射最大值（对应 10V/20mA）
|
||||
x_max = 1000 # 最大行程
|
||||
impulse_max = 163840 # 电机脉冲最大值（对应 x_max）
|
||||
|
||||
|
||||
def set_motor_limits(volthege_min_val=None, volthege_max_val=None, x_max_val=None):
|
||||
"""更新电机限幅参数（由 GUI 高级设置页面调用）
|
||||
|
||||
Args:
|
||||
volthege_min_val: 模拟量映射最小值，None 表示不更新
|
||||
volthege_max_val: 模拟量映射最大值，None 表示不更新
|
||||
x_max_val: 最大行程（对应 GUI 总限幅），None 表示不更新
|
||||
"""
|
||||
global volthege_min, volthege_max, x_max
|
||||
if volthege_min_val is not None:
|
||||
volthege_min = volthege_min_val
|
||||
if volthege_max_val is not None:
|
||||
volthege_max = volthege_max_val
|
||||
if x_max_val is not None:
|
||||
x_max = x_max_val
|
||||
|
||||
# ---------- 原有的 PLC Modbus TCP 客户端类 ----------
|
||||
class Easy521ModbusClient:
|
||||
# 参数来源（GUI 页面1 Modbus TCP 区）：
|
||||
# host <- PLC地址 (默认 192.168.1.88)
|
||||
# port <- 端口 (默认 502)
|
||||
# current_p_addr <- 读取压力寄存器地址 (默认 504)
|
||||
def __init__(self, host="192.168.1.88", port=502, slave_id=1, current_p_addr=504):
|
||||
self.host = host
|
||||
self.port = port
|
||||
self.slave_id = slave_id
|
||||
self.client = ModbusTcpClient(
|
||||
host=host,
|
||||
port=port,
|
||||
timeout=3,
|
||||
retries=3
|
||||
)
|
||||
self.connected = False
|
||||
self.current_p_addr0 = current_p_addr
|
||||
self.current_p_addr = current_p_addr
|
||||
# self.current_p_addr = 18
|
||||
self.target_p_addr = 42
|
||||
self.u_addr = 514
|
||||
# self.u_addr = 40
|
||||
# self.output_postion = 514
|
||||
self.control_flag_addr = 100
|
||||
self.M901_ADDR = 901
|
||||
self.M902_ADDR = 902
|
||||
self.M903_ADDR = 903
|
||||
self.M904_ADDR = 904
|
||||
self.M905_ADDR = 905
|
||||
self.M906_ADDR = 906
|
||||
self.q_addr = 512
|
||||
|
||||
def connect(self):
|
||||
try:
|
||||
connection = self.client.connect()
|
||||
if connection:
|
||||
print(f"成功连接到 {self.host}:{self.port}")
|
||||
self.connected = True
|
||||
else:
|
||||
print(f"无法连接到 {self.host}:{self.port}")
|
||||
self.connected = False
|
||||
return connection
|
||||
except Exception as e:
|
||||
print(f"连接错误: {e}")
|
||||
self.connected = False
|
||||
return False
|
||||
|
||||
def disconnect(self):
|
||||
self.client.close()
|
||||
self.connected = False
|
||||
print("连接已关闭")
|
||||
|
||||
def read_float(self, address):
|
||||
try:
|
||||
address = int(address)
|
||||
result = self.client.read_input_registers(
|
||||
address=address,
|
||||
count=2,
|
||||
slave=self.slave_id
|
||||
)
|
||||
if not result.isError():
|
||||
decoder = BinaryPayloadDecoder.fromRegisters(
|
||||
result.registers,
|
||||
byteorder=Endian.BIG,
|
||||
wordorder=Endian.LITTLE
|
||||
)
|
||||
return decoder.decode_32bit_float()
|
||||
else:
|
||||
print(f"读取寄存器错误: {result}")
|
||||
return None
|
||||
except Exception as e:
|
||||
print(f"读取浮点数时发生错误: {e}")
|
||||
return None
|
||||
|
||||
def write_float(self, address, float_value):
|
||||
try:
|
||||
address = int(address)
|
||||
builder = BinaryPayloadBuilder(byteorder=Endian.LITTLE, wordorder=Endian.BIG)
|
||||
builder.add_32bit_float(float_value)
|
||||
payload = builder.to_registers()
|
||||
result = self.client.write_registers(
|
||||
address=address,
|
||||
values=payload,
|
||||
slave=self.slave_id
|
||||
)
|
||||
return not result.isError()
|
||||
except Exception as e:
|
||||
print(f"写入浮点数时发生错误: {e}")
|
||||
return False
|
||||
|
||||
def write_coil(self, address, value):
|
||||
try:
|
||||
address = int(address)
|
||||
result = self.client.write_coil(address=address, value=value, slave=self.slave_id)
|
||||
return not result.isError()
|
||||
except Exception as e:
|
||||
print(f"写入线圈时发生错误: {e}")
|
||||
return False
|
||||
|
||||
def get_current_p(self):
|
||||
return self.read_float(self.current_p_addr)
|
||||
|
||||
def get_current_p0(self):
|
||||
return self.read_float(self.current_p_addr0)
|
||||
|
||||
def get_target_p(self):
|
||||
return self.read_float(self.target_p_addr)
|
||||
|
||||
def get_current_q(self):
|
||||
return self.read_float(self.q_addr)
|
||||
|
||||
def write_u(self, float_value):
|
||||
try:
|
||||
address = int(self.u_addr)
|
||||
builder = BinaryPayloadBuilder(byteorder=Endian.BIG, wordorder=Endian.LITTLE)
|
||||
builder.add_32bit_float(float_value)
|
||||
payload = builder.to_registers()
|
||||
result = self.client.write_registers(
|
||||
address=address,
|
||||
values=payload,
|
||||
slave=self.slave_id
|
||||
)
|
||||
return not result.isError()
|
||||
except Exception as e:
|
||||
print(f"写入浮点数时发生错误: {e}")
|
||||
return False
|
||||
|
||||
def start_control(self):
|
||||
success = self.write_coil(self.control_flag_addr, True)
|
||||
if success:
|
||||
print("成功写入控制标志位True")
|
||||
else:
|
||||
print("写入控制标志位失败")
|
||||
|
||||
def stop_control(self):
|
||||
success = self.write_coil(self.control_flag_addr, False)
|
||||
if success:
|
||||
print("成功写入控制标志位False")
|
||||
else:
|
||||
print("写入控制标志位失败")
|
||||
|
||||
def read_rtu_flow(self, port='COM3', slave_id=2, baudrate=9600, bytesize=8, parity='N', stopbits=1):
|
||||
client = ModbusSerialClient(
|
||||
port=port,
|
||||
baudrate=baudrate,
|
||||
bytesize=bytesize,
|
||||
parity=parity,
|
||||
stopbits=stopbits,
|
||||
timeout=3
|
||||
)
|
||||
if not client.connect():
|
||||
print(f"无法连接到串口 {port}")
|
||||
return None
|
||||
|
||||
try:
|
||||
result = client.read_holding_registers(address=22, count=2, slave=slave_id)
|
||||
if result.isError():
|
||||
print(f"RTU 读取寄存器错误: {result}")
|
||||
return None
|
||||
decoder = BinaryPayloadDecoder.fromRegisters(
|
||||
result.registers,
|
||||
byteorder=Endian.BIG,
|
||||
wordorder=Endian.BIG
|
||||
)
|
||||
value = decoder.decode_32bit_uint() / 100
|
||||
return value
|
||||
except Exception as e:
|
||||
return None
|
||||
finally:
|
||||
client.close()
|
||||
|
||||
def start_up(self):
|
||||
if self.write_coil(self.M901_ADDR, True):
|
||||
print("M901 置位 TRUE")
|
||||
time.sleep(1)
|
||||
if self.write_coil(self.M901_ADDR, False):
|
||||
print("M901 复位 FALSE")
|
||||
else:
|
||||
print("警告：M901 复位失败")
|
||||
else:
|
||||
print("警告：M901 置位失败")
|
||||
time.sleep(1)
|
||||
|
||||
if self.write_coil(self.M902_ADDR, True):
|
||||
print("M902 置位 TRUE")
|
||||
time.sleep(1)
|
||||
if self.write_coil(self.M902_ADDR, False):
|
||||
print("M902 复位 FALSE")
|
||||
else:
|
||||
print("警告：M902 复位失败")
|
||||
else:
|
||||
print("警告：M902 置位失败")
|
||||
time.sleep(1)
|
||||
|
||||
if self.write_coil(self.M905_ADDR, True):
|
||||
print("M905 (初始开度) 置位 TRUE")
|
||||
time.sleep(1)
|
||||
if self.write_coil(self.M905_ADDR, False):
|
||||
print("M905 (初始开度) 复位 FALSE")
|
||||
else:
|
||||
print("警告：M905 (初始开度) 复位失败")
|
||||
else:
|
||||
print("警告：M905 (初始开度) 置位失败")
|
||||
time.sleep(1)
|
||||
|
||||
if self.write_coil(self.M903_ADDR, True):
|
||||
print("M903 置位 TRUE（持续）")
|
||||
else:
|
||||
print("警告：M903 置位失败")
|
||||
|
||||
if self.write_coil(self.M904_ADDR, True):
|
||||
print("M904 置位 TRUE（持续）")
|
||||
else:
|
||||
print("警告：M904 置位失败")
|
||||
time.sleep(1)
|
||||
|
||||
if self.write_coil(self.M906_ADDR, True):
|
||||
print("M906 (归零) 置位 TRUE")
|
||||
time.sleep(1)
|
||||
if self.write_coil(self.M906_ADDR, False):
|
||||
print("M906 (归零) 复位 FALSE")
|
||||
else:
|
||||
print("警告：M906 (归零) 复位失败")
|
||||
else:
|
||||
print("警告：M906 (归零) 置位失败")
|
||||
|
||||
print("初始化完成，M903 和 M904 已保持为 TRUE。")
|
||||
|
||||
|
||||
# ---------- 新增：独立的电机 Modbus RTU 客户端类（含报文打印） ----------
|
||||
class MotorModbusRTUClient:
|
||||
"""电机 Modbus RTU 通讯客户端（增强调试报文打印）"""
|
||||
# 参数来源（GUI 页面1 Modbus RTU 区）：
|
||||
# port <- 端口号 (下拉)
|
||||
# baudrate <- 波特率 (默认 115200)
|
||||
# slave_id <- 站号 (默认 4)
|
||||
# bytesize <- 数据位 (默认 8)
|
||||
# stopbits <- 停止位 (默认 1)
|
||||
# parity <- 校验位 None/Odd/Even -> 'N'/'O'/'E' (默认 'N')
|
||||
def __init__(self, port='/dev/cu.usbserial-BG02B0IX', slave_id=4, baudrate=115200,
|
||||
bytesize=8, parity='N', stopbits=1):
|
||||
# def __init__(self, port='/dev/cu.usbserial-D30JITMY', slave_id=4, baudrate=115200):
|
||||
self.port = port
|
||||
self.slave_id = slave_id
|
||||
self.baudrate = baudrate
|
||||
self.bytesize = bytesize
|
||||
self.parity = parity
|
||||
self.stopbits = stopbits
|
||||
self.client = None
|
||||
|
||||
def connect(self):
|
||||
"""连接电机串口"""
|
||||
self.client = ModbusSerialClient(
|
||||
port=self.port,
|
||||
baudrate=self.baudrate,
|
||||
bytesize=self.bytesize,
|
||||
parity=self.parity,
|
||||
stopbits=self.stopbits,
|
||||
timeout=5 # 超时时间延长，便于观察
|
||||
)
|
||||
if self.client.connect():
|
||||
print(f"电机串口 {self.port} 连接成功")
|
||||
return True
|
||||
else:
|
||||
print(f"电机串口 {self.port} 连接失败")
|
||||
return False
|
||||
|
||||
def disconnect(self):
|
||||
"""断开电机串口"""
|
||||
if self.client:
|
||||
self.client.close()
|
||||
self.client = None
|
||||
print("电机串口已关闭")
|
||||
|
||||
@staticmethod
|
||||
def _compute_crc(data: bytes) -> bytes:
|
||||
"""计算 Modbus CRC-16"""
|
||||
crc = 0xFFFF
|
||||
for byte in data:
|
||||
crc ^= byte
|
||||
for _ in range(8):
|
||||
if crc & 1:
|
||||
crc = (crc >> 1) ^ 0xA001
|
||||
else:
|
||||
crc >>= 1
|
||||
return crc.to_bytes(2, byteorder='little')
|
||||
|
||||
def _print_sent_message(self, address, function_code, data_bytes):
|
||||
"""构造完整报文并打印（含CRC）"""
|
||||
raw = bytes([self.slave_id, function_code]) + address.to_bytes(2, byteorder='big') + data_bytes
|
||||
crc = self._compute_crc(raw)
|
||||
full_msg = raw + crc
|
||||
hex_str = ' '.join(f'{b:02X}' for b in full_msg)
|
||||
# print(f"[发送] {hex_str}")
|
||||
|
||||
def _write_single_register(self, address, value):
|
||||
"""写单个寄存器（功能码 06）"""
|
||||
data_bytes = value.to_bytes(2, byteorder='big')
|
||||
self._print_sent_message(address, 0x06, data_bytes)
|
||||
|
||||
try:
|
||||
result = self.client.write_register(address, value, slave=self.slave_id)
|
||||
if result.isError():
|
||||
print(f"[接收] 错误响应: {result}")
|
||||
return False
|
||||
else:
|
||||
# print(f"[接收] 成功")
|
||||
return True
|
||||
except Exception as e:
|
||||
print(f"[接收] 异常: {e}")
|
||||
return False
|
||||
|
||||
def _write_32bit(self, address, value):
|
||||
"""
|
||||
写 32 位值到两个连续寄存器（功能码 10）
|
||||
字节序：大端（高字节在前，高字在前）
|
||||
"""
|
||||
builder = BinaryPayloadBuilder(byteorder=Endian.BIG, wordorder=Endian.BIG)
|
||||
builder.add_32bit_uint(value)
|
||||
payload = builder.to_registers()
|
||||
# 构造数据部分：字节计数 + 各寄存器大端两字节
|
||||
data_bytes = bytes([len(payload) * 2])
|
||||
for reg in payload:
|
||||
data_bytes += reg.to_bytes(2, byteorder='big')
|
||||
self._print_sent_message(address, 0x10, data_bytes)
|
||||
|
||||
try:
|
||||
result = self.client.write_registers(address, payload, slave=self.slave_id)
|
||||
if result.isError():
|
||||
print(f"[接收] 错误响应: {result}")
|
||||
return False
|
||||
else:
|
||||
# print(f"[接收] 成功")
|
||||
return True
|
||||
except Exception as e:
|
||||
print(f"[接收] 异常: {e}")
|
||||
return False
|
||||
|
||||
def init(self):
|
||||
"""初始化电机参数（仅需调用一次）"""
|
||||
if not self.client or not self.client.connected:
|
||||
print("电机未连接，请先调用 connect()")
|
||||
return False
|
||||
|
||||
print("开始初始化电机参数...")
|
||||
success = True
|
||||
|
||||
# 1. 写模式 4 → 0x6007
|
||||
print("--- 步骤1: 写模式 4 到 0x6007 ---")
|
||||
if not self._write_single_register(0x6007, 4):
|
||||
success = False
|
||||
print("模式写入失败")
|
||||
else:
|
||||
print("模式写入成功")
|
||||
|
||||
# 2. 写速度 64000 → 0x6072
|
||||
print("--- 步骤2: 写速度 64000 到 0x6072 ---")
|
||||
if not self._write_32bit(0x6072, 64000):
|
||||
success = False
|
||||
print("速度写入失败")
|
||||
else:
|
||||
print("速度写入成功")
|
||||
|
||||
# 3. 写加速度 2400000 → 0x6067
|
||||
print("--- 步骤3: 写加速度 96000 到 0x6067 ---")
|
||||
if not self._write_32bit(0x6067, 96000):
|
||||
success = False
|
||||
print("加速度写入失败")
|
||||
else:
|
||||
print("加速度写入成功")
|
||||
|
||||
# 4. 写减速度 240000 → 0x6069
|
||||
print("--- 步骤4: 写减速度 96000 到 0x6069 ---")
|
||||
if not self._write_32bit(0x6069, 96000):
|
||||
success = False
|
||||
print("减速度写入失败")
|
||||
else:
|
||||
print("减速度写入成功")
|
||||
|
||||
if success:
|
||||
print("电机初始化完成。")
|
||||
else:
|
||||
print("电机初始化过程中出现错误。")
|
||||
return success
|
||||
|
||||
def _read_single_register(self, address):
|
||||
"""读取单个16位寄存器"""
|
||||
try:
|
||||
result = self.client.read_holding_registers(address, 1, slave=self.slave_id)
|
||||
if not result.isError():
|
||||
return result.registers[0]
|
||||
else:
|
||||
print(f"读取寄存器 0x{address:X} 失败: {result}")
|
||||
return None
|
||||
except Exception as e:
|
||||
print(f"读取寄存器 0x{address:X} 异常: {e}")
|
||||
return None
|
||||
|
||||
def set_position(self, position):
|
||||
"""设置目标位置并立即启动（位置 0~impulse_max"""
|
||||
if not self.client or not self.client.connected:
|
||||
print("电机未连接，请先调用 connect()")
|
||||
return False
|
||||
|
||||
position = int(position / x_max * impulse_max)
|
||||
|
||||
if not (0 <= position <= impulse_max):
|
||||
print(f"位置值 {position} 超出范围 (0~{impulse_max})")
|
||||
return False
|
||||
|
||||
self._write_32bit(0x6074, position)
|
||||
ret2 = self._write_single_register(0x6070, 112)
|
||||
if not ret2:
|
||||
print("第一次写入控制字失败，1ms后重试...")
|
||||
time.sleep(0.001)
|
||||
ret2 = self._write_single_register(0x6070, 112)
|
||||
if ret2:
|
||||
print("第二次重试成功")
|
||||
else:
|
||||
print("第二次重试仍然失败")
|
||||
|
||||
return True
|
||||
|
||||
def read_current_position(self):
|
||||
"""
|
||||
读取电机当前位置（INT 型，32位有符号整数）
|
||||
从寄存器 0x600E 开始，连续读取 2 个保持寄存器
|
||||
字节序：大端（与写操作一致）
|
||||
:return: 当前位置值（int），读取失败返回 None
|
||||
"""
|
||||
if not self.client or not self.client.connected:
|
||||
print("电机未连接，无法读取位置")
|
||||
return None
|
||||
|
||||
try:
|
||||
result = self.client.read_holding_registers(
|
||||
address=0x600E,
|
||||
count=2,
|
||||
slave=self.slave_id
|
||||
)
|
||||
if result.isError():
|
||||
print(f"读取位置寄存器失败: {result}")
|
||||
return None
|
||||
|
||||
# 解码为 32 位有符号整数，使用与写操作相同的大端字节序
|
||||
decoder = BinaryPayloadDecoder.fromRegisters(
|
||||
result.registers,
|
||||
byteorder=Endian.BIG,
|
||||
wordorder=Endian.BIG
|
||||
)
|
||||
position = decoder.decode_32bit_int()
|
||||
print(f"当前位置position: {position}")
|
||||
position_x = position / impulse_max * x_max
|
||||
return position_x
|
||||
except Exception as e:
|
||||
print(f"读取当前位置异常: {e}")
|
||||
return None
|
||||
|
||||
|
||||
# ---------- PC读取压力值 ----------
|
||||
class PressureModbusRTUClient:
|
||||
"""压力变送器 Modbus RTU 通讯客户端（读取16位压力值）"""
|
||||
def __init__(self, port='/dev/cu.usbserial-D30JITMY', slave_id=1, baudrate=9600, bytesize=8, parity='N', stopbits=1, timeout=3):
|
||||
"""
|
||||
初始化压力客户端
|
||||
:param port: 串口端口，如 COM3、/dev/ttyUSB0
|
||||
:param slave_id: 从站地址（站号），默认 1
|
||||
:param baudrate: 波特率，默认 9600
|
||||
:param bytesize: 数据位，默认 8
|
||||
:param parity: 校验位，默认 'N'（无校验）
|
||||
:param stopbits: 停止位，默认 1
|
||||
:param timeout: 通讯超时时间（秒），默认 3
|
||||
"""
|
||||
self.port = port
|
||||
self.slave_id = slave_id
|
||||
self.baudrate = baudrate
|
||||
self.bytesize = bytesize
|
||||
self.parity = parity
|
||||
self.stopbits = stopbits
|
||||
self.timeout = timeout
|
||||
self.client = None
|
||||
self.pressure_register_addr = 4 # 压力寄存器地址（04）
|
||||
|
||||
def connect(self):
|
||||
"""连接压力变送器串口"""
|
||||
self.client = ModbusSerialClient(
|
||||
port=self.port,
|
||||
baudrate=self.baudrate,
|
||||
bytesize=self.bytesize,
|
||||
parity=self.parity,
|
||||
stopbits=self.stopbits,
|
||||
timeout=self.timeout
|
||||
)
|
||||
if self.client.connect():
|
||||
print(f"压力串口 {self.port} 连接成功 (站号 {self.slave_id})")
|
||||
return True
|
||||
else:
|
||||
print(f"压力串口 {self.port} 连接失败")
|
||||
return False
|
||||
|
||||
def disconnect(self):
|
||||
"""断开压力串口"""
|
||||
if self.client:
|
||||
self.client.close()
|
||||
self.client = None
|
||||
print("压力串口已关闭")
|
||||
|
||||
def get_current_p(self):
|
||||
"""
|
||||
读取压力值
|
||||
:return: 压力值（整数），若读取失败返回 None
|
||||
"""
|
||||
if not self.client or not self.client.connected:
|
||||
print("压力客户端未连接，请先调用 connect()")
|
||||
return None
|
||||
|
||||
try:
|
||||
t1 = time.perf_counter()
|
||||
# 读取保持寄存器（功能码03），地址4，个数1
|
||||
result = self.client.read_holding_registers(
|
||||
address=self.pressure_register_addr,
|
||||
count=1,
|
||||
slave=self.slave_id
|
||||
)
|
||||
if result.isError():
|
||||
print(f"压力读取错误: {result}")
|
||||
return None
|
||||
# 返回寄存器的第一个值（16位整数）
|
||||
pressure_raw = result.registers[0]
|
||||
# print(f"读压力用时：{time.perf_counter() - t1:.3f}s")
|
||||
return pressure_raw
|
||||
except ModbusException as e:
|
||||
print(f"压力读取 Modbus 异常: {e}")
|
||||
return None
|
||||
except Exception as e:
|
||||
print(f"压力读取未知异常: {e}")
|
||||
return None
|
||||
|
||||
|
||||
# ---------- 新增：MT2-AM8 模块 Modbus TCP 客户端 ----------
|
||||
class MT2AM8Client:
|
||||
"""
|
||||
艾莫迅 MT2-AM8 模块的 Modbus TCP 通讯类
|
||||
- 默认 IP：192.168.1.12，端口 502，模块地址（站号）默认为 1
|
||||
- 输入寄存器（AI）：地址 0x00~0x03（对应 PLC 地址 30001~30004）
|
||||
- 保持寄存器（AO）：地址 0x00~0x03（对应 PLC 地址 40001~40004）
|
||||
- 模拟量值范围：0～4095（对应 0～10V 或 0～20mA）
|
||||
"""
|
||||
def __init__(self, host="192.168.1.12", port=502, slave_id=1,
|
||||
pressure_range=400, flow_range=300):
|
||||
self.host = host
|
||||
self.port = port
|
||||
self.slave_id = slave_id
|
||||
self.pressure_range = pressure_range # 压力表量程上限
|
||||
self.flow_range = flow_range # 流量计量程上限
|
||||
self.client = ModbusTcpClient(
|
||||
host=host,
|
||||
port=port,
|
||||
timeout=3,
|
||||
retries=3
|
||||
)
|
||||
self.connected = False
|
||||
|
||||
def connect(self):
|
||||
"""连接模块"""
|
||||
try:
|
||||
conn = self.client.connect()
|
||||
if conn:
|
||||
print(f"成功连接到 MT2-AM8 模块 {self.host}:{self.port}")
|
||||
self.connected = True
|
||||
else:
|
||||
print(f"无法连接到 {self.host}:{self.port}")
|
||||
self.connected = False
|
||||
return conn
|
||||
except Exception as e:
|
||||
print(f"连接错误: {e}")
|
||||
self.connected = False
|
||||
return False
|
||||
|
||||
def disconnect(self):
|
||||
"""断开连接"""
|
||||
self.client.close()
|
||||
self.connected = False
|
||||
print("连接已关闭")
|
||||
|
||||
def read_analog_input(self, channel):
|
||||
"""
|
||||
读取单路模拟量输入原始值（16位无符号整数）
|
||||
:param channel: 通道号 0~3（对应 AI1~AI4）
|
||||
:return: 0~4095 的整数值，失败返回 None
|
||||
"""
|
||||
try:
|
||||
result = self.client.read_input_registers(
|
||||
address=channel,
|
||||
count=1,
|
||||
slave=self.slave_id
|
||||
)
|
||||
if not result.isError():
|
||||
return result.registers[0]
|
||||
else:
|
||||
print(f"读取输入寄存器错误: {result}")
|
||||
return None
|
||||
except Exception as e:
|
||||
print(f"读取模拟量输入异常: {e}")
|
||||
return None
|
||||
|
||||
# def read_all_analog_inputs(self):
|
||||
# """
|
||||
# 一次性读取全部 4 路模拟量输入
|
||||
# :return: 长度为4的列表（int），失败返回 None
|
||||
# """
|
||||
# try:
|
||||
# result = self.client.read_input_registers(
|
||||
# address=0,
|
||||
# count=4,
|
||||
# slave=self.slave_id
|
||||
# )
|
||||
# if not result.isError():
|
||||
# return result.registers
|
||||
# else:
|
||||
# print(f"读取全部输入寄存器错误: {result}")
|
||||
# return None
|
||||
# except Exception as e:
|
||||
# print(f"读取全部模拟量输入异常: {e}")
|
||||
# return None
|
||||
|
||||
def write_analog_output(self, channel, value):
|
||||
"""
|
||||
写入单路模拟量输出（保持寄存器）
|
||||
:param channel: 通道号 0~3（对应 AO1~AO4）
|
||||
:param value: {volthege_min}~{volthege_max} 的整数值
|
||||
:return: True 成功，False 失败
|
||||
"""
|
||||
# if not 0 <= channel <= 3:
|
||||
# print("通道号必须为 0~3")
|
||||
# return False
|
||||
if not 0 <= value <= volthege_max:
|
||||
print(f"值 {value} 超出范围 ({volthege_min}~{volthege_max})")
|
||||
return False
|
||||
try:
|
||||
result = self.client.write_register(
|
||||
address=channel,
|
||||
value=value,
|
||||
slave=self.slave_id
|
||||
)
|
||||
return not result.isError()
|
||||
except Exception as e:
|
||||
print(f"写入模拟量输出异常: {e}")
|
||||
return False
|
||||
|
||||
# def write_all_analog_outputs(self, values):
|
||||
# """
|
||||
# 一次性写入全部 4 路模拟量输出（用于批量设置）
|
||||
# :param values: 长度为4的列表或元组，每个元素为 0~4095
|
||||
# :return: True 成功，False 失败
|
||||
# """
|
||||
# if len(values) != 4:
|
||||
# print("需提供 4 个输出值")
|
||||
# return False
|
||||
# for v in values:
|
||||
# if not 0 <= v <= 4095:
|
||||
# print(f"值 {v} 超出范围 (0~4095)")
|
||||
# return False
|
||||
# try:
|
||||
# result = self.client.write_registers(
|
||||
# address=0,
|
||||
# values=list(values),
|
||||
# slave=self.slave_id
|
||||
# )
|
||||
# return not result.isError()
|
||||
# except Exception as e:
|
||||
# print(f"批量写入模拟量输出异常: {e}")
|
||||
# return False
|
||||
|
||||
def get_pressure(self, channel):
|
||||
"""
|
||||
读取压力值并转换为实际物理量
|
||||
转换公式：raw / (volthege_max - volthege_min) * pressure_range
|
||||
:param channel: 压力传感器模拟量通道地址
|
||||
:return: 实际压力值（kPa），失败返回 None
|
||||
"""
|
||||
raw = self.read_analog_input(channel)
|
||||
if raw is None:
|
||||
return None
|
||||
pressure = (raw - volthege_min) / (volthege_max - volthege_min) * self.pressure_range
|
||||
# print(f"读取压力通道 {channel} 原始值: {raw}, 转换后压力: {pressure:.2f} kPa")
|
||||
return pressure
|
||||
|
||||
def get_flow(self, channel):
|
||||
"""
|
||||
读取流量值并转换为实际物理量
|
||||
转换公式：raw / (volthege_max - volthege_min) * flow_range
|
||||
:param channel: 流量计模拟量通道地址
|
||||
:return: 实际流量值（L/min），失败返回 None
|
||||
"""
|
||||
raw = self.read_analog_input(channel)
|
||||
if raw is None:
|
||||
return None
|
||||
flow = (raw - volthege_min) / (volthege_max - volthege_min) * self.flow_range
|
||||
# print(f"读取流量通道 {channel} 原始值: {raw}, 转换后流量: {flow:.2f} L/min")
|
||||
return flow
|
||||
|
||||
|
||||
# def set_motor_speed(self, voltage_percent):
|
||||
# """
|
||||
# 通过模拟量输出控制电机（例如 0~100% 对应 0~10V）
|
||||
# :param voltage_percent: 0~100 的浮点数，表示百分比
|
||||
# """
|
||||
# if not 0 <= voltage_percent <= 100:
|
||||
# print("百分比需在 0~100 之间")
|
||||
# return False
|
||||
# # 将百分比映射到 0~4095
|
||||
# raw_value = int(voltage_percent / 100.0 * 4095)
|
||||
# return self.write_analog_output(0, raw_value) # 假设电机接在 AO1
|
||||
|
||||
def set_motor_position(self, voltage_distance, channel=0):
|
||||
"""
|
||||
通过模拟量输出控制电机（例如 0~1000 对应 0~10V）
|
||||
:param voltage_distance: 0~1000 的浮点数，表示行程
|
||||
:param channel: 模拟量输出通道号，默认 0（AO1）
|
||||
"""
|
||||
if not (0 <= voltage_distance <= x_max):
|
||||
print(f"行程需在 0~{x_max} 之间")
|
||||
return False
|
||||
# 将行程映射到 0~4095
|
||||
# Map the full travel range to the configured analog-output range.
|
||||
# The lower endpoint must include volthege_min; otherwise position 0
|
||||
# produces raw value 0 and is rejected by write_analog_output().
|
||||
raw_value = int(
|
||||
volthege_min
|
||||
+ voltage_distance / x_max * (volthege_max - volthege_min)
|
||||
)
|
||||
# print(f"设置电机行程为 {voltage_distance}，模拟量输出值 {raw_value}")
|
||||
return self.write_analog_output(channel, raw_value)
|
||||
|
||||
|
||||
# ---------- 主函数：测试示例 ----------
|
||||
# if __name__ == "__main__":
|
||||
# # （可选）启用 pymodbus 详细日志，可观察底层收发帧
|
||||
# # logging.basicConfig()
|
||||
# # logging.getLogger('pymodbus').setLevel(logging.DEBUG)
|
||||
#
|
||||
# motor = MotorModbusRTUClient()
|
||||
#
|
||||
#
|
||||
# print("连接电机 (Modbus RTU)...")
|
||||
# if not motor.connect():
|
||||
# print("电机连接失败，退出。")
|
||||
# exit(1)
|
||||
#
|
||||
# # 增加短暂延时，等待驱动器接口就绪
|
||||
# time.sleep(1)
|
||||
#
|
||||
# print("初始化电机参数...")
|
||||
# if not motor.init():
|
||||
# print("电机初始化失败，退出。")
|
||||
# motor.disconnect()
|
||||
# exit(1)
|
||||
#
|
||||
# print("\n========== 电机位置控制测试 ==========")
|
||||
# print("输入目标位置 (0~60000)，输入 'q' 退出。函数已修改，只需输入开度。\n")
|
||||
#
|
||||
# try:
|
||||
# while True:
|
||||
# user_input = input("目标位置: ").strip()
|
||||
# if user_input.lower() in ('q', 'quit', 'exit'):
|
||||
# break
|
||||
# if not user_input:
|
||||
# continue
|
||||
# try:
|
||||
# pos = int(user_input)
|
||||
# motor.set_position(pos)
|
||||
# except ValueError:
|
||||
# print("错误：请输入有效的整数。")
|
||||
# except KeyboardInterrupt:
|
||||
# print("\n用户中断测试。")
|
||||
# finally:
|
||||
# motor.disconnect()
|
||||
# print("程序结束。")
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,97 @@
|
||||
"""气体流量闭环控制的集中配置。
|
||||
|
||||
首次连接真实设备前，请重点核对：MT2-AM8 地址、AI/AO 通道、压力上限、
|
||||
阀门打开端行程和关闭端行程。PID 参数是安全起步值，不是最终整定结果。
|
||||
"""
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# MT2-AM8 通讯与量程
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
MT2AM8_HOST = "192.168.1.12"
|
||||
MT2AM8_PORT = 502
|
||||
MT2AM8_SLAVE_ID = 1
|
||||
|
||||
# MT2AM8Client 用这些量程把模拟量转换成物理量。
|
||||
PRESSURE_RANGE_KPA = 400.0
|
||||
FLOW_METER_RANGE_SLM = 300.0
|
||||
|
||||
# AI/AO 通道均从 0 开始。AI0 和 AO0 属于不同的寄存器区，可以同时使用。
|
||||
FLOW_INPUT_CHANNEL = 0
|
||||
PRESSURE_INPUT_CHANNEL = 1 # 没有压力传感器时改为 None
|
||||
MOTOR_OUTPUT_CHANNEL = 0
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 控制目标和控制周期
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
TARGET_FLOW_MIN_SLM = 0.0
|
||||
TARGET_FLOW_MAX_SLM = 100.0
|
||||
ZERO_FLOW_THRESHOLD_SLM = 0.5
|
||||
|
||||
CONTROL_PERIOD_S = 0.1 # 100 ms，10 Hz
|
||||
MAX_CONTROL_DT_S = 0.3 # 超过此值视为控制循环严重超时
|
||||
STATUS_PRINT_PERIOD_S = 1.0
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# PID：输出统一定义为阀门开度百分比
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
# 第一版建议先按 PI 使用（KD=0），然后通过阶跃实验重新整定。
|
||||
PID_KP = 0.5
|
||||
PID_KI = 0.1
|
||||
PID_KD = 0.0
|
||||
|
||||
OPENING_MIN_PCT = 0.0
|
||||
OPENING_MAX_PCT = 100.0
|
||||
MAX_OPENING_RATE_PCT_S = 20.0 # 每秒最多改变 20% 开度
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 阀门执行机构
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
# 电机行程越大，阀门开度越小。
|
||||
# opening=0% -> MOTOR_CLOSED_POSITION（完全关闭）
|
||||
# opening=100% -> MOTOR_OPEN_POSITION（打开端/机械死区边界）
|
||||
MOTOR_CLOSED_POSITION = 1000.0
|
||||
MOTOR_OPEN_POSITION = 240.0
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 安全阈值
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
# 物理读数允许范围；超过范围立即关阀。
|
||||
FLOW_VALID_MIN_SLM = -2.0
|
||||
FLOW_VALID_MAX_SLM = 120.0
|
||||
MAX_PRESSURE_KPA = 350.0 # 必须按实际管路额定压力确认
|
||||
|
||||
# 短暂读取失败时保持上一输出，不下发新位置；连续达到阈值后关阀。
|
||||
MAX_CONSECUTIVE_FLOW_FAILURES = 3
|
||||
MAX_CONSECUTIVE_PRESSURE_FAILURES = 3
|
||||
|
||||
# 日志目录相对于工程目录。
|
||||
LOG_DIRECTORY = "logs"
|
||||
LOG_FILE_NAME = "flow_control.log"
|
||||
|
||||
|
||||
def validate_config():
|
||||
"""在接触硬件前检查明显的配置错误。"""
|
||||
if CONTROL_PERIOD_S <= 0:
|
||||
raise ValueError("CONTROL_PERIOD_S 必须大于 0")
|
||||
if MAX_CONTROL_DT_S < CONTROL_PERIOD_S:
|
||||
raise ValueError("MAX_CONTROL_DT_S 不得小于 CONTROL_PERIOD_S")
|
||||
if TARGET_FLOW_MIN_SLM > TARGET_FLOW_MAX_SLM:
|
||||
raise ValueError("目标流量上下限配置错误")
|
||||
if not 0 <= OPENING_MIN_PCT < OPENING_MAX_PCT <= 100:
|
||||
raise ValueError("阀门开度范围必须位于 0~100%，且下限小于上限")
|
||||
if MOTOR_OPEN_POSITION >= MOTOR_CLOSED_POSITION:
|
||||
raise ValueError("本系统行程越大开度越小，因此打开端行程必须小于关闭端行程")
|
||||
if MOTOR_OPEN_POSITION < 0 or MOTOR_CLOSED_POSITION > 1000:
|
||||
raise ValueError("电机行程必须位于 MT2AM8Client 当前采用的 0~1000 范围")
|
||||
if MAX_OPENING_RATE_PCT_S <= 0:
|
||||
raise ValueError("MAX_OPENING_RATE_PCT_S 必须大于 0")
|
||||
if MAX_CONSECUTIVE_FLOW_FAILURES < 1:
|
||||
raise ValueError("MAX_CONSECUTIVE_FLOW_FAILURES 必须至少为 1")
|
||||
if PRESSURE_INPUT_CHANNEL is not None and MAX_PRESSURE_KPA is None:
|
||||
raise ValueError("启用压力通道时必须配置 MAX_PRESSURE_KPA")
|
||||
|
||||
@@ -0,0 +1,145 @@
|
||||
# controllers.py
|
||||
import os, sys, time
|
||||
|
||||
|
||||
def _pid_log(msg: str):
|
||||
"""PID 内部日志，直接写文件 + 刷盘"""
|
||||
try:
|
||||
log_dir = os.path.join(os.path.dirname(sys.executable), "logs")
|
||||
os.makedirs(log_dir, exist_ok=True)
|
||||
log_file = os.path.join(log_dir, "control_debug.log")
|
||||
with open(log_file, "a", encoding="utf-8") as f:
|
||||
f.write(f"[{time.strftime('%H:%M:%S.%f')[:-3]}] [PID] {msg}\n")
|
||||
f.flush()
|
||||
os.fsync(f.fileno())
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
class IncrementalPID:
|
||||
"""增量式PID控制器"""
|
||||
|
||||
def __init__(self, kp: float, ki: float, kd: float, dt: float,
|
||||
out_min: float, out_max: float, xa_full: float = 1062.5):
|
||||
# PID参数
|
||||
self.kp = kp
|
||||
self.ki = ki
|
||||
self.kd = kd
|
||||
self.dt = dt # 默认50HZ执行周期防止除零
|
||||
self.motor_max = 300.0
|
||||
self.du_max = self.motor_max * self.dt
|
||||
self.dead_area = 240.0
|
||||
self.xa_full = xa_full # 总限幅（最大行程）
|
||||
|
||||
# self.kp = 1.392
|
||||
# self.ki = 30.2
|
||||
# self.kd = 0.000485
|
||||
# self.dt = 0.0059
|
||||
|
||||
# 限幅设置
|
||||
self.out_min = out_min
|
||||
self.out_max = out_max
|
||||
|
||||
# 输入输出
|
||||
self.target_pressure = 0.0 # 参考值（设定压力大小）
|
||||
self.current_pressure = 0.0 # 反馈值
|
||||
self.error = 0.0 # 当前误差
|
||||
|
||||
# 计算系数
|
||||
self.a0 = 0.0
|
||||
self.a1 = 0.0
|
||||
self.a2 = 0.0
|
||||
self._calculate_coefficients()
|
||||
|
||||
# 控制器状态
|
||||
self.prev_error = 0.0 # 前次误差 e(k-1)
|
||||
self.prev_error2 = 0.0 # 前前次误差 e(k-2)
|
||||
self.output = 0.0 # 控制器总输出
|
||||
|
||||
def _calculate_coefficients(self):
|
||||
"""重新计算增量式PID系数"""
|
||||
if self.dt <= 0:
|
||||
return
|
||||
self.a0 = self.kp + (self.ki * self.dt / 2.0) + (2.0 * self.kd / self.dt)
|
||||
self.a1 = -self.kp + (self.ki * self.dt / 2.0) - (4.0 * self.kd / self.dt)
|
||||
self.a2 = (2.0 * self.kd) / self.dt
|
||||
|
||||
def update_pressure_values(self, current_pressure, target_pressure):
|
||||
"""更新当前压力和目标压力值"""
|
||||
self.current_pressure = current_pressure
|
||||
self.target_pressure = target_pressure
|
||||
|
||||
def update(self, du_max=None):
|
||||
# 计算当前误差
|
||||
self.error = -(self.target_pressure - self.current_pressure)
|
||||
|
||||
# if abs(self.error) < 1:
|
||||
# return self.output # 误差过小，直接返回当前输出
|
||||
|
||||
# 计算控制增量
|
||||
delta = (self.a0 * self.error + self.a1 * self.prev_error + self.a2 * self.prev_error2)
|
||||
|
||||
if du_max is not None:
|
||||
self.du_max = du_max
|
||||
else:
|
||||
self.du_max = self.get_du_max(self.target_pressure)
|
||||
|
||||
# 纯 Python 限幅（替代 np.clip）
|
||||
if delta > self.du_max:
|
||||
delta = self.du_max
|
||||
elif delta < -self.du_max:
|
||||
delta = -self.du_max
|
||||
|
||||
# 计算新输出
|
||||
new_output = self.output + delta
|
||||
# print(f"output:{self.output}, delta:{delta}, new_output:{new_output}")
|
||||
|
||||
# 应用输出限幅
|
||||
new_output = max(self.out_min, min(self.out_max, new_output))
|
||||
|
||||
# 更新历史状态
|
||||
self.prev_error2 = self.prev_error
|
||||
self.prev_error = self.error
|
||||
self.output = new_output
|
||||
return new_output
|
||||
|
||||
def reset(self):
|
||||
"""重置PID控制器状态（保留参数）"""
|
||||
self.prev_error = 0.0
|
||||
self.prev_error2 = 0.0
|
||||
self.output = 0.0
|
||||
|
||||
def update_parameters(self, kp: float, ki: float, kd: float):
|
||||
self.kp = kp
|
||||
self.ki = ki
|
||||
self.kd = kd
|
||||
self._calculate_coefficients()
|
||||
|
||||
def set_du_max(self, value):
|
||||
"""设置 du_max（供外部模块通过方法调用设置，避免跨 .pyd 属性写入 crash）"""
|
||||
self.du_max = value
|
||||
|
||||
def get_du_max(self, target_pressure):
|
||||
"""根据目标压力计算 PID 最大增量限幅（纯 Python 线性插值）"""
|
||||
x = float(target_pressure)
|
||||
dt = float(self.dt)
|
||||
|
||||
if x <= 0.0:
|
||||
val = 500.0 * dt
|
||||
|
||||
elif x >= 200.0:
|
||||
val = 250.0 * dt
|
||||
|
||||
elif x <= 100.0:
|
||||
# 0~100：从 500 线性下降到 300
|
||||
val = (500.0 - 2.0 * x) * dt
|
||||
|
||||
else:
|
||||
# 100~200：从 300 线性下降到 250
|
||||
val = (350.0 - 0.5 * x) * dt
|
||||
|
||||
return val
|
||||
|
||||
def init_v(self, position_x):
|
||||
v = (self.xa_full - position_x) / (self.xa_full - self.dead_area) * 100
|
||||
return v
|
||||
+211
@@ -0,0 +1,211 @@
|
||||
"""Generic incremental PID controller.
|
||||
|
||||
The controller operates on a generic setpoint and measurement. Any actuator
|
||||
mapping (for example, converting valve opening to motor travel) belongs in the
|
||||
caller or hardware layer, not in this module.
|
||||
"""
|
||||
|
||||
|
||||
class IncrementalPID:
|
||||
"""Incremental PID controller with output and output-rate limits.
|
||||
|
||||
``error`` is always calculated as ``setpoint - measurement``. The output
|
||||
is the accumulated controller command, bounded by ``out_min`` and
|
||||
``out_max``.
|
||||
|
||||
``output_rate_limit`` is expressed in output units per second. When it is
|
||||
set, the maximum output change in one update is
|
||||
``output_rate_limit * dt``. ``du_max`` remains available as a legacy
|
||||
per-update limit through :meth:`set_du_max` or the ``update`` keyword.
|
||||
``xa_full`` is accepted for compatibility with older callers but is not
|
||||
used; actuator travel and dead-zone mapping do not belong in a PID.
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
kp: float,
|
||||
ki: float,
|
||||
kd: float,
|
||||
dt: float,
|
||||
out_min: float,
|
||||
out_max: float,
|
||||
xa_full=None,
|
||||
output_rate_limit=None,
|
||||
):
|
||||
if dt <= 0:
|
||||
raise ValueError("dt must be greater than zero")
|
||||
if out_min > out_max:
|
||||
raise ValueError("out_min must not be greater than out_max")
|
||||
if output_rate_limit is not None and output_rate_limit < 0:
|
||||
raise ValueError("output_rate_limit must not be negative")
|
||||
|
||||
self.kp = float(kp)
|
||||
self.ki = float(ki)
|
||||
self.kd = float(kd)
|
||||
self.dt = float(dt)
|
||||
self.out_min = float(out_min)
|
||||
self.out_max = float(out_max)
|
||||
self.output_rate_limit = (
|
||||
None if output_rate_limit is None else float(output_rate_limit)
|
||||
)
|
||||
|
||||
# Kept only as a compatibility attribute. It has no control meaning
|
||||
# in this generic controller and is intentionally not used.
|
||||
self.xa_full = xa_full
|
||||
|
||||
self.setpoint = 0.0
|
||||
self.measurement = 0.0
|
||||
self.error = 0.0
|
||||
|
||||
self.a0 = 0.0
|
||||
self.a1 = 0.0
|
||||
self.a2 = 0.0
|
||||
self._calculate_coefficients()
|
||||
|
||||
self.prev_error = 0.0
|
||||
self.prev_error2 = 0.0
|
||||
self.output = 0.0
|
||||
|
||||
# Legacy per-update increment limit. The newer
|
||||
# output_rate_limit takes precedence when configured.
|
||||
self.du_max = None
|
||||
|
||||
def _calculate_coefficients(self):
|
||||
"""Calculate the discrete incremental-PID coefficients."""
|
||||
self.a0 = self.kp + (self.ki * self.dt / 2.0) + (2.0 * self.kd / self.dt)
|
||||
self.a1 = -self.kp + (self.ki * self.dt / 2.0) - (4.0 * self.kd / self.dt)
|
||||
self.a2 = (2.0 * self.kd) / self.dt
|
||||
|
||||
def set_values(self, measurement, setpoint):
|
||||
"""Set the current measurement and desired setpoint."""
|
||||
self.measurement = float(measurement)
|
||||
self.setpoint = float(setpoint)
|
||||
|
||||
# Public aliases retained for older code that still reads these names.
|
||||
# They are plain aliases and do not add pressure-specific control logic.
|
||||
@property
|
||||
def current_pressure(self):
|
||||
return self.measurement
|
||||
|
||||
@current_pressure.setter
|
||||
def current_pressure(self, value):
|
||||
self.measurement = float(value)
|
||||
|
||||
@property
|
||||
def target_pressure(self):
|
||||
return self.setpoint
|
||||
|
||||
@target_pressure.setter
|
||||
def target_pressure(self, value):
|
||||
self.setpoint = float(value)
|
||||
|
||||
def update_values(self, measurement, setpoint):
|
||||
"""Compatibility-friendly alias for :meth:`set_values`."""
|
||||
self.set_values(measurement, setpoint)
|
||||
|
||||
def update_pressure_values(self, current_pressure, target_pressure):
|
||||
"""Legacy alias; use :meth:`set_values` for new code."""
|
||||
self.set_values(current_pressure, target_pressure)
|
||||
|
||||
def update(
|
||||
self,
|
||||
measurement=None,
|
||||
setpoint=None,
|
||||
*,
|
||||
dt=None,
|
||||
output_rate_limit=None,
|
||||
du_max=None,
|
||||
):
|
||||
"""Run one controller step and return the bounded output.
|
||||
|
||||
``measurement`` and ``setpoint`` may be omitted when they were already
|
||||
supplied with :meth:`set_values` (or the legacy alias). ``dt`` is an
|
||||
optional per-step override and is measured in seconds.
|
||||
|
||||
``output_rate_limit`` is a per-second limit. The legacy ``du_max``
|
||||
keyword is a per-update limit and takes precedence for that call.
|
||||
"""
|
||||
if measurement is not None:
|
||||
self.measurement = float(measurement)
|
||||
if setpoint is not None:
|
||||
self.setpoint = float(setpoint)
|
||||
|
||||
if dt is not None:
|
||||
if dt <= 0:
|
||||
raise ValueError("dt must be greater than zero")
|
||||
if float(dt) != self.dt:
|
||||
self.dt = float(dt)
|
||||
self._calculate_coefficients()
|
||||
|
||||
if output_rate_limit is not None:
|
||||
if output_rate_limit < 0:
|
||||
raise ValueError("output_rate_limit must not be negative")
|
||||
rate_limit = float(output_rate_limit)
|
||||
else:
|
||||
rate_limit = self.output_rate_limit
|
||||
|
||||
self.error = self.setpoint - self.measurement
|
||||
delta = (
|
||||
self.a0 * self.error
|
||||
+ self.a1 * self.prev_error
|
||||
+ self.a2 * self.prev_error2
|
||||
)
|
||||
|
||||
if du_max is not None:
|
||||
if du_max < 0:
|
||||
raise ValueError("du_max must not be negative")
|
||||
self.du_max = float(du_max)
|
||||
|
||||
if du_max is not None:
|
||||
max_delta = float(du_max)
|
||||
elif rate_limit is not None:
|
||||
max_delta = rate_limit * self.dt
|
||||
elif self.du_max is not None:
|
||||
max_delta = abs(float(self.du_max))
|
||||
else:
|
||||
max_delta = None
|
||||
|
||||
if max_delta is not None:
|
||||
delta = max(-max_delta, min(max_delta, delta))
|
||||
|
||||
new_output = self.output + delta
|
||||
new_output = max(self.out_min, min(self.out_max, new_output))
|
||||
|
||||
self.prev_error2 = self.prev_error
|
||||
self.prev_error = self.error
|
||||
self.output = new_output
|
||||
return new_output
|
||||
|
||||
def reset(self, initial_output=0.0):
|
||||
"""Reset controller history and initialize the output command."""
|
||||
initial_output = float(initial_output)
|
||||
self.output = max(self.out_min, min(self.out_max, initial_output))
|
||||
self.prev_error = 0.0
|
||||
self.prev_error2 = 0.0
|
||||
self.error = 0.0
|
||||
|
||||
def update_parameters(self, kp: float, ki: float, kd: float):
|
||||
"""Update PID gains and recalculate the discrete coefficients."""
|
||||
self.kp = float(kp)
|
||||
self.ki = float(ki)
|
||||
self.kd = float(kd)
|
||||
self._calculate_coefficients()
|
||||
|
||||
def set_dt(self, dt: float):
|
||||
"""Set the controller period in seconds."""
|
||||
if dt <= 0:
|
||||
raise ValueError("dt must be greater than zero")
|
||||
self.dt = float(dt)
|
||||
self._calculate_coefficients()
|
||||
|
||||
def set_output_rate_limit(self, value):
|
||||
"""Set or clear the output slew-rate limit in output units/second."""
|
||||
if value is not None and value < 0:
|
||||
raise ValueError("output_rate_limit must not be negative")
|
||||
self.output_rate_limit = None if value is None else float(value)
|
||||
|
||||
def set_du_max(self, value):
|
||||
"""Legacy setter for a maximum output change per update."""
|
||||
if value is not None and value < 0:
|
||||
raise ValueError("du_max must not be negative")
|
||||
self.du_max = None if value is None else float(value)
|
||||
+458
@@ -0,0 +1,458 @@
|
||||
"""流量闭环控制层。
|
||||
|
||||
本模块只负责“读取传感器 -> PID -> 开度/行程映射 -> 下发电机位置”以及
|
||||
安全处理；底层 Modbus 通讯仍由 PcControl.MT2AM8Client 完成。
|
||||
"""
|
||||
|
||||
from dataclasses import asdict, dataclass
|
||||
import math
|
||||
import time
|
||||
from typing import Optional
|
||||
|
||||
|
||||
@dataclass
|
||||
class ControlStepResult:
|
||||
"""一个控制周期的可记录结果。"""
|
||||
|
||||
timestamp: float
|
||||
target_flow_slm: float
|
||||
measured_flow_slm: Optional[float]
|
||||
pressure_kpa: Optional[float]
|
||||
error_slm: Optional[float]
|
||||
opening_pct: float
|
||||
motor_position: float
|
||||
actual_dt_s: float
|
||||
status: str = "OK"
|
||||
|
||||
def to_dict(self):
|
||||
return asdict(self)
|
||||
|
||||
|
||||
class FlowControlFault(RuntimeError):
|
||||
"""包含故障代码、阶段和现场数据的控制故障。"""
|
||||
|
||||
def __init__(self, code, stage, message, context=None):
|
||||
super().__init__(message)
|
||||
self.code = str(code)
|
||||
self.stage = str(stage)
|
||||
self.context = dict(context or {})
|
||||
|
||||
def __str__(self):
|
||||
return (
|
||||
f"[{self.code}] 阶段={self.stage}: {super().__str__()} | "
|
||||
f"上下文={self.context}"
|
||||
)
|
||||
|
||||
|
||||
def opening_to_motor_position(
|
||||
opening_pct,
|
||||
motor_open_position,
|
||||
motor_closed_position,
|
||||
):
|
||||
"""把 0~100% 阀门开度换算成反向作用的电机行程。"""
|
||||
opening = float(opening_pct)
|
||||
open_position = float(motor_open_position)
|
||||
closed_position = float(motor_closed_position)
|
||||
|
||||
if not math.isfinite(opening):
|
||||
raise ValueError("opening_pct 必须是有限数值")
|
||||
if not 0.0 <= opening <= 100.0:
|
||||
raise ValueError("opening_pct 必须位于 0~100%")
|
||||
if open_position >= closed_position:
|
||||
raise ValueError("打开端行程必须小于关闭端行程")
|
||||
|
||||
return closed_position - opening / 100.0 * (
|
||||
closed_position - open_position
|
||||
)
|
||||
|
||||
|
||||
class FlowControlLoop:
|
||||
"""基于 MT2AM8Client 和 IncrementalPID 的单回路流量控制器。"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
hardware,
|
||||
pid,
|
||||
*,
|
||||
period_s,
|
||||
flow_channel,
|
||||
motor_channel,
|
||||
motor_open_position,
|
||||
motor_closed_position,
|
||||
target_min_slm=0.0,
|
||||
target_max_slm=100.0,
|
||||
zero_flow_threshold_slm=0.5,
|
||||
flow_valid_min_slm=-2.0,
|
||||
flow_valid_max_slm=120.0,
|
||||
pressure_channel=None,
|
||||
max_pressure_kpa=None,
|
||||
max_control_dt_s=None,
|
||||
max_consecutive_flow_failures=3,
|
||||
max_consecutive_pressure_failures=3,
|
||||
logger=None,
|
||||
):
|
||||
if period_s <= 0:
|
||||
raise ValueError("period_s 必须大于 0")
|
||||
if target_min_slm > target_max_slm:
|
||||
raise ValueError("目标流量上下限配置错误")
|
||||
if flow_valid_min_slm > flow_valid_max_slm:
|
||||
raise ValueError("流量有效范围配置错误")
|
||||
if pressure_channel is not None and max_pressure_kpa is None:
|
||||
raise ValueError("启用压力监测时必须设置 max_pressure_kpa")
|
||||
|
||||
self.hardware = hardware
|
||||
self.pid = pid
|
||||
self.period_s = float(period_s)
|
||||
self.flow_channel = int(flow_channel)
|
||||
self.motor_channel = int(motor_channel)
|
||||
self.motor_open_position = float(motor_open_position)
|
||||
self.motor_closed_position = float(motor_closed_position)
|
||||
self.target_min_slm = float(target_min_slm)
|
||||
self.target_max_slm = float(target_max_slm)
|
||||
self.zero_flow_threshold_slm = float(zero_flow_threshold_slm)
|
||||
self.flow_valid_min_slm = float(flow_valid_min_slm)
|
||||
self.flow_valid_max_slm = float(flow_valid_max_slm)
|
||||
self.pressure_channel = (
|
||||
None if pressure_channel is None else int(pressure_channel)
|
||||
)
|
||||
self.max_pressure_kpa = (
|
||||
None if max_pressure_kpa is None else float(max_pressure_kpa)
|
||||
)
|
||||
self.max_control_dt_s = (
|
||||
None if max_control_dt_s is None else float(max_control_dt_s)
|
||||
)
|
||||
self.max_consecutive_flow_failures = int(max_consecutive_flow_failures)
|
||||
self.max_consecutive_pressure_failures = int(
|
||||
max_consecutive_pressure_failures
|
||||
)
|
||||
self.logger = logger
|
||||
|
||||
self.target_flow_slm = 0.0
|
||||
self.last_flow_slm = None
|
||||
self.last_pressure_kpa = None
|
||||
self.last_opening_pct = 0.0
|
||||
self.last_motor_position = self.motor_closed_position
|
||||
self.last_step_time = None
|
||||
self.flow_failure_count = 0
|
||||
self.pressure_failure_count = 0
|
||||
self.running = False
|
||||
self.faulted = False
|
||||
|
||||
def set_target_flow(self, target_flow_slm):
|
||||
"""设置目标流量；超出允许范围时拒绝，而不是静默截断。"""
|
||||
target = float(target_flow_slm)
|
||||
if not math.isfinite(target):
|
||||
raise ValueError("目标流量必须是有限数值")
|
||||
if not self.target_min_slm <= target <= self.target_max_slm:
|
||||
raise ValueError(
|
||||
f"目标流量 {target} SLM 超出 "
|
||||
f"{self.target_min_slm}~{self.target_max_slm} SLM"
|
||||
)
|
||||
self.target_flow_slm = target
|
||||
|
||||
def start(self, initial_opening=0.0):
|
||||
"""复位状态并启动控制；默认从关闭开度开始。"""
|
||||
initial_opening = self._bounded_opening(initial_opening)
|
||||
self.pid.reset(initial_output=initial_opening)
|
||||
self.last_opening_pct = initial_opening
|
||||
self.last_motor_position = opening_to_motor_position(
|
||||
initial_opening,
|
||||
self.motor_open_position,
|
||||
self.motor_closed_position,
|
||||
)
|
||||
self.last_step_time = None
|
||||
self.flow_failure_count = 0
|
||||
self.pressure_failure_count = 0
|
||||
self.faulted = False
|
||||
self.running = True
|
||||
|
||||
def stop(self, close_valve=True):
|
||||
"""停止 PID；默认同时安全关阀。"""
|
||||
self.running = False
|
||||
self.pid.reset(initial_output=0.0)
|
||||
if close_valve:
|
||||
return self.safe_close("STOP_REQUESTED")
|
||||
return True
|
||||
|
||||
def step(self, now=None):
|
||||
"""执行一个控制周期。短暂读取失败会保持上一输出。"""
|
||||
if not self.running:
|
||||
raise FlowControlFault(
|
||||
"CONTROL_NOT_RUNNING",
|
||||
"PRECHECK",
|
||||
"控制器尚未 start()",
|
||||
self._context(),
|
||||
)
|
||||
if self.faulted:
|
||||
raise FlowControlFault(
|
||||
"CONTROL_FAULTED",
|
||||
"PRECHECK",
|
||||
"控制器处于故障锁定状态，需要重新 start()",
|
||||
self._context(),
|
||||
)
|
||||
if not bool(getattr(self.hardware, "connected", False)):
|
||||
self._trip(
|
||||
"DEVICE_DISCONNECTED",
|
||||
"PRECHECK",
|
||||
"MT2-AM8 未连接",
|
||||
)
|
||||
|
||||
current_time = time.perf_counter() if now is None else float(now)
|
||||
actual_dt = (
|
||||
self.period_s
|
||||
if self.last_step_time is None
|
||||
else current_time - self.last_step_time
|
||||
)
|
||||
self.last_step_time = current_time
|
||||
if actual_dt <= 0:
|
||||
actual_dt = self.period_s
|
||||
if self.max_control_dt_s is not None and actual_dt > self.max_control_dt_s:
|
||||
self._trip(
|
||||
"CONTROL_LOOP_OVERRUN",
|
||||
"TIMING",
|
||||
f"实际控制周期 {actual_dt:.3f}s 超过上限 "
|
||||
f"{self.max_control_dt_s:.3f}s",
|
||||
{"actual_dt_s": actual_dt},
|
||||
)
|
||||
|
||||
flow = self._read_flow()
|
||||
pressure = self._read_pressure()
|
||||
|
||||
if flow is None:
|
||||
return self._held_result(
|
||||
actual_dt,
|
||||
pressure,
|
||||
f"FLOW_READ_RETRY_{self.flow_failure_count}",
|
||||
)
|
||||
if self.pressure_channel is not None and pressure is None:
|
||||
return self._held_result(
|
||||
actual_dt,
|
||||
pressure,
|
||||
f"PRESSURE_READ_RETRY_{self.pressure_failure_count}",
|
||||
)
|
||||
|
||||
if not self.flow_valid_min_slm <= flow <= self.flow_valid_max_slm:
|
||||
self._trip(
|
||||
"FLOW_OUT_OF_RANGE",
|
||||
"FLOW_SAFETY_CHECK",
|
||||
f"流量读数 {flow:.3f} SLM 超出允许范围",
|
||||
{"measured_flow_slm": flow},
|
||||
)
|
||||
if (
|
||||
pressure is not None
|
||||
and self.max_pressure_kpa is not None
|
||||
and pressure > self.max_pressure_kpa
|
||||
):
|
||||
self._trip(
|
||||
"PRESSURE_OVER_LIMIT",
|
||||
"PRESSURE_SAFETY_CHECK",
|
||||
f"压力 {pressure:.3f} kPa 超过上限 "
|
||||
f"{self.max_pressure_kpa:.3f} kPa",
|
||||
{"pressure_kpa": pressure},
|
||||
)
|
||||
|
||||
if self.target_flow_slm <= self.zero_flow_threshold_slm:
|
||||
self.pid.reset(initial_output=0.0)
|
||||
opening = 0.0
|
||||
else:
|
||||
opening = self.pid.update(
|
||||
measurement=flow,
|
||||
setpoint=self.target_flow_slm,
|
||||
dt=actual_dt,
|
||||
)
|
||||
if not self._is_finite_number(opening):
|
||||
self._trip(
|
||||
"PID_OUTPUT_INVALID",
|
||||
"PID_UPDATE",
|
||||
f"PID 输出不是有限数值: {opening!r}",
|
||||
)
|
||||
opening = self._bounded_opening(opening)
|
||||
|
||||
position = opening_to_motor_position(
|
||||
opening,
|
||||
self.motor_open_position,
|
||||
self.motor_closed_position,
|
||||
)
|
||||
self._write_motor_position(position, opening)
|
||||
|
||||
self.last_flow_slm = flow
|
||||
self.last_pressure_kpa = pressure
|
||||
self.last_opening_pct = opening
|
||||
self.last_motor_position = position
|
||||
return ControlStepResult(
|
||||
timestamp=time.time(),
|
||||
target_flow_slm=self.target_flow_slm,
|
||||
measured_flow_slm=flow,
|
||||
pressure_kpa=pressure,
|
||||
error_slm=self.target_flow_slm - flow,
|
||||
opening_pct=opening,
|
||||
motor_position=position,
|
||||
actual_dt_s=actual_dt,
|
||||
)
|
||||
|
||||
def safe_close(self, reason="UNSPECIFIED"):
|
||||
"""尝试把阀门置于关闭端；失败时返回 False 并详细记录。"""
|
||||
try:
|
||||
success = bool(
|
||||
self.hardware.set_motor_position(
|
||||
self.motor_closed_position,
|
||||
channel=self.motor_channel,
|
||||
)
|
||||
)
|
||||
except Exception as exc:
|
||||
self._log(
|
||||
"critical",
|
||||
"SAFE_CLOSE_FAILED reason=%s exception=%s context=%s",
|
||||
reason,
|
||||
repr(exc),
|
||||
self._context(),
|
||||
)
|
||||
return False
|
||||
|
||||
if success:
|
||||
self.last_opening_pct = 0.0
|
||||
self.last_motor_position = self.motor_closed_position
|
||||
self._log("warning", "阀门已安全关闭，原因=%s", reason)
|
||||
return True
|
||||
|
||||
self._log(
|
||||
"critical",
|
||||
"SAFE_CLOSE_FAILED reason=%s hardware_return=False context=%s",
|
||||
reason,
|
||||
self._context(),
|
||||
)
|
||||
return False
|
||||
|
||||
def _read_flow(self):
|
||||
try:
|
||||
value = self.hardware.get_flow(self.flow_channel)
|
||||
except Exception as exc:
|
||||
self._register_read_failure("FLOW", exc)
|
||||
return None
|
||||
if value is None or not self._is_finite_number(value):
|
||||
self._register_read_failure("FLOW", f"invalid value: {value!r}")
|
||||
return None
|
||||
self.flow_failure_count = 0
|
||||
return float(value)
|
||||
|
||||
def _read_pressure(self):
|
||||
if self.pressure_channel is None:
|
||||
return None
|
||||
try:
|
||||
value = self.hardware.get_pressure(self.pressure_channel)
|
||||
except Exception as exc:
|
||||
self._register_read_failure("PRESSURE", exc)
|
||||
return None
|
||||
if value is None or not self._is_finite_number(value):
|
||||
self._register_read_failure("PRESSURE", f"invalid value: {value!r}")
|
||||
return None
|
||||
self.pressure_failure_count = 0
|
||||
return float(value)
|
||||
|
||||
def _register_read_failure(self, sensor, detail):
|
||||
if sensor == "FLOW":
|
||||
self.flow_failure_count += 1
|
||||
count = self.flow_failure_count
|
||||
limit = self.max_consecutive_flow_failures
|
||||
else:
|
||||
self.pressure_failure_count += 1
|
||||
count = self.pressure_failure_count
|
||||
limit = self.max_consecutive_pressure_failures
|
||||
|
||||
self._log(
|
||||
"error",
|
||||
"%s_READ_FAILED count=%d/%d detail=%r context=%s",
|
||||
sensor,
|
||||
count,
|
||||
limit,
|
||||
detail,
|
||||
self._context(),
|
||||
)
|
||||
if count >= limit:
|
||||
self._trip(
|
||||
f"{sensor}_READ_FAILED",
|
||||
f"READ_{sensor}",
|
||||
f"{sensor} 连续读取失败 {count} 次",
|
||||
{"failure_detail": repr(detail), "failure_count": count},
|
||||
)
|
||||
|
||||
def _write_motor_position(self, position, opening):
|
||||
try:
|
||||
success = self.hardware.set_motor_position(
|
||||
position,
|
||||
channel=self.motor_channel,
|
||||
)
|
||||
except Exception as exc:
|
||||
self._trip(
|
||||
"MOTOR_COMMAND_FAILED",
|
||||
"WRITE_MOTOR",
|
||||
"下发电机行程时发生异常",
|
||||
{
|
||||
"exception": repr(exc),
|
||||
"requested_opening_pct": opening,
|
||||
"requested_motor_position": position,
|
||||
},
|
||||
)
|
||||
if not success:
|
||||
self._trip(
|
||||
"MOTOR_COMMAND_FAILED",
|
||||
"WRITE_MOTOR",
|
||||
"MT2-AM8 返回电机位置写入失败",
|
||||
{
|
||||
"requested_opening_pct": opening,
|
||||
"requested_motor_position": position,
|
||||
},
|
||||
)
|
||||
|
||||
def _trip(self, code, stage, message, extra_context=None):
|
||||
context = self._context()
|
||||
context.update(extra_context or {})
|
||||
self.faulted = True
|
||||
self.running = False
|
||||
close_ok = self.safe_close(code)
|
||||
context["safe_close_success"] = close_ok
|
||||
fault = FlowControlFault(code, stage, message, context)
|
||||
self._log("critical", "%s", fault)
|
||||
raise fault
|
||||
|
||||
def _held_result(self, actual_dt, pressure, status):
|
||||
return ControlStepResult(
|
||||
timestamp=time.time(),
|
||||
target_flow_slm=self.target_flow_slm,
|
||||
measured_flow_slm=None,
|
||||
pressure_kpa=pressure,
|
||||
error_slm=None,
|
||||
opening_pct=self.last_opening_pct,
|
||||
motor_position=self.last_motor_position,
|
||||
actual_dt_s=actual_dt,
|
||||
status=status,
|
||||
)
|
||||
|
||||
def _context(self):
|
||||
return {
|
||||
"target_flow_slm": self.target_flow_slm,
|
||||
"last_valid_flow_slm": self.last_flow_slm,
|
||||
"last_pressure_kpa": self.last_pressure_kpa,
|
||||
"opening_pct": self.last_opening_pct,
|
||||
"motor_position": self.last_motor_position,
|
||||
"device_connected": bool(getattr(self.hardware, "connected", False)),
|
||||
"flow_failure_count": self.flow_failure_count,
|
||||
"pressure_failure_count": self.pressure_failure_count,
|
||||
}
|
||||
|
||||
def _bounded_opening(self, opening):
|
||||
value = float(opening)
|
||||
if not math.isfinite(value):
|
||||
raise ValueError("阀门开度必须是有限数值")
|
||||
return max(0.0, min(100.0, value))
|
||||
|
||||
def _log(self, level, message, *args):
|
||||
if self.logger is not None:
|
||||
getattr(self.logger, level)(message, *args)
|
||||
|
||||
@staticmethod
|
||||
def _is_finite_number(value):
|
||||
try:
|
||||
return math.isfinite(float(value))
|
||||
except (TypeError, ValueError):
|
||||
return False
|
||||
@@ -0,0 +1,213 @@
|
||||
"""气体流量闭环控制程序入口。
|
||||
|
||||
示例：
|
||||
python main.py --target 50
|
||||
|
||||
程序启动后会先下发关闭位置，再开始闭环。Ctrl+C、控制故障或其他异常都会
|
||||
再次尝试关阀并断开 MT2-AM8。
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import logging
|
||||
from pathlib import Path
|
||||
import sys
|
||||
import time
|
||||
|
||||
import config
|
||||
from controllers import IncrementalPID
|
||||
from flow_control import FlowControlFault, FlowControlLoop
|
||||
|
||||
|
||||
def build_logger():
|
||||
log_dir = Path(__file__).resolve().parent / config.LOG_DIRECTORY
|
||||
log_dir.mkdir(parents=True, exist_ok=True)
|
||||
log_path = log_dir / config.LOG_FILE_NAME
|
||||
|
||||
logger = logging.getLogger("flow_control")
|
||||
logger.setLevel(logging.INFO)
|
||||
logger.handlers.clear()
|
||||
|
||||
formatter = logging.Formatter(
|
||||
"%(asctime)s.%(msecs)03d %(levelname)s %(message)s",
|
||||
datefmt="%Y-%m-%d %H:%M:%S",
|
||||
)
|
||||
file_handler = logging.FileHandler(log_path, encoding="utf-8")
|
||||
file_handler.setFormatter(formatter)
|
||||
logger.addHandler(file_handler)
|
||||
|
||||
console_handler = logging.StreamHandler()
|
||||
console_handler.setFormatter(formatter)
|
||||
logger.addHandler(console_handler)
|
||||
return logger
|
||||
|
||||
|
||||
def build_control_loop(hardware, logger):
|
||||
pid = IncrementalPID(
|
||||
kp=config.PID_KP,
|
||||
ki=config.PID_KI,
|
||||
kd=config.PID_KD,
|
||||
dt=config.CONTROL_PERIOD_S,
|
||||
out_min=config.OPENING_MIN_PCT,
|
||||
out_max=config.OPENING_MAX_PCT,
|
||||
output_rate_limit=config.MAX_OPENING_RATE_PCT_S,
|
||||
)
|
||||
return FlowControlLoop(
|
||||
hardware=hardware,
|
||||
pid=pid,
|
||||
period_s=config.CONTROL_PERIOD_S,
|
||||
flow_channel=config.FLOW_INPUT_CHANNEL,
|
||||
pressure_channel=config.PRESSURE_INPUT_CHANNEL,
|
||||
motor_channel=config.MOTOR_OUTPUT_CHANNEL,
|
||||
motor_open_position=config.MOTOR_OPEN_POSITION,
|
||||
motor_closed_position=config.MOTOR_CLOSED_POSITION,
|
||||
target_min_slm=config.TARGET_FLOW_MIN_SLM,
|
||||
target_max_slm=config.TARGET_FLOW_MAX_SLM,
|
||||
zero_flow_threshold_slm=config.ZERO_FLOW_THRESHOLD_SLM,
|
||||
flow_valid_min_slm=config.FLOW_VALID_MIN_SLM,
|
||||
flow_valid_max_slm=config.FLOW_VALID_MAX_SLM,
|
||||
max_pressure_kpa=config.MAX_PRESSURE_KPA,
|
||||
max_control_dt_s=config.MAX_CONTROL_DT_S,
|
||||
max_consecutive_flow_failures=config.MAX_CONSECUTIVE_FLOW_FAILURES,
|
||||
max_consecutive_pressure_failures=(
|
||||
config.MAX_CONSECUTIVE_PRESSURE_FAILURES
|
||||
),
|
||||
logger=logger,
|
||||
)
|
||||
|
||||
|
||||
def parse_args(argv=None):
|
||||
parser = argparse.ArgumentParser(description="MT2-AM8 气体流量 PID 控制")
|
||||
parser.add_argument(
|
||||
"--target",
|
||||
type=float,
|
||||
required=True,
|
||||
help=(
|
||||
"目标流量 SLM，允许范围 "
|
||||
f"{config.TARGET_FLOW_MIN_SLM}~{config.TARGET_FLOW_MAX_SLM}"
|
||||
),
|
||||
)
|
||||
return parser.parse_args(argv)
|
||||
|
||||
|
||||
def format_status(result):
|
||||
flow_text = (
|
||||
"--" if result.measured_flow_slm is None
|
||||
else f"{result.measured_flow_slm:.2f}"
|
||||
)
|
||||
pressure_text = (
|
||||
"--" if result.pressure_kpa is None
|
||||
else f"{result.pressure_kpa:.2f}"
|
||||
)
|
||||
return (
|
||||
f"状态={result.status} 目标={result.target_flow_slm:.2f} SLM "
|
||||
f"实际={flow_text} SLM 压力={pressure_text} kPa "
|
||||
f"开度={result.opening_pct:.2f}% "
|
||||
f"行程={result.motor_position:.1f} dt={result.actual_dt_s:.3f}s"
|
||||
)
|
||||
|
||||
|
||||
def run(target_flow_slm):
|
||||
config.validate_config()
|
||||
logger = build_logger()
|
||||
try:
|
||||
from PcControl import MT2AM8Client
|
||||
except ModuleNotFoundError as exc:
|
||||
if exc.name and exc.name.startswith("pymodbus"):
|
||||
logger.critical(
|
||||
"缺少 PcControl.py 所需的 pymodbus；请在运行本工程的 Python "
|
||||
"环境中安装与现有硬件代码兼容的 pymodbus 版本"
|
||||
)
|
||||
return 6
|
||||
raise
|
||||
|
||||
hardware = MT2AM8Client(
|
||||
host=config.MT2AM8_HOST,
|
||||
port=config.MT2AM8_PORT,
|
||||
slave_id=config.MT2AM8_SLAVE_ID,
|
||||
pressure_range=config.PRESSURE_RANGE_KPA,
|
||||
flow_range=config.FLOW_METER_RANGE_SLM,
|
||||
)
|
||||
controller = build_control_loop(hardware, logger)
|
||||
controller.set_target_flow(target_flow_slm)
|
||||
|
||||
connected = False
|
||||
exit_code = 0
|
||||
try:
|
||||
logger.info(
|
||||
"正在连接 MT2-AM8 %s:%s，目标流量=%.3f SLM",
|
||||
config.MT2AM8_HOST,
|
||||
config.MT2AM8_PORT,
|
||||
target_flow_slm,
|
||||
)
|
||||
connected = bool(hardware.connect())
|
||||
if not connected:
|
||||
raise FlowControlFault(
|
||||
"DEVICE_CONNECT_FAILED",
|
||||
"STARTUP",
|
||||
"无法连接 MT2-AM8",
|
||||
{
|
||||
"host": config.MT2AM8_HOST,
|
||||
"port": config.MT2AM8_PORT,
|
||||
"slave_id": config.MT2AM8_SLAVE_ID,
|
||||
},
|
||||
)
|
||||
|
||||
if not controller.safe_close("STARTUP"):
|
||||
raise FlowControlFault(
|
||||
"SAFE_CLOSE_FAILED",
|
||||
"STARTUP",
|
||||
"启动前无法确认阀门关闭命令已成功写入",
|
||||
{"motor_closed_position": config.MOTOR_CLOSED_POSITION},
|
||||
)
|
||||
|
||||
controller.start(initial_opening=0.0)
|
||||
logger.info("闭环控制已启动；按 Ctrl+C 停止")
|
||||
|
||||
next_deadline = time.perf_counter()
|
||||
next_status_time = next_deadline
|
||||
while True:
|
||||
now = time.perf_counter()
|
||||
result = controller.step(now=now)
|
||||
|
||||
if now >= next_status_time or result.status != "OK":
|
||||
logger.info(format_status(result))
|
||||
next_status_time = now + config.STATUS_PRINT_PERIOD_S
|
||||
|
||||
next_deadline += config.CONTROL_PERIOD_S
|
||||
sleep_s = next_deadline - time.perf_counter()
|
||||
if sleep_s > 0:
|
||||
time.sleep(sleep_s)
|
||||
else:
|
||||
# 丢弃已经错过的节拍，避免连续快速补跑 PID。
|
||||
next_deadline = time.perf_counter()
|
||||
|
||||
except KeyboardInterrupt:
|
||||
logger.info("收到 Ctrl+C，正在停止控制")
|
||||
except FlowControlFault as exc:
|
||||
exit_code = 2
|
||||
logger.critical("控制故障：%s", exc)
|
||||
except Exception:
|
||||
exit_code = 3
|
||||
logger.exception("未处理异常，系统将进入安全关闭")
|
||||
finally:
|
||||
if connected:
|
||||
close_ok = controller.safe_close("PROGRAM_EXIT")
|
||||
if not close_ok:
|
||||
exit_code = max(exit_code, 4)
|
||||
logger.critical("程序退出时安全关阀失败，请立即人工检查")
|
||||
try:
|
||||
hardware.disconnect()
|
||||
except Exception:
|
||||
exit_code = max(exit_code, 5)
|
||||
logger.exception("断开 MT2-AM8 时发生异常")
|
||||
logger.info("程序结束，退出码=%d", exit_code)
|
||||
return exit_code
|
||||
|
||||
|
||||
def main(argv=None):
|
||||
args = parse_args(argv)
|
||||
return run(args.target)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
sys.exit(main())
|
||||
Reference in New Issue
Block a user