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 ovenStates = Enum('ovenStates', 'NOP HEAT HOLD') controllerStates = Enum('controllerStates', 'ACQU TRACK') class Controller(TimerListener): def __init__(self, params, plant, stirrer): print(json.dumps({'Controller': params}, indent=4, sort_keys=True)) self.sim_warp_factor = params['sim_warp_factor'] self.ovenState = ovenStates.NOP self.controllerState = controllerStates.ACQU self.plant = plant self.stirrer = stirrer self.dt = params['dt'] self.params = params self.pid_hold = Pid() self.pid_rate = Pid() self.timer_ist = 0 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.rast_abort = False self.receipe_sema = threading.Semaphore(0) self.is_pause = False self.is_restart = False self.receipe = 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.rastTimer = Timer("Rast", self) self.sensorTimer = Timer("Sensor", self) self.plantTimer = Timer("Plant", self) self.reportTimer = Timer("Report", self) self.timerMgr.registerTimer(self.sensorTimer) self.timerMgr.registerTimer(self.plantTimer) self.timerMgr.registerTimer(self.reportTimer) self.rast_running = False self.power_hold = 0 self.power_heat = 1500 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) if timer == self.plantTimer: # Process plant self.plant.process() if timer == self.reportTimer: self.report() def log(self, s): now = time.time() print("{:.2f}: {}".format(now, s)) def report(self): if self.rast_running: 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.timer_ist > 0: self.log("Rast remaining : {:0.1f} min.".format(self.timer_ist/60)) def start(self, recipe): self.is_pause = False self.receipe = recipe print(json.dumps({recipe['Name'] : recipe}, indent=4, sort_keys=True)) self.sensorTimer.start(self.sensor_dt/self.sim_warp_factor) self.plantTimer.start(1/self.sim_warp_factor) self.reportTimer.start(self.report_interval) self.rastTimer.start(self.dt/self.sim_warp_factor) self.thread = threading.Thread(target=self.receipe_run, args=(recipe,)) self.thread.start() def stop(self): if self.thread != None: self.rast_abort = True self.thread.join() self.thread = None self.rast_abort = False self.receipe = None def restart(self): receipe = self.receipe self.is_restart = True self.stop() self.start(receipe) self.is_restart = False def pause(self): if self.thread != None: self.is_pause = True self.stirrer.deactivate() self.plant.activate(False) def cont(self): if self.thread != None: self.is_pause = False def wait_finished(self): self.receipe_sema.acquire() pass def receipe_run(self, recipe): # Stirrer self.stirrSpeedHeat = recipe['stirrSpeedHeat'] self.stirrSpeedRast = recipe['stirrSpeedRast'] self.stirrDutyRast = recipe['stirrDutyRast'] self.stirrCycleTime = recipe['stirrCycleTime'] # Stirrer and plant self.stirrer.activate() self.plant.activate(True) self.stirrer.setCycleTime(self.stirrCycleTime) self.time = 0 rasten = recipe['Rasten'] rasten.append(None) rast_count = 0 self.timer_ist = 0 while(not self.rast_abort): self.timerMgr.process() if self.rastTimer.isElapsed(): rast = rasten[rast_count] if rast is not None: isFinished = self.rast(rast) if isFinished: rast_count += 1 self.timer_ist = 0 else: break self.rast_running = True waut = self.timerMgr.getMinTimeout() time.sleep(waut) self.rast_running = False # Stirrer and plant self.stirrer.deactivate() self.plant.activate(False) self.receipe_sema.release() def rast(self, rast): # Temperature self.ctrl_theta_soll = rast['temp'] self.ctrl_theta = self.theta_ist_k self.ctrl_heatrate_soll = rast['heatRate'] self.ctrl_heatrate = self.heatrate_ist_k # Timer timer_soll = 60*rast['time'] # ------------------------------ # The loop # ------------------------------ controllerStateNext = self.controllerState ovenStateNext = self.ovenState # ----------------------------------------- self.stirrer.process() 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.NOP: if ctrl_theta < self.ctrl_theta_soll: ovenStateNext = ovenStates.HEAT if self.ovenState == ovenStates.HEAT: if (ctrl_theta + 1.0) >= self.ctrl_theta_soll: ovenStateNext = ovenStates.HOLD self.log("Set rast timer to {:0.1f} min.".format(timer_soll/60)) self.timer_ist = timer_soll if self.ovenState == ovenStates.HOLD: if self.timer_ist == 0: ovenStateNext = ovenStates.NOP if self.ovenState != ovenStates.NOP: 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) if self.ovenState == ovenStates.HOLD: self.power_hold += self.params['P_max'] * self.pid_hold.get_y() if self.ovenState == ovenStates.HEAT: self.power_heat += self.params['P_max'] * self.pid_rate.get_y() self.power_hold = max(self.params['P_min'], min(self.params['P_max'], self.power_hold)) self.power_heat = max(self.params['P_min'], min(self.params['P_max'], self.power_heat)) power_soll = min(self.power_hold, self.power_hold) if self.ovenState == ovenStates.HEAT: power_soll = min(self.power_heat, self.power_heat) self.plant.setPower(power_soll) power_ist = self.plant.getPower() 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, power_soll) self.power_hold_v = np.append(self.power_hold_v, self.power_hold) self.power_heat_v = np.append(self.power_heat_v, self.power_heat) self.error_v = np.append(self.error_v, ctrl_theta_err) self.power_soll = power_soll # ----------------------------------------- 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 ovenStateNext == ovenStates.HOLD: self.pid_hold.reset() self.stirrer.setSpeed(self.stirrSpeedRast) self.stirrer.setDutyCycle(self.stirrDutyRast) if controllerStateNext != self.controllerState: self.log("{} -> {}".format(self.controllerState, controllerStateNext)) self.ovenState = ovenStateNext self.controllerState = controllerStateNext # ------------------------------ self.time += self.dt if self.timer_ist > 0: self.timer_ist -= self.dt else: self.timer_ist = 0 return self.ovenState == ovenStates.NOP