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 ovenStates = Enum('ovenStates', 'NOP HEAT HOLD') controllerStates = Enum('controllerStates', 'ACQU TRACK') class Controller(): def __init__(self, params, plant, stirrer): print(json.dumps({'Controller': params}, indent=4, sort_keys=True)) self.ovenState = ovenStates.NOP self.controllerState = controllerStates.ACQU self.plant = plant self.stirrer = stirrer self.dt = params['dt'] self.params = params self.pid = Pid() self.timer_ist = 0 self.theta = 0 self.heatrate = 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_v = np.empty(0) self.error_v = np.empty(0) self.time = 0 self.sim_warp_factor = params['sim_warp_factor'] self.report_interval = 1.0 self.report_last_time = 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) kparams = { 'dt': 1, 'var_P': 0, 'var_Q': 0.00001, 'var_R': 2, 'var_Z': 0 } self.kalman = Kalman(kparams) def log(self, s): now = time.time() print("{:.2f}: {}".format(now, s)) def report(self): now = time.time() if (now - self.report_last_time) >= self.report_interval: self.log("Temp IST = {:0.1f} °C".format(self.theta)) self.log("Heatrate IST = {:0.1f} °C/min.".format(self.heatrate)) self.log("Power = {:0.0f} W".format(self.plant.getPower())) if self.timer_ist > 0: self.log("Rast remaining : {:0.1f} min.".format(self.timer_ist/60)) # self.log("theta_err = {:0.1f} °C".format(theta_err)) self.report_last_time = now def start(self, recipe): self.is_pause = False self.receipe = recipe print(json.dumps({recipe['Name'] : recipe}, indent=4, sort_keys=True)) 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.setPower(0) 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'] self.time = 0 for rast in recipe['Rasten']: self.rast(rast) if self.rast_abort: break if not self.is_restart: self.receipe_sema.release() def rast(self, rast): # Stirrer self.stirrer.activate() self.stirrer.setCycleTime(self.stirrCycleTime) # Temperature self.theta = self.plant.getTemperature() theta_soll = rast['temp'] heatrate_soll = rast['heatRate'] self.log("Target temperature {} °C".format(theta_soll)) # Timer self.timer_ist = 0 timer_soll = 60*rast['time'] # Kalman filter initial value self.kalman.initial((self.theta, 0)) self.theta_sm.initial(self.theta) # ------------------------------ # The loop # ------------------------------ doLoop = True useKalman = True while(doLoop): time_start = time.time() controllerStateNext = self.controllerState ovenStateNext = self.ovenState # ----------------------------------------- self.stirrer.process() self.plant.process() theta = self.plant.getTemperature() # Process Kalman Z = self.kalman.process_measurement((theta, 0)) xp = self.kalman.process(Z) theta_ist_k = xp[0, 0] dtheta_ist_k = xp[1, 0] self.theta_k_v = np.append(self.theta_k_v, theta_ist_k) self.heatrate_k_v = np.append(self.heatrate_k_v, dtheta_ist_k * 60 / self.dt) # Process conventional theta_ist = self.theta_sm.process(theta) dtheta_ist = self.heatrate_sm.process(theta_ist - self.theta) self.theta_v = np.append(self.theta_v, theta_ist) self.heatrate_v = np.append(self.heatrate_v, dtheta_ist * 60/self.dt) if useKalman: ctrl_theta_err = theta_soll - theta_ist_k ctrl_heatrate_err = heatrate_soll - dtheta_ist_k * 60/self.dt ctrl_theta = theta_ist_k ctrl_dtheta = dtheta_ist_k else: theta_err = theta_soll - theta_ist dtheta_err = heatrate_soll - dtheta_ist ctrl_theta_err = self.theta_err_sm.process(theta_err) ctrl_heatrate_err = self.heatrate_err_sm.process(60/self.dt * dtheta_err) ctrl_theta = theta_ist ctrl_dtheta = dtheta_ist if self.ovenState == ovenStates.NOP: if ctrl_theta < theta_soll: ovenStateNext = ovenStates.HEAT pid_err = 0 if self.ovenState == ovenStates.HEAT: pid_err = ctrl_heatrate_err pid_params_acqu = self.params['Heat']['Acqu']['Pid'] pid_params_track = self.params['Heat']['Track']['Pid'] pid_thresh_acqu = 0.2 pid_thresh_track = 0.5 if (ctrl_theta + 1.0) >= 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: pid_err = ctrl_theta_err pid_params_acqu = self.params['Hold']['Acqu']['Pid'] pid_params_track = self.params['Hold']['Track']['Pid'] pid_thresh_acqu = 0.2 pid_thresh_track = 0.5 if self.timer_ist == 0: ovenStateNext = ovenStates.NOP doLoop = False if self.ovenState != ovenStates.NOP: if self.controllerState == controllerStates.ACQU: pid_params = pid_params_acqu if abs(pid_err) < pid_thresh_acqu: controllerStateNext = controllerStates.TRACK if self.controllerState == controllerStates.TRACK: pid_params = pid_params_track if abs(pid_err) > pid_thresh_track: controllerStateNext = controllerStates.ACQU self.pid.process(self.dt, pid_params, pid_err) y = self.pid.get_y() power = max(self.params['P_min'], min(self.params['P_max'], self.params['P_max']*y)) self.plant.setPower(power) self.time_v = np.append(self.time_v, self.time/60) self.power_v = np.append(self.power_v, power) self.error_v = np.append(self.error_v, pid_err) #self.error_v = np.append(self.error_v, self.stirrer.getSpeed()) # ----------------------------------------- if ovenStateNext != self.ovenState: self.log("{} -> {}".format(self.ovenState, ovenStateNext)) if ovenStateNext == ovenStates.HEAT: self.stirrer.setSpeed(self.stirrSpeedHeat) self.stirrer.setDutyCycle(1.0) if ovenStateNext == ovenStates.HOLD: 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 # ------------------------------ time_stop = time.time() time_to_sleep = self.dt / self.sim_warp_factor - (time_stop - time_start) if time_to_sleep < 0: self.log("Warning: dt is too small!") time_to_sleep = 0 self.time += self.dt self.theta = ctrl_theta self.heatrate = ctrl_dtheta * 60 / self.dt if self.timer_ist > 0: self.timer_ist -= self.dt else: self.timer_ist = 0 self.report() while(doLoop): time.sleep(time_to_sleep) if self.is_pause == False: break if self.rast_abort: break if self.rast_abort: doLoop = False self.stirrer.deactivate()