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>
112 lines
2.7 KiB
Python
112 lines
2.7 KiB
Python
from components import APid
|
|
from components.pid.pid import Pid
|
|
from components.pid.tc_constants import States, THRESH_HOLD_IDLE, THRESH_HOLD_HEAT, \
|
|
THRESH_IDLE_HEAT, THRESH_IDLE_HOLD, THRESH_HEAT_HOLD, THRESH_HEAT_IDLE
|
|
|
|
|
|
class TempControllerBase(APid):
|
|
def __init__(self, dt, params):
|
|
APid.__init__(self)
|
|
self.pid_hold = Pid(dt)
|
|
self.pid_rate = Pid(dt)
|
|
self.theta_ist_set = 0
|
|
self.theta_soll_set = 0
|
|
self.heatrate_ist_set = 0
|
|
self.heatrate_soll_set = 1.0
|
|
self.heatrate_soll = 1.0
|
|
self.theta_ist = 0
|
|
self.heatrate_ist = 0
|
|
self.params = params
|
|
self.y = -1
|
|
self.state = States.INIT
|
|
self.use_kalman = True
|
|
self.pid_hold.set_params(params['Hold'])
|
|
self.pid_rate.set_params(params['Heat'])
|
|
self.is_startup = True
|
|
|
|
def init_kalman(self, value):
|
|
raise NotImplementedError
|
|
|
|
def on_state_entered(self, state):
|
|
pass
|
|
|
|
def post_pid(self):
|
|
pass
|
|
|
|
def set_theta_ist(self, value):
|
|
self.theta_ist_set = value
|
|
if self.is_startup:
|
|
self.is_startup = False
|
|
self.init_kalman(value)
|
|
|
|
def get_theta_ist(self):
|
|
return self.theta_ist
|
|
|
|
def set_heatrate_ist(self, value):
|
|
self.heatrate_ist_set = value
|
|
|
|
def get_heatrate_ist(self):
|
|
return self.heatrate_ist
|
|
|
|
def set_theta_soll(self, value):
|
|
self.theta_soll_set = value
|
|
|
|
def get_theta_soll(self):
|
|
return self.theta_soll
|
|
|
|
def get_theta_soll_set(self):
|
|
return self.theta_soll_set
|
|
|
|
def set_heatrate_soll(self, value):
|
|
self.heatrate_soll_set = value
|
|
|
|
def get_heatrate_soll(self):
|
|
return self.heatrate_soll
|
|
|
|
def get_heatrate_soll_set(self):
|
|
return self.heatrate_soll_set
|
|
|
|
def process_fsm(self, diff):
|
|
state_next = self.state
|
|
if self.state == States.INIT:
|
|
if not self.is_startup:
|
|
state_next = States.IDLE
|
|
elif self.state == States.IDLE:
|
|
if diff >= THRESH_IDLE_HEAT:
|
|
state_next = States.HEAT
|
|
self.pid_rate.reset()
|
|
elif diff >= -THRESH_IDLE_HOLD:
|
|
state_next = States.HOLD
|
|
self.pid_rate.reset()
|
|
elif self.state == States.HOLD:
|
|
if diff >= THRESH_HOLD_HEAT:
|
|
state_next = States.HEAT
|
|
self.pid_rate.reset()
|
|
elif diff <= -THRESH_HOLD_IDLE:
|
|
state_next = States.IDLE
|
|
elif self.state == States.HEAT:
|
|
if diff <= -THRESH_HEAT_IDLE:
|
|
state_next = States.IDLE
|
|
elif diff <= THRESH_HEAT_HOLD:
|
|
state_next = States.HOLD
|
|
self.pid_hold.reset()
|
|
|
|
if state_next != self.state:
|
|
self.state = state_next
|
|
print("New state = {}".format(state_next))
|
|
self.on_state_entered(state_next)
|
|
|
|
def process_pid(self, theta_err, heatrate_err):
|
|
self.pid_hold.process(theta_err, -self.theta_ist)
|
|
self.pid_rate.process(heatrate_err, -self.heatrate_ist)
|
|
|
|
if self.state == States.IDLE:
|
|
self.y = 0
|
|
else:
|
|
self.y = self.pid_rate.get_y()
|
|
|
|
self.post_pid()
|
|
|
|
def get_power(self):
|
|
return self.y
|