146 lines
4.6 KiB
Python
146 lines
4.6 KiB
Python
# 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
|