Extract FSM logic from TempControllerBase into TempControllerFsm
States, DEFAULT_THRESHOLDS, the three PIDs, FSM state vars, process_fsm(), on_state_entered(), set_enabled(), and is_holding() move to a new TempControllerFsm class in temp_controller_fsm.py. TempControllerBase inherits from both APid and TempControllerFsm; States is re-exported so existing imports in temp_controller_smith.py are unaffected. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01Tqxrk8uj4M3w3d3eXm3xK8
This commit is contained in:
Claude Sonnet 4.6
@@ -1,245 +1,130 @@
|
||||
from components import APid
|
||||
from components.pid.pid import Pid
|
||||
import enum
|
||||
from components.pid.temp_controller_fsm import TempControllerFsm, States, DEFAULT_THRESHOLDS
|
||||
|
||||
DEFAULT_THRESHOLDS = {
|
||||
"HoldHeat": 1.0,
|
||||
# HoldCool used to be 0.1 ("eagerly give up and coast" made sense back
|
||||
# when COOL meant nothing more than going idle - it's an active state
|
||||
# with its own PID now, so a threshold this tight relative to a real
|
||||
# heater's discrete power steps/sensor noise causes the system to
|
||||
# chatter in and out of it every tick, resetting both pid_hold's and
|
||||
# pid_cool's integrators each time and never letting either actually
|
||||
# converge. Symmetric with the others instead.
|
||||
"HoldCool": 1.0,
|
||||
"HeatHold": 1.0,
|
||||
"HeatCool": 1.0,
|
||||
"CoolHold": 1.0,
|
||||
"CoolHeat": 1.0,
|
||||
}
|
||||
|
||||
class States(enum.Enum):
|
||||
INIT = -1,
|
||||
IDLE = 0,
|
||||
HEAT = 1,
|
||||
HOLD = 2,
|
||||
COOL = 3
|
||||
class TempControllerBase(APid, TempControllerFsm):
|
||||
|
||||
class TempControllerBase(APid):
|
||||
def __init__(self, dt):
|
||||
APid.__init__(self)
|
||||
TempControllerFsm.__init__(self, 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
|
||||
# None until set_params() is called - mirrors Pid's own
|
||||
# params/set_params() (components/pid/pid.py), whose process()
|
||||
# already relies on the same "None means not configured yet".
|
||||
self.params = None
|
||||
self.y = -1
|
||||
|
||||
def __init__(self, dt):
|
||||
APid.__init__(self)
|
||||
self.pid_hold = Pid(dt)
|
||||
self.pid_heat = Pid(dt)
|
||||
# Separate gains for ramping down (negative diff) - the actuator
|
||||
# (e.g. a heat-only Pot/heater) is responsible for clamping the
|
||||
# resulting negative power to whatever it's actually capable of;
|
||||
# the controller itself no longer assumes "can't cool" == "must go
|
||||
# idle".
|
||||
self.pid_cool = 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
|
||||
# None until set_params() is called - mirrors Pid's own
|
||||
# params/set_params() (components/pid/pid.py), whose process()
|
||||
# already relies on the same "None means not configured yet".
|
||||
self.params = None
|
||||
self.thresholds = None
|
||||
self.y = -1
|
||||
self.state = States.INIT
|
||||
self.is_startup = True
|
||||
# Master on/off switch: while disabled, the FSM is held in IDLE and
|
||||
# the controller tries not to drive the heater at all (output
|
||||
# forced to 0) - off by default, enabled either by the user
|
||||
# (manual mode) or by SudTask for the duration of a run.
|
||||
self.enabled = False
|
||||
def set_params(self, params):
|
||||
self.params = params
|
||||
self.thresholds = {**DEFAULT_THRESHOLDS, **params.get('Thresholds', {})}
|
||||
self.pid_hold.set_params(params['Hold'])
|
||||
self.pid_heat.set_params(params['Heat'])
|
||||
self.pid_cool.set_params(params['Cool'])
|
||||
|
||||
def set_params(self, params):
|
||||
self.params = params
|
||||
self.thresholds = {**DEFAULT_THRESHOLDS, **params.get('Thresholds', {})}
|
||||
self.pid_hold.set_params(params['Hold'])
|
||||
self.pid_heat.set_params(params['Heat'])
|
||||
self.pid_cool.set_params(params['Cool'])
|
||||
def _require_params(self):
|
||||
"""Called by each subclass's process() before doing anything else -
|
||||
without it, a missing set_params() call would otherwise only
|
||||
surface as an unhelpful "'NoneType' object is not subscriptable"
|
||||
buried inside Pid.process() (components/pid/pid.py), once
|
||||
process_pid() below gets to it."""
|
||||
if self.params is None:
|
||||
raise RuntimeError(
|
||||
"{}.process(): PID gains not set - call set_params() first".format(type(self).__name__))
|
||||
|
||||
def _require_params(self):
|
||||
"""Called by each subclass's process() before doing anything else -
|
||||
without it, a missing set_params() call would otherwise only
|
||||
surface as an unhelpful "'NoneType' object is not subscriptable"
|
||||
buried inside Pid.process() (components/pid/pid.py), once
|
||||
process_pid() below gets to it."""
|
||||
if self.params is None:
|
||||
raise RuntimeError(
|
||||
"{}.process(): PID gains not set - call set_params() first".format(type(self).__name__))
|
||||
def is_configured(self):
|
||||
"""Whether set_params() has been called - lets a caller (e.g.
|
||||
TcTask's own process loop) check upfront and skip processing
|
||||
gracefully while not yet configured, rather than relying on
|
||||
process() raising. Overridden by TempController(Smith) to also
|
||||
require its internal model's plant params/ambient."""
|
||||
return self.params is not None
|
||||
|
||||
def is_configured(self):
|
||||
"""Whether set_params() has been called - lets a caller (e.g.
|
||||
TcTask's own process loop) check upfront and skip processing
|
||||
gracefully while not yet configured, rather than relying on
|
||||
process() raising. Overridden by TempController(Smith) to also
|
||||
require its internal model's plant params/ambient."""
|
||||
return self.params is not None
|
||||
def post_pid(self):
|
||||
pass
|
||||
|
||||
def on_state_entered(self, state):
|
||||
pass
|
||||
def set_ambient_temperature(self, theta_amb):
|
||||
pass
|
||||
|
||||
def post_pid(self):
|
||||
pass
|
||||
def set_model_plant_params(self, model_params):
|
||||
pass
|
||||
|
||||
def set_ambient_temperature(self, theta_amb):
|
||||
pass
|
||||
def set_model_power(self, power):
|
||||
pass
|
||||
|
||||
def set_model_plant_params(self, model_params):
|
||||
pass
|
||||
def set_theta_ist(self, value):
|
||||
self.theta_ist_set = value
|
||||
if self.is_startup:
|
||||
self.is_startup = False
|
||||
|
||||
def set_model_power(self, power):
|
||||
pass
|
||||
def get_theta_ist(self):
|
||||
return self.theta_ist
|
||||
|
||||
def set_theta_ist(self, value):
|
||||
self.theta_ist_set = value
|
||||
if self.is_startup:
|
||||
self.is_startup = False
|
||||
def get_theta_ist_set(self):
|
||||
"""The raw sensor reading, as last pushed via set_theta_ist() -
|
||||
unlike get_theta_ist() (self.theta_ist), this is updated the
|
||||
instant a reading comes in, regardless of whether process() has
|
||||
ever actually run (e.g. before set_params()/a model-based
|
||||
controller's plant params have been configured - see SudTask.
|
||||
send_forecast(), which needs a real "current temperature" at the
|
||||
moment a Sud is loaded, before this controller may have ticked
|
||||
even once)."""
|
||||
return self.theta_ist_set
|
||||
|
||||
def get_theta_ist(self):
|
||||
return self.theta_ist
|
||||
def set_heatrate_ist(self, value):
|
||||
self.heatrate_ist_set = value
|
||||
|
||||
def get_theta_ist_set(self):
|
||||
"""The raw sensor reading, as last pushed via set_theta_ist() -
|
||||
unlike get_theta_ist() (self.theta_ist), this is updated the
|
||||
instant a reading comes in, regardless of whether process() has
|
||||
ever actually run (e.g. before set_params()/a model-based
|
||||
controller's plant params have been configured - see SudTask.
|
||||
send_forecast(), which needs a real "current temperature" at the
|
||||
moment a Sud is loaded, before this controller may have ticked
|
||||
even once)."""
|
||||
return self.theta_ist_set
|
||||
def get_heatrate_ist(self):
|
||||
return self.heatrate_ist
|
||||
|
||||
def set_heatrate_ist(self, value):
|
||||
self.heatrate_ist_set = value
|
||||
def set_theta_soll(self, value):
|
||||
self.theta_soll_set = value
|
||||
# Recompute the FSM right away against the new target, rather than
|
||||
# waiting for the next process() tick - otherwise self.state can
|
||||
# still read HOLD from the previous target for up to one tick
|
||||
# after a much-further-away one is pushed, which would make
|
||||
# is_holding() report "reached" instantly instead of once the gap
|
||||
# has actually closed.
|
||||
self.process_fsm(self.theta_soll_set - self.theta_ist)
|
||||
|
||||
def get_heatrate_ist(self):
|
||||
return self.heatrate_ist
|
||||
def get_theta_soll(self):
|
||||
return self.theta_soll
|
||||
|
||||
def set_theta_soll(self, value):
|
||||
self.theta_soll_set = value
|
||||
# Recompute the FSM right away against the new target, rather than
|
||||
# waiting for the next process() tick - otherwise self.state can
|
||||
# still read HOLD from the previous target for up to one tick
|
||||
# after a much-further-away one is pushed, which would make
|
||||
# is_holding() report "reached" instantly instead of once the gap
|
||||
# has actually closed.
|
||||
self.process_fsm(self.theta_soll_set - self.theta_ist)
|
||||
def get_theta_soll_set(self):
|
||||
return self.theta_soll_set
|
||||
|
||||
def get_theta_soll(self):
|
||||
return self.theta_soll
|
||||
def set_heatrate_soll(self, value):
|
||||
self.heatrate_soll_set = value
|
||||
|
||||
def get_theta_soll_set(self):
|
||||
return self.theta_soll_set
|
||||
def get_heatrate_soll(self):
|
||||
return self.heatrate_soll
|
||||
|
||||
def is_holding(self):
|
||||
"""Whether the FSM currently considers theta_ist close enough to
|
||||
theta_soll_set to no longer be actively heating/cooling toward
|
||||
it - the single source of truth for "is a ramp toward the
|
||||
current target done" (see tasks/sud.py's SudTask)."""
|
||||
return self.state == States.HOLD
|
||||
def get_heatrate_soll_set(self):
|
||||
return self.heatrate_soll_set
|
||||
|
||||
def set_heatrate_soll(self, value):
|
||||
self.heatrate_soll_set = value
|
||||
def process_pid(self, theta_err, heatrate_err, hold_scale=1.0):
|
||||
self.pid_hold.process(theta_err, -self.theta_ist, hold_scale)
|
||||
|
||||
def set_enabled(self, value):
|
||||
self.enabled = value
|
||||
# Only the PID actually driving y is advanced - otherwise the
|
||||
# inactive one (e.g. pid_heat while COOL has pid_cool driving)
|
||||
# would keep silently integrating against a heatrate_err that
|
||||
# isn't actually under its control, building a stale windup that
|
||||
# causes a discontinuity in y the moment it takes back over.
|
||||
if self.state == States.IDLE:
|
||||
self.y = 0
|
||||
elif self.state == States.COOL:
|
||||
self.pid_cool.process(heatrate_err, -self.heatrate_ist)
|
||||
self.y = self.pid_cool.get_y()
|
||||
else:
|
||||
self.pid_heat.process(heatrate_err, -self.heatrate_ist)
|
||||
self.y = self.pid_heat.get_y()
|
||||
|
||||
def get_heatrate_soll(self):
|
||||
return self.heatrate_soll
|
||||
self.post_pid()
|
||||
|
||||
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:
|
||||
# Wait for a real sensor reading before acting on anything,
|
||||
# regardless of enabled - avoids reacting to the bogus
|
||||
# theta_ist=0 default.
|
||||
if not self.is_startup:
|
||||
state_next = States.IDLE
|
||||
elif not self.enabled:
|
||||
state_next = States.IDLE
|
||||
elif self.state == States.IDLE:
|
||||
# Just (re-)enabled - resolve straight to HEAT/COOL/HOLD against
|
||||
# the real gap, the same threshold check the HOLD branch below
|
||||
# uses, rather than landing in HOLD and waiting for the next
|
||||
# tick to correct it: is_holding() is read synchronously within
|
||||
# this very same call chain (tasks/sud.py's SudTask.on_step_
|
||||
# changed() pushes the new step's setpoint via set_theta_soll(),
|
||||
# which calls this method directly), so an unconditional HOLD
|
||||
# here gets mistaken for "ramp already reached" and can finish
|
||||
# a freshly (re-)started step instantly - see SudTask.
|
||||
# on_process()'s is_holding() check. pid_heat/pid_cool were
|
||||
# frozen (see process_pid()) and possibly stale for as long as
|
||||
# we were disabled - start whichever one matters clean rather
|
||||
# than resuming wherever it last left off.
|
||||
self.pid_hold.reset()
|
||||
self.pid_heat.reset()
|
||||
self.pid_cool.reset()
|
||||
if diff >= self.thresholds['HoldHeat']:
|
||||
state_next = States.HEAT
|
||||
elif diff <= -self.thresholds['HoldCool']:
|
||||
state_next = States.COOL
|
||||
else:
|
||||
state_next = States.HOLD
|
||||
elif self.state == States.HOLD:
|
||||
if diff >= self.thresholds['HoldHeat']:
|
||||
state_next = States.HEAT
|
||||
self.pid_heat.reset()
|
||||
elif diff <= -self.thresholds['HoldCool']:
|
||||
state_next = States.COOL
|
||||
self.pid_cool.reset()
|
||||
elif self.state == States.HEAT:
|
||||
if diff <= -self.thresholds['HeatCool']:
|
||||
state_next = States.COOL
|
||||
self.pid_cool.reset()
|
||||
elif diff <= self.thresholds['HeatHold']:
|
||||
state_next = States.HOLD
|
||||
self.pid_hold.reset()
|
||||
elif self.state == States.COOL:
|
||||
if diff >= self.thresholds['CoolHeat']:
|
||||
state_next = States.HEAT
|
||||
self.pid_heat.reset()
|
||||
elif diff >= -self.thresholds['CoolHold']:
|
||||
state_next = States.HOLD
|
||||
self.pid_hold.reset()
|
||||
# pid_heat was frozen during COOL (see process_pid()) -
|
||||
# resume it clean rather than from whatever it last held
|
||||
# before COOL took over, which by now may be a stale fit
|
||||
# for a completely different part of the curve.
|
||||
self.pid_heat.reset()
|
||||
|
||||
if state_next != self.state:
|
||||
self.state = state_next
|
||||
self.on_state_entered(state_next)
|
||||
|
||||
def process_pid(self, theta_err, heatrate_err, hold_scale=1.0):
|
||||
self.pid_hold.process(theta_err, -self.theta_ist, hold_scale)
|
||||
|
||||
# Only the PID actually driving y is advanced - otherwise the
|
||||
# inactive one (e.g. pid_heat while COOL has pid_cool driving)
|
||||
# would keep silently integrating against a heatrate_err that
|
||||
# isn't actually under its control, building a stale windup that
|
||||
# causes a discontinuity in y the moment it takes back over.
|
||||
if self.state == States.IDLE:
|
||||
self.y = 0
|
||||
elif self.state == States.COOL:
|
||||
self.pid_cool.process(heatrate_err, -self.heatrate_ist)
|
||||
self.y = self.pid_cool.get_y()
|
||||
else:
|
||||
self.pid_heat.process(heatrate_err, -self.heatrate_ist)
|
||||
self.y = self.pid_heat.get_y()
|
||||
|
||||
self.post_pid()
|
||||
|
||||
def get_power(self):
|
||||
return self.y
|
||||
def get_power(self):
|
||||
return self.y
|
||||
|
||||
@@ -0,0 +1,125 @@
|
||||
import enum
|
||||
from components.pid.pid import Pid
|
||||
|
||||
|
||||
DEFAULT_THRESHOLDS = {
|
||||
"HoldHeat": 1.0,
|
||||
# HoldCool used to be 0.1 ("eagerly give up and coast" made sense back
|
||||
# when COOL meant nothing more than going idle - it's an active state
|
||||
# with its own PID now, so a threshold this tight relative to a real
|
||||
# heater's discrete power steps/sensor noise causes the system to
|
||||
# chatter in and out of it every tick, resetting both pid_hold's and
|
||||
# pid_cool's integrators each time and never letting either actually
|
||||
# converge. Symmetric with the others instead.
|
||||
"HoldCool": 1.0,
|
||||
"HeatHold": 1.0,
|
||||
"HeatCool": 1.0,
|
||||
"CoolHold": 1.0,
|
||||
"CoolHeat": 1.0,
|
||||
}
|
||||
|
||||
|
||||
class States(enum.Enum):
|
||||
INIT = -1,
|
||||
IDLE = 0,
|
||||
HEAT = 1,
|
||||
HOLD = 2,
|
||||
COOL = 3
|
||||
|
||||
|
||||
class TempControllerFsm:
|
||||
|
||||
def __init__(self, dt):
|
||||
self.pid_hold = Pid(dt)
|
||||
self.pid_heat = Pid(dt)
|
||||
# Separate gains for ramping down (negative diff) - the actuator
|
||||
# (e.g. a heat-only Pot/heater) is responsible for clamping the
|
||||
# resulting negative power to whatever it's actually capable of;
|
||||
# the controller itself no longer assumes "can't cool" == "must go
|
||||
# idle".
|
||||
self.pid_cool = Pid(dt)
|
||||
self.thresholds = None
|
||||
self.state = States.INIT
|
||||
self.is_startup = True
|
||||
# Master on/off switch: while disabled, the FSM is held in IDLE and
|
||||
# the controller tries not to drive the heater at all (output
|
||||
# forced to 0) - off by default, enabled either by the user
|
||||
# (manual mode) or by SudTask for the duration of a run.
|
||||
self.enabled = False
|
||||
|
||||
def on_state_entered(self, state):
|
||||
pass
|
||||
|
||||
def set_enabled(self, value):
|
||||
self.enabled = value
|
||||
|
||||
def is_holding(self):
|
||||
"""Whether the FSM currently considers theta_ist close enough to
|
||||
theta_soll_set to no longer be actively heating/cooling toward
|
||||
it - the single source of truth for "is a ramp toward the
|
||||
current target done" (see tasks/sud.py's SudTask)."""
|
||||
return self.state == States.HOLD
|
||||
|
||||
def process_fsm(self, diff):
|
||||
state_next = self.state
|
||||
if self.state == States.INIT:
|
||||
# Wait for a real sensor reading before acting on anything,
|
||||
# regardless of enabled - avoids reacting to the bogus
|
||||
# theta_ist=0 default.
|
||||
if not self.is_startup:
|
||||
state_next = States.IDLE
|
||||
elif not self.enabled:
|
||||
state_next = States.IDLE
|
||||
elif self.state == States.IDLE:
|
||||
# Just (re-)enabled - resolve straight to HEAT/COOL/HOLD against
|
||||
# the real gap, the same threshold check the HOLD branch below
|
||||
# uses, rather than landing in HOLD and waiting for the next
|
||||
# tick to correct it: is_holding() is read synchronously within
|
||||
# this very same call chain (tasks/sud.py's SudTask.on_step_
|
||||
# changed() pushes the new step's setpoint via set_theta_soll(),
|
||||
# which calls this method directly), so an unconditional HOLD
|
||||
# here gets mistaken for "ramp already reached" and can finish
|
||||
# a freshly (re-)started step instantly - see SudTask.
|
||||
# on_process()'s is_holding() check. pid_heat/pid_cool were
|
||||
# frozen (see process_pid()) and possibly stale for as long as
|
||||
# we were disabled - start whichever one matters clean rather
|
||||
# than resuming wherever it last left off.
|
||||
self.pid_hold.reset()
|
||||
self.pid_heat.reset()
|
||||
self.pid_cool.reset()
|
||||
if diff >= self.thresholds['HoldHeat']:
|
||||
state_next = States.HEAT
|
||||
elif diff <= -self.thresholds['HoldCool']:
|
||||
state_next = States.COOL
|
||||
else:
|
||||
state_next = States.HOLD
|
||||
elif self.state == States.HOLD:
|
||||
if diff >= self.thresholds['HoldHeat']:
|
||||
state_next = States.HEAT
|
||||
self.pid_heat.reset()
|
||||
elif diff <= -self.thresholds['HoldCool']:
|
||||
state_next = States.COOL
|
||||
self.pid_cool.reset()
|
||||
elif self.state == States.HEAT:
|
||||
if diff <= -self.thresholds['HeatCool']:
|
||||
state_next = States.COOL
|
||||
self.pid_cool.reset()
|
||||
elif diff <= self.thresholds['HeatHold']:
|
||||
state_next = States.HOLD
|
||||
self.pid_hold.reset()
|
||||
elif self.state == States.COOL:
|
||||
if diff >= self.thresholds['CoolHeat']:
|
||||
state_next = States.HEAT
|
||||
self.pid_heat.reset()
|
||||
elif diff >= -self.thresholds['CoolHold']:
|
||||
state_next = States.HOLD
|
||||
self.pid_hold.reset()
|
||||
# pid_heat was frozen during COOL (see process_pid()) -
|
||||
# resume it clean rather than from whatever it last held
|
||||
# before COOL took over, which by now may be a stale fit
|
||||
# for a completely different part of the curve.
|
||||
self.pid_heat.reset()
|
||||
|
||||
if state_next != self.state:
|
||||
self.state = state_next
|
||||
self.on_state_entered(state_next)
|
||||
Reference in New Issue
Block a user