commit 84b2bdb35d0595e91bc8440b4a35234255e88a7a Author: rangang Date: Wed Aug 12 17:40:45 2026 +0800 first commit diff --git a/111.py b/111.py new file mode 100644 index 0000000..9309134 --- /dev/null +++ b/111.py @@ -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 \ No newline at end of file diff --git a/PcControl.py b/PcControl.py new file mode 100644 index 0000000..59fd2ea --- /dev/null +++ b/PcControl.py @@ -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("程序结束。") diff --git a/__pycache__/PcControl.cpython-312.pyc b/__pycache__/PcControl.cpython-312.pyc new file mode 100644 index 0000000..d3305fb Binary files /dev/null and b/__pycache__/PcControl.cpython-312.pyc differ diff --git a/__pycache__/config.cpython-312.pyc b/__pycache__/config.cpython-312.pyc new file mode 100644 index 0000000..55ccef0 Binary files /dev/null and b/__pycache__/config.cpython-312.pyc differ diff --git a/__pycache__/controllers.cpython-312.pyc b/__pycache__/controllers.cpython-312.pyc new file mode 100644 index 0000000..2ffdeb2 Binary files /dev/null and b/__pycache__/controllers.cpython-312.pyc differ diff --git a/__pycache__/flow_control.cpython-312.pyc b/__pycache__/flow_control.cpython-312.pyc new file mode 100644 index 0000000..89382ed Binary files /dev/null and b/__pycache__/flow_control.cpython-312.pyc differ diff --git a/config.py b/config.py new file mode 100644 index 0000000..e5dadf2 --- /dev/null +++ b/config.py @@ -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") + diff --git a/controllers (2).py b/controllers (2).py new file mode 100644 index 0000000..cd007dc --- /dev/null +++ b/controllers (2).py @@ -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 diff --git a/controllers.py b/controllers.py new file mode 100644 index 0000000..6f92984 --- /dev/null +++ b/controllers.py @@ -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) diff --git a/flow_control.py b/flow_control.py new file mode 100644 index 0000000..e5466a2 --- /dev/null +++ b/flow_control.py @@ -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 diff --git a/main.py b/main.py new file mode 100644 index 0000000..642ba80 --- /dev/null +++ b/main.py @@ -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()) diff --git a/问题.md b/问题.md new file mode 100644 index 0000000..e3ef3c1 --- /dev/null +++ b/问题.md @@ -0,0 +1,3 @@ +1.PcControl.py中set_motor_position方法对于电机行程到模拟量的映射是否考虑了volthege_min不为0的情况？原版未考虑，现版暂时考虑了进去 +2.最大行程是1000还是1062.5？PcControl.py中是1000，而controllers.py中是1062.5 +3.init_v预置初始阀门开度是否有用到？read_motor_position读取电机行程，现在是否能做到？ \ No newline at end of file