import numpy as np import json from enum import Enum from pid import Pid from utils import Smoother, Stable import time import threading from kalman import Kalman from timer import Timer, TimerManager, TimerListener import queue ovenStates = Enum('ovenStates', 'IDLE HEAT HOLD') states = Enum('controllerStates', 'IDLE NORMAL PAUSE') commands = Enum('controllerCommands', 'RUN PAUSE ABORT STIRR_OFF STIRR_ON') class Controller(TimerListener): def __init__(self, params, plant, stirrer, recipe): print(json.dumps({'Controller': params}, indent=4, sort_keys=True)) self.sim_warp_factor = params['sim_warp_factor'] self.ovenState = ovenStates.IDLE self.plant = plant self.stirrer = stirrer self.dt = params['dt'] self.params = params self.pid_hold = Pid() self.pid_rate = Pid() self.sensorTime_v = np.empty(0) self.theta_raw_v = np.empty(0) self.theta_v = np.empty(0) self.theta_k_v = np.empty(0) self.heatrate_v = np.empty(0) self.heatrate_k_v = np.empty(0) self.time_v = np.empty(0) self.power_soll = 0 self.power_v = np.empty(0) self.power_heat_v = np.empty(0) self.power_hold_v = np.empty(0) self.error_v = np.empty(0) self.time = 0 self.report_interval = 1.0 self.thread = None self.receipe_sema = threading.Semaphore(0) self.receipe = recipe self.rasten = recipe['Rasten'] self.rasten.append(None) self.theta_sm = Smoother(1.0) self.heatrate_sm = Smoother(0.01) self.theta_err_sm = Smoother(1.0) self.heatrate_err_sm = Smoother(1.0) self.useKalman = params['useKalman'] self.kalman = Kalman(params['kalman']) self.sensor_dt = params['kalman']['dt'] self.timerMgr = TimerManager() self.processTimer = Timer("Process", self) self.rastTimer = Timer("Rast", self) self.sensorTimer = Timer("Sensor", self) self.plantTimer = Timer("Plant", self) self.reportTimer = Timer("Report", self) self.timerMgr.registerTimer(self.processTimer) self.timerMgr.registerTimer(self.rastTimer) self.timerMgr.registerTimer(self.sensorTimer) self.timerMgr.registerTimer(self.plantTimer) self.timerMgr.registerTimer(self.reportTimer) self.power_hold_offset = 200 self.power_heat_offset = 1500 self.queue = queue.Queue() self.rast_done = False self.receipe_done = False self.thread = threading.Thread(target=self.main) self.thread.start() self.fp_log = None # Stirrer self.stirrSpeedHeat = recipe['stirrSpeedHeat'] self.stirrSpeedRast = recipe['stirrSpeedRast'] self.stirrDutyRast = recipe['stirrDutyRast'] self.stirrCycleTime = recipe['stirrCycleTime'] print(json.dumps({recipe['Name'] : recipe}, indent=4, sort_keys=True)) def command(self, cmd): self.queue.put_nowait(cmd) def onTimer(self, timer): if timer == self.sensorTimer: self.sensorTime_v = np.append(self.sensorTime_v, timer.count*self.sensor_dt / 60) # Temperature self.theta_raw = self.plant.getTemperature() if timer.count == 0: # Kalman filter initial value self.kalman.initial((self.theta_raw, 0)) self.theta_sm.initial(self.theta_raw) self.theta_raw_v = np.append(self.theta_raw_v, self.theta_raw) # Process Kalman Z = self.kalman.process_measurement((self.theta_raw, 0)) xp = self.kalman.process(Z) self.theta_ist_k = xp[0, 0] self.heatrate_ist_k = xp[1, 0] * 60 self.theta_k_v = np.append(self.theta_k_v, self.theta_ist_k) self.heatrate_k_v = np.append(self.heatrate_k_v, self.heatrate_ist_k) elif timer == self.plantTimer: # Process plant self.plant.process() # Process stirrer self.stirrer.process() elif timer == self.reportTimer: self.report() elif timer == self.processTimer: rast = self.rasten[self.rast_index] if rast is not None: self.ctrl_theta_soll = rast['temp'] self.ctrl_heatrate_soll = rast['heatRate'] isFinished = self.rast(rast) if isFinished: if not self.rastTimer.isActive and not self.rast_done: rast_timer_soll = 60 * rast['time'] self.log ("Start rast timer with {} min.".format(rast_timer_soll/60)) self.rastTimer.start(rast_timer_soll / self.sim_warp_factor, mode='oneshot') self.rast_done = True else: self.receipe_done = True elif timer == self.rastTimer: self.log("Rast timer elapsed") rast = self.rasten[self.rast_index] if not rast["waitForUser"]: self.rast_index += 1 self.rast_done = False self.ovenState = ovenStates.IDLE else: self.log("Wait user input!") def log(self, s): now = time.strftime("%a, %d %b %Y %H:%M:%S", time.localtime()) print("{}: {}".format(now, s)) self.fp_log.write("{}: {}\n".format(now, s)) def report(self): self.log("Temp SOLL {} °C".format(self.ctrl_theta_soll)) self.log("Temp IST = {:0.1f} °C".format(self.ctrl_theta)) self.log("Heatrate SOLL = {:0.1f} °C/min.".format(self.ctrl_heatrate_soll)) self.log("Heatrate IST = {:0.1f} °C/min.".format(self.ctrl_heatrate)) self.log("Power = {:0.0f} W".format(self.power_soll)) if self.rastTimer.isActive: self.log("Rast remaining : {:0.1f} min.".format(self.rastTimer.elapseDuration / 60)) def wait_finished(self): self.receipe_sema.acquire() pass def main(self): self.sensorTimer.start(self.sensor_dt/self.sim_warp_factor) self.plantTimer.start(1/self.sim_warp_factor) self.stirrer.deactivate() self.plant.activate(False) run = True while(run and not self.receipe_done): self.timerMgr.process() wait = self.timerMgr.getMinTimeout(1.0/self.sim_warp_factor) try: msg = self.queue.get(block=True, timeout=wait) print ("Received: ", msg) except: continue if msg == commands.PAUSE: pass elif msg == commands.RUN: self.stirrer.activate() self.plant.activate(True) if not self.processTimer.isActive: self.fp_log = open(self.receipe['Name'] + '.log', "w") self.time = 0 self.rast_index = 0 self.reportTimer.start(self.report_interval) self.processTimer.start(self.dt / self.sim_warp_factor) # Stirrer and plant self.stirrer.setCycleTime(self.stirrCycleTime) self.receipe_done = False else: self.rast_index += 1 self.rast_done = False self.ovenState = ovenStates.IDLE elif msg == commands.ABORT: run = False elif msg == commands.STIRR_ON: self.stirrer.activate() elif msg == commands.STIRR_OFF: self.stirrer.deactivate() # Stirrer and plant self.stirrer.deactivate() self.plant.activate(False) self.fp_log.close() self.receipe_sema.release() def rast(self, rast): # Temperature self.ctrl_theta = self.theta_ist_k self.ctrl_heatrate = self.heatrate_ist_k ovenStateNext = self.ovenState # ----------------------------------------- ctrl_theta = self.ctrl_theta ctrl_heatrate = self.ctrl_heatrate ctrl_theta_err = self.ctrl_theta_soll - ctrl_theta ctrl_heatrate_err = self.ctrl_heatrate_soll - ctrl_heatrate if self.ovenState != ovenStates.IDLE: self.pid_hold.process(self.dt, self.params['Hold']['Pid'], ctrl_theta_err) self.pid_rate.process(self.dt, self.params['Heat']['Pid'], ctrl_heatrate_err) power_hold = self.power_hold_offset + self.params['P_max'] * self.pid_hold.get_y() power_heat = self.power_heat_offset + self.params['P_max'] * self.pid_rate.get_y() power_hold = max(self.params['P_min'], min(self.params['P_max'], power_hold)) power_heat = max(self.params['P_min'], min(self.params['P_max'], power_heat)) self.power_soll = power_hold if self.ovenState == ovenStates.HEAT: self.power_soll = power_heat self.plant.setPower(self.power_soll) self.time_v = np.append(self.time_v, self.time/60) self.theta_v = np.append(self.theta_v, ctrl_theta) self.heatrate_v = np.append(self.heatrate_v, ctrl_heatrate) self.power_v = np.append(self.power_v, self.power_soll) self.power_hold_v = np.append(self.power_hold_v, power_hold) self.power_heat_v = np.append(self.power_heat_v, power_heat) self.error_v = np.append(self.error_v, ctrl_theta_err) self.time += self.dt # ----------------------------------------- rast_finished = False if self.ovenState == ovenStates.IDLE: if (ctrl_theta + 1.0) < self.ctrl_theta_soll: ovenStateNext = ovenStates.HEAT else: ovenStateNext = ovenStates.HOLD if self.ovenState == ovenStates.HEAT: if (ctrl_theta + 1.0) >= self.ctrl_theta_soll: ovenStateNext = ovenStates.HOLD if ovenStateNext != self.ovenState: self.log("{} -> {}".format(self.ovenState, ovenStateNext)) if ovenStateNext == ovenStates.HEAT: self.pid_rate.reset() self.stirrer.setSpeed(self.stirrSpeedHeat) self.stirrer.setDutyCycle(1.0) if self.ovenState == ovenStates.HOLD: self.power_hold_offset = power_hold if ovenStateNext == ovenStates.HOLD: self.pid_hold.reset() self.stirrer.setSpeed(self.stirrSpeedRast) self.stirrer.setDutyCycle(self.stirrDutyRast) if self.ovenState == ovenStates.HEAT: self.power_heat_offset = power_heat # ------------------------------ self.rast_running = True self.ovenState = ovenStateNext return self.ovenState == ovenStates.HOLD