jensandClaude Sonnet 4.6
1bf3929b60
TempControllerBase no longer threads model_params through (only the Smith subclass needs it, for its own Pot model and Kalman filters). Enable the Smith predictor's actual delay-compensated error term (theta_err now uses theta_ist_plant - theta_ist_model_delay + theta_ist_model instead of the plain plant reading), which is the correction this controller is named for. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
111 lines
3.0 KiB
Python
111 lines
3.0 KiB
Python
from components.plant.pot import Pot
|
|
from matplotlib.pyplot import plot, figure, subplot, grid, show, legend
|
|
from components.pid import Kalman
|
|
from components.pid.temp_controller_base import TempControllerBase
|
|
from components.pid.tc_constants import *
|
|
import numpy as np
|
|
|
|
|
|
class TempController(TempControllerBase):
|
|
def __init__(self, dt, params):
|
|
TempControllerBase.__init__(self, dt, params)
|
|
self.kalman = Kalman(dt, params['Kalman'])
|
|
|
|
def init_kalman(self, value):
|
|
self.kalman.initial((value, 0))
|
|
|
|
def process(self):
|
|
# Process Kalman
|
|
if self.use_kalman:
|
|
Z = self.kalman.process_measurement((self.theta_ist_set, 0), 0.0)
|
|
xp = self.kalman.process(Z)
|
|
self.theta_ist = xp[0, 0]
|
|
self.heatrate_ist = xp[1, 0] * 60
|
|
else:
|
|
self.theta_ist = self.theta_ist_set
|
|
self.heatrate_ist = self.heatrate_ist_set
|
|
|
|
# Compensate for max heat rate to reduce overshoot
|
|
if self.heatrate_soll_set > 0:
|
|
self.pid_hold.scale(1.0/self.heatrate_soll_set)
|
|
|
|
self.heatrate_soll = self.heatrate_soll_set * self.pid_hold.get_y()
|
|
theta_err = self.theta_soll_set - self.theta_ist
|
|
heatrate_err = self.heatrate_soll - self.heatrate_ist
|
|
|
|
diff = self.theta_soll_set - self.theta_ist
|
|
self.process_fsm(diff)
|
|
self.process_pid(theta_err, heatrate_err)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
dt = 1.0
|
|
|
|
temp_ist = 0
|
|
temp_soll = 20
|
|
ctrl = TempController(dt, Test.tc_ctrl_params)
|
|
plant = Pot(dt, Test.tc_pot_params)
|
|
_temp_ist = np.empty(0)
|
|
_temp_soll = np.empty(0)
|
|
_y = np.empty(0)
|
|
_fb = np.empty(0)
|
|
_t = np.empty(0)
|
|
_heatrate_ist_kalman = np.empty(0)
|
|
_temp_ist_kalman = np.empty(0)
|
|
a = 0.5
|
|
fb = 0
|
|
rho = 0.02
|
|
temps = [{'Temp': 20, 'Duration': 1000}, {'Temp': 40, 'Duration': 1000}, {'Temp': 50, 'Duration': 1000}, {'Temp': 60, 'Duration': 1000}, {'Temp': 70, 'Duration': 1000}, {'Temp': 80, 'Duration': 1000}, {'Temp': 78, 'Duration': 1000}]
|
|
|
|
t = 0
|
|
for temp in temps:
|
|
temp_soll = temp['Temp']
|
|
hold_counter = temp['Duration']
|
|
hold = False
|
|
ctrl.set_theta_soll(temp_soll)
|
|
ctrl.set_heatrate_soll(1.0)
|
|
|
|
while True:
|
|
if hold:
|
|
if hold_counter == 0:
|
|
break
|
|
hold_counter -= 1
|
|
plant.process()
|
|
temp_ist = plant.get_temperature() + 0.0 * np.random.randn()
|
|
ctrl.set_theta_ist(temp_ist)
|
|
ctrl.process()
|
|
|
|
y = 3500*ctrl.get_power()
|
|
power = max(0, y)
|
|
plant.set_power(power)
|
|
fb = plant.get_power()
|
|
if abs(temp_ist - temp_soll) < 0.1:
|
|
hold = True
|
|
|
|
temp_ist -= rho
|
|
_temp_ist = np.append(_temp_ist, temp_ist)
|
|
_temp_soll = np.append(_temp_soll, temp_soll)
|
|
_y = np.append(_y, y)
|
|
_fb = np.append(_fb, fb)
|
|
_t = np.append(_t, t)
|
|
_heatrate_ist_kalman = np.append(_heatrate_ist_kalman, max(-1, min(3, ctrl.heatrate_ist)))
|
|
_temp_ist_kalman = np.append(_temp_ist, ctrl.theta_ist)
|
|
t += 1
|
|
|
|
figure(1)
|
|
subplot(3, 1, 1)
|
|
plot(_t, _temp_ist, _t, _temp_soll, 'r-', linewidth=1)
|
|
legend(["ist", "soll"])
|
|
grid(True)
|
|
subplot(3, 1, 2)
|
|
plot(_t, _y, '-b', _t, _fb, '-r', linewidth=1)
|
|
legend(["y", "pot"])
|
|
grid(True)
|
|
subplot(3, 1, 3)
|
|
plot(_t, _heatrate_ist_kalman, '-b', linewidth=1)
|
|
legend(["heatrate"])
|
|
grid(True)
|
|
show()
|
|
|
|
print("End of program")
|