generated from hulk/gd32f4xx_template_cmake_vscode
Tools: PID debug toolchain from V0.1.x bring-up
test_run.py: judged closed-loop test over COM5 (fixed sequence, J1~J4
This commit is contained in:
@@ -0,0 +1,355 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
"""V1（已烧录 0.1.0）vs V2（连续 PI 候选）离线对比仿真。
|
||||
|
||||
V1 问题：err<0 硬切 0% + err>=0 只有 Kp*err，稳态维持功率全靠 Ki=0.05 慢慢填
|
||||
（填到 ~10% 要几分钟）→ 目标附近等效"P+上侧死区"继电器 → ±1℃/15~20s 极限环
|
||||
（真机 2026-10-15 截图实测形态）。
|
||||
V2：连续 PID 公式覆盖 err<0（输出钳 [0,100] 天然处理"不能制冷"），
|
||||
Ki 0.05→0.5、I_MAX 2000→200（Ki*I_MAX=100% 不变）。
|
||||
|
||||
热模型按本次实测标定：50℃ 处冷却 ~0.1 C/s、~12% 占空净升 ~0.12 C/s
|
||||
→ a≈1.9 C/s@100%，b≈0.0036/s（50℃ 维持功率 ≈5%）。
|
||||
"""
|
||||
|
||||
KP, KI, KD = 10.0, 0.05, 30.0 # V1
|
||||
KP2, KI2, KD2 = 10.0, 0.5, 30.0 # V2 候选
|
||||
RAMP_CPS = 0.8
|
||||
SLOWBAND_C, SLOWBAND_DUTY = 7.0, 25.0
|
||||
I_MAX, I_MAX2 = 2000.0, 200.0
|
||||
PWM_PERIOD_MS, PWM_STEP, PID_PERIOD_MS = 2000, 5, 500
|
||||
|
||||
A, B, TA, TAU_S = 1.9, 0.0036, 22.0, 4.0
|
||||
|
||||
|
||||
def plant_step(Tc, Ts, power, dt):
|
||||
Tc += (A * power - B * (Tc - TA)) * dt
|
||||
Ts += (Tc - Ts) / TAU_S * dt
|
||||
return Tc, Ts
|
||||
|
||||
|
||||
class Base:
|
||||
_last_power = 0.0 # 上一拍 PWM 输出（传感器加热偏置建模用）
|
||||
|
||||
def __init__(self, init_temp=None):
|
||||
self.enable = False
|
||||
self.target = 0.0
|
||||
self.duty = 0
|
||||
self.integral = 0.0
|
||||
self.phase_ms = 0
|
||||
self.divider = 0
|
||||
self.Tc = TA if init_temp is None else init_temp
|
||||
self.Ts = self.Tc
|
||||
|
||||
def tick(self):
|
||||
prev_duty = self.duty
|
||||
self.phase_ms = (self.phase_ms + 10) % PWM_PERIOD_MS
|
||||
self.divider += 10
|
||||
if self.divider >= PID_PERIOD_MS:
|
||||
self.divider = 0
|
||||
self.pid_compute()
|
||||
if self.duty > prev_duty:
|
||||
self.phase_ms = 0
|
||||
on_ms = self.duty * (PWM_PERIOD_MS // (100 // PWM_STEP))
|
||||
power = 0.0 if self.duty == 0 else (
|
||||
1.0 if self.duty >= 100 // PWM_STEP else (1.0 if self.phase_ms < on_ms else 0.0))
|
||||
self.Tc, self.Ts = plant_step(self.Tc, self.Ts, power, 0.01)
|
||||
self._last_power = power
|
||||
return power
|
||||
|
||||
def quant(self, u):
|
||||
u = max(0.0, min(100.0, u))
|
||||
return int((u + PWM_STEP / 2.0) / PWM_STEP)
|
||||
|
||||
def ramp_to(self):
|
||||
diff = self.target - self.ramp_target
|
||||
step = RAMP_CPS * (PID_PERIOD_MS / 1000.0)
|
||||
if diff > step:
|
||||
self.ramp_target += step
|
||||
elif diff < -step:
|
||||
self.ramp_target -= step
|
||||
else:
|
||||
self.ramp_target = self.target
|
||||
|
||||
def set_enable(self, on):
|
||||
if self.enable == on:
|
||||
return
|
||||
self.enable = on
|
||||
if on:
|
||||
self.ramp_target = max(0.0, self.Ts)
|
||||
self.prev_temp = self.ramp_target
|
||||
else:
|
||||
self.ramp_target = self.duty = 0
|
||||
self.integral = 0.0
|
||||
|
||||
|
||||
class V1(Base):
|
||||
"""已烧录 0.1.0：err<0 硬切 + 慢积分。"""
|
||||
|
||||
def pid_compute(self):
|
||||
t = self.Ts
|
||||
if not self.enable:
|
||||
self.duty, self.integral, self.prev_temp = 0, 0.0, t
|
||||
return
|
||||
self.ramp_to()
|
||||
err = self.ramp_target - t
|
||||
d_t = t - self.prev_temp
|
||||
if err < 0:
|
||||
self.integral = max(0.0, self.integral + err)
|
||||
self.prev_temp = t
|
||||
self.duty = 0
|
||||
return
|
||||
u_pd = KP * err - KD * d_t
|
||||
if err < SLOWBAND_C:
|
||||
u_pd = min(SLOWBAND_DUTY, u_pd)
|
||||
u_pd = max(0.0, u_pd)
|
||||
u = u_pd + KI * self.integral
|
||||
if u < 100.0:
|
||||
self.integral = min(I_MAX, self.integral + err)
|
||||
self.prev_temp = t
|
||||
self.duty = self.quant(u)
|
||||
|
||||
|
||||
class V2(Base):
|
||||
"""候选：连续 PID（无 err<0 硬切）+ 快积分 + 执行器受限不吸收。
|
||||
|
||||
吸收条件：err>0 时仅当 P/D 未被钳（0 < u_pd_raw < 当前上限，即执行器
|
||||
真实响应误差）才吸收——爬升/温和带钳位段不积分，防瞬态 windup；
|
||||
err<0 时只要 u>0 就回退（维持功率受 P 负项与 D 共同压制）。
|
||||
"""
|
||||
|
||||
def pid_compute(self):
|
||||
t = self.Ts
|
||||
if not self.enable:
|
||||
self.duty, self.integral, self.prev_temp = 0, 0.0, t
|
||||
return
|
||||
self.ramp_to()
|
||||
err = self.ramp_target - t
|
||||
d_t = t - self.prev_temp
|
||||
u_pd_raw = KP2 * err - KD2 * d_t
|
||||
cap = SLOWBAND_DUTY if err < SLOWBAND_C else 100.0
|
||||
u_pd = max(0.0, min(cap, u_pd_raw))
|
||||
u = u_pd + KI2 * self.integral
|
||||
if (err > 0 and 0.0 < u_pd_raw < cap) or (err < 0 and u > 0.0):
|
||||
self.integral = max(0.0, min(I_MAX2, self.integral + err))
|
||||
self.prev_temp = t
|
||||
self.duty = self.quant(u)
|
||||
|
||||
|
||||
class V3(Base):
|
||||
"""V3：连续 PID + 积分吸收限定在目标近旁窄带（|err|<ZONE）。
|
||||
|
||||
远处/爬升段不积分（slowband 钳位 + P 已经在工作），只有贴近目标才允许
|
||||
积分建立稳态功率——杜绝瞬态 windup；err<0 且 u>0 时照常回退。
|
||||
参数从 V3_KP/KI/KD 全局取，便于扫参。"""
|
||||
|
||||
def pid_compute(self):
|
||||
t = self.Ts
|
||||
if not self.enable:
|
||||
self.duty, self.integral, self.prev_temp = 0, 0.0, t
|
||||
return
|
||||
self.ramp_to()
|
||||
err = self.ramp_target - t
|
||||
d_t = t - self.prev_temp
|
||||
u_pd_raw = V3_KP * err - V3_KD * d_t
|
||||
cap = SLOWBAND_DUTY if err < SLOWBAND_C else 100.0
|
||||
u_pd = max(0.0, min(cap, u_pd_raw))
|
||||
u = u_pd + V3_KI * self.integral
|
||||
if (0.0 < err < V3_ZONE and 0.0 < u_pd_raw < cap) or (err < 0 and u > 0.0):
|
||||
self.integral = max(0.0, min(V3_IMAX, self.integral + err))
|
||||
self.prev_temp = t
|
||||
self.duty = self.quant(u)
|
||||
|
||||
|
||||
V3_KP, V3_KI, V3_KD, V3_ZONE, V3_IMAX = 10.0, 0.25, 30.0, 2.0, 400.0
|
||||
|
||||
|
||||
class V4(Base):
|
||||
"""V4(final) = V3 + ① err<0 时 P 负项直接抵扣积分（总量钳 [0,100]）
|
||||
② 积分窄带 2.0->0.6（单次爬升最多喂 ~I=12 ≈ 真机维持功率，防膨胀）
|
||||
③ 故障无扰恢复（<3s 且 |ΔT|<2℃ 还原暂存积分）
|
||||
真机根因（2026-10-15 DAPLink 抓证 t=397~486s）：积分爬升段膨胀到 65
|
||||
（维持仅需 ~12）→ 过线后 P 钳 0、Ki·I 顶 10~20% 慢泄 → +2℃ 飙升 →
|
||||
泄光下潜 → 再爬再喂，自持极限环（无毛刺也发生）。"""
|
||||
|
||||
def __init__(self, *a, **kw):
|
||||
super().__init__(*a, **kw)
|
||||
self.fault_beats = 0
|
||||
self.stash_I = 0.0
|
||||
self.stash_T = 0.0
|
||||
self.glitch_now = False
|
||||
self.noise = __import__("random").Random(20261015)
|
||||
|
||||
def sensor(self):
|
||||
t = -273.15 if self.glitch_now else self.Ts
|
||||
if t > -100.0:
|
||||
t += self.noise.uniform(-0.05, 0.05) # 读数噪声
|
||||
t -= 0.15 if self._last_power > 0.5 else 0.0 # 加热时鳍片梯度偏低读
|
||||
return t
|
||||
|
||||
def pid_compute(self):
|
||||
t = self.sensor()
|
||||
if t <= -273.0:
|
||||
if self.fault_beats < 255:
|
||||
self.fault_beats += 1
|
||||
if self.fault_beats == 3:
|
||||
self.stash_I = self.integral
|
||||
self.stash_T = self.prev_temp
|
||||
if self.fault_beats >= 3:
|
||||
self.duty = 0
|
||||
self.integral = 0.0
|
||||
self.prev_temp = t
|
||||
return
|
||||
if self.fault_beats >= 3:
|
||||
dur = self.fault_beats * (PID_PERIOD_MS / 1000.0)
|
||||
if dur <= 3.0 and abs(t - self.stash_T) < 2.0:
|
||||
self.integral = min(V4_IMAX, self.stash_I)
|
||||
self.fault_beats = 0
|
||||
if not self.enable:
|
||||
self.duty, self.integral, self.prev_temp = 0, 0.0, t
|
||||
return
|
||||
self.ramp_to()
|
||||
err = self.ramp_target - t
|
||||
d_t = t - self.prev_temp
|
||||
u_pd_raw = V3_KP * err - V3_KD * d_t
|
||||
cap = SLOWBAND_DUTY if err < SLOWBAND_C else 100.0
|
||||
if err >= 0.0:
|
||||
u_pd = max(0.0, min(cap, u_pd_raw))
|
||||
u = u_pd + V3_KI * self.integral
|
||||
else:
|
||||
u = u_pd_raw + V3_KI * self.integral # P 负项直接抵扣积分
|
||||
if (0.0 < err < V4_ZONE and 0.0 < u_pd_raw < cap) or (err < 0 and u > 0.0):
|
||||
self.integral = max(0.0, min(V4_IMAX, self.integral + err))
|
||||
u = max(0.0, min(100.0, u))
|
||||
self.prev_temp = t
|
||||
self.duty = self.quant(u)
|
||||
|
||||
|
||||
V4_ZONE, V4_IMAX = 0.6, 160.0
|
||||
|
||||
|
||||
class V4_OLD(Base):
|
||||
"""V3 原样（err<0 积分独撑 + 窄带 2.0 + 故障清零重建），毛刺/噪声注入对照。"""
|
||||
|
||||
def __init__(self, *a, **kw):
|
||||
super().__init__(*a, **kw)
|
||||
self.fault_beats = 0
|
||||
self.glitch_now = False
|
||||
self.noise = __import__("random").Random(20261015)
|
||||
|
||||
def sensor(self):
|
||||
t = -273.15 if self.glitch_now else self.Ts
|
||||
if t > -100.0:
|
||||
t += self.noise.uniform(-0.05, 0.05)
|
||||
t -= 0.15 if self._last_power > 0.5 else 0.0
|
||||
return t
|
||||
|
||||
def pid_compute(self):
|
||||
t = self.sensor()
|
||||
if t <= -273.0:
|
||||
if self.fault_beats < 3:
|
||||
self.fault_beats += 1
|
||||
if self.fault_beats >= 3:
|
||||
self.duty = 0
|
||||
self.integral = 0.0
|
||||
self.prev_temp = t
|
||||
return
|
||||
self.fault_beats = 0
|
||||
if not self.enable:
|
||||
self.duty, self.integral, self.prev_temp = 0, 0.0, t
|
||||
return
|
||||
self.ramp_to()
|
||||
err = self.ramp_target - t
|
||||
d_t = t - self.prev_temp
|
||||
u_pd_raw = V3_KP * err - V3_KD * d_t
|
||||
cap = SLOWBAND_DUTY if err < SLOWBAND_C else 100.0
|
||||
u_pd = max(0.0, min(cap, u_pd_raw))
|
||||
u = u_pd + V3_KI * self.integral # err<0：P 钳 0，积分独撑
|
||||
if (0.0 < err < V3_ZONE and 0.0 < u_pd_raw < cap) or (err < 0 and u > 0.0):
|
||||
self.integral = max(0.0, min(V3_IMAX, self.integral + err))
|
||||
u = max(0.0, min(100.0, u))
|
||||
self.prev_temp = t
|
||||
self.duty = self.quant(u)
|
||||
|
||||
|
||||
def run(cls, label, events, init_temp=None, end=420, plant=None, glitches=()):
|
||||
global A, B, TAU_S
|
||||
if plant:
|
||||
A, B, TAU_S = plant
|
||||
p = cls(init_temp)
|
||||
samples = []
|
||||
idx, t_sim = 0, 0.0
|
||||
events = sorted(events, key=lambda e: e[0])
|
||||
while t_sim < end:
|
||||
if idx < len(events) and t_sim >= events[idx][0] - 1e-9:
|
||||
_, act = events[idx]
|
||||
if act == "enable":
|
||||
p.set_enable(True)
|
||||
elif act == "disable":
|
||||
p.set_enable(False)
|
||||
else:
|
||||
p.target = act
|
||||
idx += 1
|
||||
if hasattr(p, "glitch_now"):
|
||||
p.glitch_now = any(gs <= t_sim < gs + gd for gs, gd in glitches)
|
||||
p.tick()
|
||||
samples.append(p.Ts)
|
||||
t_sim += 0.01
|
||||
|
||||
def stats(t0, t1, tgt):
|
||||
seg = samples[int(t0 / 0.01):int(t1 / 0.01)]
|
||||
if not seg:
|
||||
return
|
||||
reach = next((i * 0.01 for i, v in enumerate(seg) if abs(v - tgt) <= 0.3), None)
|
||||
if reach is None:
|
||||
print(f" [{t0:4.0f}-{t1:4.0f}s tgt={tgt:g}] 未进 ±0.3 带（段内 "
|
||||
f"{min(seg):.2f}~{max(seg):.2f}）")
|
||||
return
|
||||
r = int(reach / 0.01)
|
||||
post = seg[r:]
|
||||
tail = seg[max(r, len(seg) - 6000):]
|
||||
print(f" [{t0:4.0f}-{t1:4.0f}s tgt={tgt:g}] 进入±0.3带 {reach:5.1f}s | 到达后 "
|
||||
f"[{min(post):.2f},{max(post):.2f}] | 末60s [{min(tail):.2f},{max(tail):.2f}] "
|
||||
f"±{(max(tail)-min(tail))/2:.2f}")
|
||||
|
||||
print(f"\n== {label} ==")
|
||||
for i, (t0, act) in enumerate(events):
|
||||
if isinstance(act, float) or isinstance(act, int):
|
||||
t1 = events[i + 1][0] if i + 1 < len(events) else end
|
||||
stats(t0 + 2, min(t1, end), act)
|
||||
for gs, gd in glitches:
|
||||
seg = samples[int(gs / 0.01):int(min(gs + 120, end) / 0.01)]
|
||||
if seg:
|
||||
print(f" [glitch @{gs:g}s +{gd:g}s] 120s 窗内 [{min(seg):.2f},{max(seg):.2f}] "
|
||||
f"max|Δ|={max(abs(v - p.target) for v in seg):.2f}")
|
||||
return samples
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
# 基准工况（按实测斜率标定）：a=1.9 b=0.0036 tau=4
|
||||
BASE = (1.9, 0.0036, 4.0)
|
||||
# 恶劣工况：维持功率 ~11%（b=0.0075）+ 传感器滞后 8s
|
||||
HARSH = (1.9, 0.0075, 8.0)
|
||||
|
||||
import sys
|
||||
if len(sys.argv) > 1 and sys.argv[1] == "sweep":
|
||||
# 扫参：V3 参数网格，两工况各跑 50C 阶跃 + 从上方使能
|
||||
for kp, ki, kd in [(10, 0.5, 30), (10, 0.5, 10), (10, 0.5, 0),
|
||||
(15, 0.5, 30), (10, 0.25, 30), (10, 1.0, 30),
|
||||
(10, 0.5, 50), (15, 0.5, 10)]:
|
||||
V3_KP, V3_KI, V3_KD = float(kp), float(ki), float(kd)
|
||||
print(f"\n### V3 Kp={kp} Ki={ki} Kd={kd}")
|
||||
run(V3, f"HARSH 50C 阶跃", [(0, "enable"), (1, 50.0)], end=420, plant=HARSH)
|
||||
run(V3, f"HARSH 从上方使能", [(0, "enable"), (1, 50.0)], init_temp=51.0, end=420, plant=HARSH)
|
||||
run(V3, f"BASE 50C 阶跃", [(0, "enable"), (1, 50.0)], end=420, plant=BASE)
|
||||
sys.exit(0)
|
||||
|
||||
# C：镜像本次实测——从 51C（高于目标）使能，目标 50
|
||||
GL = [(200, 1.4), (320, 1.4)] # 毛刺注入：1.4s NTC 开路 ×2（真机 DAPLink 抓证形态）
|
||||
EV = [(0, "enable"), (1, 50.0)]
|
||||
run(V4_OLD, "V3(旧) 毛刺注入 tgt=50（恶劣）", EV, init_temp=51.0, end=460, plant=HARSH, glitches=GL)
|
||||
run(V4, "V4 毛刺注入 tgt=50（恶劣）", EV, init_temp=51.0, end=460, plant=HARSH, glitches=GL)
|
||||
run(V4, "V4 毛刺注入 tgt=50（基准）", EV, init_temp=51.0, end=460, plant=BASE, glitches=GL)
|
||||
# 回归：无毛刺主场景
|
||||
run(V4, "V4 50C 阶跃（恶劣）", EV, end=420, plant=HARSH)
|
||||
run(V4, "V4 30C 阶跃（基准）", [(0, "enable"), (1, 30.0)], plant=BASE)
|
||||
run(V4, "V4 50->40 下调（恶劣）", [(0, "enable"), (1, 50.0), (400, 40.0)], end=700, plant=HARSH)
|
||||
Reference in New Issue
Block a user