diff --git a/components/pid/temp_controller.py b/components/pid/temp_controller.py index 0b34212..ac769cb 100644 --- a/components/pid/temp_controller.py +++ b/components/pid/temp_controller.py @@ -1,64 +1,18 @@ from components.plant.pot import Pot from matplotlib.pyplot import plot, figure, subplot, grid, show, legend -from components import APid -from components.pid import Pid, Kalman +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(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 +class TempController(TempControllerBase): + def __init__(self, dt, params, model_params=None): + TempControllerBase.__init__(self, dt, params, model_params) self.kalman = Kalman(dt, params['Kalman']) - 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 set_theta_ist(self, value): - self.theta_ist_set = value - if self.is_startup: - self.is_startup = False - self.kalman.initial((value, 0)) - - 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 init_kalman(self, value): + self.kalman.initial((value, 0)) def process(self): # Process Kalman @@ -83,48 +37,6 @@ class TempController(APid): self.process_fsm(diff) self.process_pid(theta_err, heatrate_err) - def process_fsm(self, diff): - # Process state - 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)) - - 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() - - def get_power(self): - return self.y - if __name__ == '__main__': dt = 1.0 @@ -196,4 +108,3 @@ if __name__ == '__main__': show() print("End of program") - diff --git a/components/pid/temp_controller_base.py b/components/pid/temp_controller_base.py new file mode 100644 index 0000000..828c6e8 --- /dev/null +++ b/components/pid/temp_controller_base.py @@ -0,0 +1,112 @@ +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, model_params=None): + 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.model_params = model_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 diff --git a/components/pid/temp_controller_smith.py b/components/pid/temp_controller_smith.py index 6d33516..b2943fd 100755 --- a/components/pid/temp_controller_smith.py +++ b/components/pid/temp_controller_smith.py @@ -1,73 +1,42 @@ from components.plant.pot import Pot from matplotlib.pyplot import plot, figure, subplot, grid, show, legend -from components import APid -from components.pid import Pid, Kalman +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(APid): +class TempController(TempControllerBase): def __init__(self, dt, params, model_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.model_params = model_params + TempControllerBase.__init__(self, dt, params, model_params) self.kalman_model = Kalman(dt, params['Kalman']) self.kalman_model_delay = Kalman(dt, params['Kalman']) self.kalman_plant = Kalman(dt, params['Kalman']) - 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.model = Pot(dt, model_params) - self.is_startup = True + + self.theta_ist_plant = 0 + self.dtheta_ist_plant = 0 + self.theta_ist_model = 0 + self.dtheta_ist_model = 0 + self.theta_ist_model_delay = 0 + self.dtheta_ist_model_delay = 0 def set_model_power(self, power): self.model.set_power(power) - def set_theta_ist(self, value): - self.theta_ist_set = value - if self.is_startup: - self.is_startup = False - self.kalman_model.initial((value, 0)) - self.kalman_model_delay.initial((value, 0)) - self.kalman_plant.initial((value, 0)) + def init_kalman(self, value): + self.kalman_model.initial((value, 0)) + self.kalman_model_delay.initial((value, 0)) + self.kalman_plant.initial((value, 0)) - def get_theta_ist(self): - return self.theta_ist + def on_state_entered(self, state): + if state == States.HEAT: + self.model.initial(self.theta_ist) + self.kalman_model.initial((self.theta_ist, 0)) + self.kalman_model_delay.initial((self.theta_ist, 0)) - 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 post_pid(self): + self.model.process() def process(self): # Process Kalman of Plant @@ -114,54 +83,6 @@ class TempController(APid): self.process_fsm(diff) self.process_pid(theta_err, heatrate_err) - def process_fsm(self, diff): - # Process state - 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)) - if state_next == States.HEAT: - self.model.initial(self.theta_ist) - self.kalman_model.initial((self.theta_ist, 0)) - self.kalman_model_delay.initial((self.theta_ist, 0)) - - 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.model.process() - - def get_power(self): - return self.y - if __name__ == '__main__': dt = 1.0 @@ -246,4 +167,3 @@ if __name__ == '__main__': show() print("End of program") -