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') 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.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.receipe_sema = threading.Semaphore(0) 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_offset = 200 self.power_heat_offset = 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.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.rast_running = False self.thread = threading.Thread(target=self.receipe_run, args=(recipe,)) self.thread.start() def stop(self): if self.thread != None: self.receipe = None self.thread.join() self.thread = None def pause(self): if self.thread != None: self.ovenState = ovenStates.HOLD def cont(self): if self.thread != None: self.ovenState = ovenStates.NOP 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(self.receipe is not None): 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 waut = self.timerMgr.getMinTimeout() time.sleep(waut) # 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 # ------------------------------ 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.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) 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)) power_soll = min(power_hold, power_hold) if self.ovenState == ovenStates.HEAT: power_soll = min(power_heat, 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, 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.power_soll = power_soll self.time += self.dt if self.timer_ist > 0: self.timer_ist -= self.dt else: self.timer_ist = 0 if self.ovenState == ovenStates.HOLD: if self.timer_ist == 0: if not rast["waitForUser"]: ovenStateNext = ovenStates.NOP # ----------------------------------------- if ovenStateNext != self.ovenState: self.log("{} -> {}".format(self.ovenState, ovenStateNext)) if ovenStateNext == ovenStates.HEAT: self.power_hold_offset = power_hold self.pid_rate.reset() self.stirrer.setSpeed(self.stirrSpeedHeat) self.stirrer.setDutyCycle(1.0) if ovenStateNext == ovenStates.HOLD: self.power_heat_offset = power_heat self.pid_hold.reset() self.stirrer.setSpeed(self.stirrSpeedRast) self.stirrer.setDutyCycle(self.stirrDutyRast) # ------------------------------ self.rast_running = True self.ovenState = ovenStateNext return self.ovenState == ovenStates.NOP