329 lines
12 KiB
Python
329 lines
12 KiB
Python
# control_engine.py
|
||
"""控制引擎：管理控制主循环，支持 PID / RL / MANUAL 三种模式。
|
||
|
||
纯业务逻辑，无 UI 依赖。通过回调与 UI 层通信。
|
||
|
||
设计原则：所有操作在主线程执行（QTimer 驱动），避免 Cython 编译后
|
||
在 PyInstaller 子线程中 segfault。
|
||
"""
|
||
|
||
import time
|
||
import os
|
||
import traceback
|
||
import sys
|
||
import logging
|
||
import numpy as np
|
||
from controllers import IncrementalPID
|
||
|
||
|
||
logger = logging.getLogger("ReinLoop.ControlEngine")
|
||
|
||
|
||
class ControlEngine:
|
||
"""控制主引擎"""
|
||
|
||
def __init__(self, pid: IncrementalPID):
|
||
self.pid = pid
|
||
self._running = False
|
||
self._cycle_count = 0
|
||
self._last_tick = 0.0
|
||
|
||
# 缓存参数（start 时从 UI 获取）
|
||
self.mode = "PID"
|
||
self.flow = 0.0
|
||
self.volume = 0.0
|
||
self.target_pressure = 80.0
|
||
self.manual_valve = 0.0
|
||
self.dz = None
|
||
self.motor_max = None
|
||
self.xa_full = 1062.5
|
||
self.collect_data = False
|
||
|
||
# 压力 EMA 滤波
|
||
self.pressure_alpha = 1 # 平滑系数（0~1），越小越平滑
|
||
self._pressure_filtered = None # 滤波后的压力值
|
||
|
||
# 外部依赖
|
||
self._conn_mgr = None
|
||
self._model_mgr = None
|
||
self._data_collector = None
|
||
|
||
# RL 模式相关
|
||
self.last_target_rl = None
|
||
self.Kp_0 = 1.0
|
||
self.Ki_0 = 0.4
|
||
|
||
# 回调
|
||
self._on_log = None
|
||
self._on_display_update = None
|
||
self._on_pid_ui_update = None
|
||
self._on_started = None
|
||
self._on_stopped = None
|
||
|
||
# ---- 依赖注入 ----
|
||
def set_connection_manager(self, mgr):
|
||
self._conn_mgr = mgr
|
||
|
||
def set_model_manager(self, mgr):
|
||
self._model_mgr = mgr
|
||
|
||
def set_data_collector(self, collector):
|
||
self._data_collector = collector
|
||
|
||
# ---- 回调设置 ----
|
||
def set_log_callback(self, cb):
|
||
self._on_log = cb
|
||
|
||
def set_display_update_callback(self, cb):
|
||
self._on_display_update = cb
|
||
|
||
def set_pid_ui_update_callback(self, cb):
|
||
self._on_pid_ui_update = cb
|
||
|
||
def set_started_callback(self, cb):
|
||
self._on_started = cb
|
||
|
||
def set_stopped_callback(self, cb):
|
||
self._on_stopped = cb
|
||
|
||
def log(self, message):
|
||
if self._on_log:
|
||
self._on_log(message)
|
||
|
||
@property
|
||
def is_running(self) -> bool:
|
||
return self._running
|
||
|
||
# ---- 启动/停止 ----
|
||
def start(self):
|
||
"""启动控制循环（主线程调用）"""
|
||
self.log("正在启动控制循环...")
|
||
|
||
if not self._conn_mgr or not self._conn_mgr.is_connected():
|
||
self.log("启动失败: 请先连接压力表")
|
||
return
|
||
|
||
if self.mode == "RL":
|
||
if not self._model_mgr or not self._model_mgr.is_model_loaded():
|
||
self.log("启动失败: 模型未加载，请先选择工况并点击【加载模型】按钮")
|
||
return
|
||
|
||
# 预置初始阀位（读取当前电机位置）
|
||
# try:
|
||
# position_x = self._conn_mgr.read_motor_position()
|
||
# initial_valve = self.pid.init_v(position_x)
|
||
# self.pid.output = initial_valve
|
||
# self.log(f"预置初始阀位 {initial_valve:.1f}%")
|
||
# except Exception as e:
|
||
# self.log(f"读取初始开度失败，将使用 80% 启动: {e}")
|
||
# self.pid.output = 80.0
|
||
|
||
self.pid.output = 100.0
|
||
|
||
# 设置死区
|
||
if self.dz is not None:
|
||
self.pid.dead_area = self.dz
|
||
|
||
# RL 模式：在主线程预先完成模型预测
|
||
if self.mode == "RL":
|
||
try:
|
||
current_p = self._conn_mgr.read_pressure()
|
||
if current_p is None:
|
||
current_p = 0.0
|
||
self._rl_predict(current_p, self.target_pressure)
|
||
self.log(f"RL 初始预测: Kp={self.pid.kp:.4f}, Ki={self.pid.ki:.4f}")
|
||
except Exception as e:
|
||
self.log(f"模型调用异常，使用默认pid: {e}")
|
||
|
||
# 重置状态
|
||
self.last_target_rl = None
|
||
self._pressure_filtered = None # 复位滤波器
|
||
self._cycle_count = 0
|
||
self._last_tick = time.perf_counter()
|
||
|
||
if self._data_collector:
|
||
self._data_collector.reset()
|
||
|
||
self._running = True
|
||
|
||
if self._on_started:
|
||
self._on_started()
|
||
|
||
self.log(f"控制循环已启动 (模式: {self.mode}, 目标: {self.target_pressure} kPa)")
|
||
|
||
def control_tick(self):
|
||
"""主线程 QTimer 每次触发时调用——执行一个控制周期"""
|
||
if not self._running:
|
||
return
|
||
|
||
cycle_start = time.perf_counter()
|
||
|
||
try:
|
||
# 1. 读取当前压力（原始值）
|
||
raw_pressure = self._conn_mgr.read_pressure()
|
||
if raw_pressure is None:
|
||
self.log("读取当前压力失败，检查地址和连接")
|
||
return
|
||
|
||
# EMA 低通滤波：平滑毛刺
|
||
if self._pressure_filtered is None:
|
||
self._pressure_filtered = raw_pressure
|
||
else:
|
||
self._pressure_filtered = (self.pressure_alpha * raw_pressure
|
||
+ (1 - self.pressure_alpha) * self._pressure_filtered)
|
||
current_pressure = self._pressure_filtered
|
||
|
||
target_pressure = self.target_pressure
|
||
mode = self.mode
|
||
|
||
# 2. 根据模式计算阀门开度
|
||
if mode == "PID":
|
||
valve_opening = self._pid_step(current_pressure, target_pressure)
|
||
|
||
elif mode == "RL":
|
||
valve_opening = self._rl_step(current_pressure, target_pressure)
|
||
|
||
elif mode == "MANUAL":
|
||
valve_opening = self._manual_step()
|
||
|
||
else:
|
||
self.log("错误！未知控制模式")
|
||
valve_opening = 0.0
|
||
|
||
# 3. 数据采集
|
||
if self.collect_data and self._data_collector:
|
||
self._data_collector.record_step(
|
||
cycle_count=self._cycle_count,
|
||
current_pressure=current_pressure,
|
||
target_pressure=target_pressure,
|
||
valve_opening=valve_opening,
|
||
kp=self.pid.kp, ki=self.pid.ki, kd=self.pid.kd,
|
||
q_in=self.flow, v=self.volume
|
||
)
|
||
|
||
# 4. 更新 UI 显示
|
||
if self._on_display_update:
|
||
self._on_display_update(current_pressure, target_pressure, valve_opening)
|
||
|
||
self._cycle_count += 1
|
||
|
||
# 5. 周期精确计时：若本周期用时不满 dt，sleep 补足
|
||
elapsed = time.perf_counter() - cycle_start
|
||
dt = self.pid.dt
|
||
# dt = 0.2
|
||
if elapsed < dt:
|
||
time.sleep(dt - elapsed)
|
||
|
||
# 6. 记录实际周期时长
|
||
now = time.perf_counter()
|
||
tick_time = now - cycle_start
|
||
# print(f"本周期用时 {tick_time*1000:.1f}ms (目标 {dt*1000:.0f}ms)")
|
||
self._last_tick = now
|
||
|
||
except Exception as e:
|
||
# control_tick 原本会捕获异常，因此异常不会进入 main.py 的
|
||
# sys.excepthook。这里必须主动把完整 traceback 打到控制台。
|
||
err_detail = traceback.format_exc()
|
||
|
||
print("\n" + "=" * 80, file=sys.stderr, flush=True)
|
||
print("ControlEngine.control_tick 发生异常：", file=sys.stderr, flush=True)
|
||
print(err_detail, file=sys.stderr, flush=True)
|
||
print("=" * 80, file=sys.stderr, flush=True)
|
||
|
||
# main.py 已配置控制台和文件日志；这里会同步写入 logs 目录。
|
||
logger.error("控制周期错误：\n%s", err_detail)
|
||
|
||
# UI 中保留一行简要信息，避免多行文本被控件截断。
|
||
self.log(
|
||
f"控制周期错误: {type(e).__name__}: {e}；"
|
||
f"完整 traceback 请看运行控制台或 logs 日志"
|
||
)
|
||
|
||
def stop(self):
|
||
"""停止控制循环"""
|
||
self._running = False
|
||
|
||
if self._data_collector:
|
||
self._data_collector.finalize_and_upload(self.flow, self.volume)
|
||
|
||
self.log("控制循环已停止")
|
||
|
||
if self._on_stopped:
|
||
self._on_stopped()
|
||
|
||
# ---- PID 模式 ----
|
||
def _pid_step(self, current_pressure, target_pressure):
|
||
"""PID 控制单步"""
|
||
self.pid.update_pressure_values(current_pressure, target_pressure)
|
||
valve_opening = self.pid.update()
|
||
xa = self.xa_full * (100 - valve_opening) / 100
|
||
self._conn_mgr.set_motor_position(xa)
|
||
return valve_opening
|
||
|
||
# ---- RL 模式 ----
|
||
def _rl_step(self, current_pressure, target_pressure):
|
||
"""RL 增强控制单步"""
|
||
# 跟踪目标压力变化，触发 RL 重预测
|
||
if self.last_target_rl is None:
|
||
self.last_target_rl = target_pressure
|
||
elif self.last_target_rl != target_pressure:
|
||
self.last_target_rl = target_pressure
|
||
try:
|
||
self._rl_predict(current_pressure, target_pressure)
|
||
except Exception as e:
|
||
self.log(f"模型调用异常，使用默认pid: {e}")
|
||
|
||
# 检查高级设置中的单步限幅是否有填入，如果有，使用填入的值；如果没有，使用默认函数
|
||
# if self.motor_max is not None:
|
||
# self.pid.set_du_max(self.motor_max * self.pid.dt)
|
||
# else:
|
||
# self.pid.get_du_max(target_pressure)
|
||
self.pid.update_pressure_values(current_pressure, target_pressure)
|
||
if self.motor_max is not None:
|
||
du_max = self.motor_max * self.pid.dt
|
||
else:
|
||
du_max = None
|
||
# PID 计算
|
||
valve_opening = self.pid.update(du_max)
|
||
|
||
# 位置换算（考虑死区）
|
||
xa = self.pid.dead_area + (100 - valve_opening) * (self.xa_full - self.pid.dead_area) / 100
|
||
self._conn_mgr.set_motor_position(xa)
|
||
|
||
return valve_opening
|
||
|
||
def _rl_predict(self, current_p, target_p):
|
||
"""执行 RL 模型预测并更新 PID 参数（只在主线程调用）"""
|
||
model = self._model_mgr.rl_model
|
||
if model is None:
|
||
print("[RL] 错误: rl_model 为 None，跳过预测")
|
||
return
|
||
|
||
obs = np.array([
|
||
self.flow / 100,
|
||
current_p / 100,
|
||
(target_p - current_p) / 100
|
||
], dtype=np.float32)
|
||
print(f"[RL] 预测 obs={obs}", flush=True)
|
||
action, _ = model.predict(obs, deterministic=True)
|
||
print(f"[RL] model.predict 完成, action={action}")
|
||
|
||
action_space = model.action_space
|
||
Kp_0 = float(action_space.high[0])
|
||
Ki_0 = float(action_space.high[1])
|
||
kp = float(Kp_0 + action[0])
|
||
ki = float(Ki_0 + action[1])
|
||
|
||
self.Kp_0 = Kp_0
|
||
self.Ki_0 = Ki_0
|
||
self.pid.update_parameters(kp, ki, self.pid.kd)
|
||
print(f"[RL] PID 参数已更新: Kp={kp:.4f}, Ki={ki:.4f}")
|
||
if self._on_pid_ui_update:
|
||
self._on_pid_ui_update(kp, ki, self.pid.kd)
|
||
|
||
# ---- MANUAL 模式 ----
|
||
def _manual_step(self):
|
||
"""手动模式单步"""
|
||
xa = self.pid.dead_area + (100 - self.manual_valve) * (self.xa_full - self.pid.dead_area) / 100
|
||
self._conn_mgr.set_motor_position(xa)
|
||
return self.manual_valve |