From f088e9f4fead14b66839a3ed22d7773312976593 Mon Sep 17 00:00:00 2001 From: Jens Ahrensfeld Date: Tue, 23 Apr 2019 11:29:40 +0000 Subject: [PATCH] - Controller switched to timer mode git-svn-id: http://moon:8086/svn/projects/HendiControl@258 fda53097-d464-4ada-af97-ba876c37ca34 --- Control/brewpi/brewpi.py | 13 +- Control/brewpi/controller.py | 329 ++++++++++++++++++----------------- 2 files changed, 172 insertions(+), 170 deletions(-) diff --git a/Control/brewpi/brewpi.py b/Control/brewpi/brewpi.py index 7238d82..d76ffd8 100755 --- a/Control/brewpi/brewpi.py +++ b/Control/brewpi/brewpi.py @@ -5,7 +5,7 @@ import json from controller import Controller from mass import Mass from stirrer import Stirrer -from pololu1376 import Pololu1376 +#from pololu1376 import Pololu1376 from matplotlib.pyplot import figure, clf, plot, xlabel, ylabel, xlim, ylim, title, grid, axes, show, subplot @@ -15,17 +15,16 @@ def results_plot(self): for n in range(0, len(self.time_v)): d = self.time_v[n] - v1 = self.theta_raw_v[n] - v2 = self.theta_k_v[n] + v1 = self.theta_v[n] + v2 = self.heatrate_v[n] v3 = self.power_v[n] v4 = self.error_v[n] - v5 = self.heatrate_k_v[n] - fp.write("{:6.3f} {:6.3f} {:6.3f} {:6.3f} {:6.3f} {:6.3f}\n".format(d, v1, v2, v3, v4, v5)) + fp.write("{:6.3f} {:6.3f} {:6.3f} {:6.3f} {:6.3f}\n".format(d, v1, v2, v3, v4,)) fp.close() figure(1) subplot(4, 1, 1) - plot(self.time_v, self.theta_raw_v, 'g-', self.time_v, self.theta_k_v, 'r-', linewidth=1) + plot(self.time_v, self.theta_v, 'r-', linewidth=1) title('Temperature') grid(True) ylabel('°C') @@ -40,7 +39,7 @@ def results_plot(self): grid(True) ylabel('°C') subplot(4, 1, 4) - plot(self.time_v, self.heatrate_k_v, 'r-', linewidth=1) + plot(self.time_v, self.heatrate_v, 'r-', linewidth=1) title('Heatrate') grid(True) ylabel('°C/min') diff --git a/Control/brewpi/controller.py b/Control/brewpi/controller.py index 809c2b6..d3ee1ce 100644 --- a/Control/brewpi/controller.py +++ b/Control/brewpi/controller.py @@ -6,15 +6,18 @@ 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(): +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 @@ -23,8 +26,7 @@ class Controller(): self.params = params self.pid = Pid() self.timer_ist = 0 - self.theta = 0 - self.heatrate = 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) @@ -34,9 +36,7 @@ class Controller(): 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) @@ -49,29 +49,68 @@ class Controller(): 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.reportTimer = Timer("Report", self) + self.timerMgr.registerTimer(self.sensorTimer) + self.timerMgr.registerTimer(self.reportTimer) + self.rast_running = False + + def onTimer(self, timer): + if timer == self.sensorTimer: + self.sensorTime_v = np.append(self.sensorTime_v, timer.count*self.sensor_dt / 60) + + # Process plant + self.plant.process() + + # 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.sensor_dt + 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.reportTimer: + self.report() 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)) + 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.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.sensorTimer.start(self.sensor_dt/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() @@ -112,178 +151,142 @@ class Controller(): 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 - for rast in recipe['Rasten']: - self.rast(rast) - if self.rast_abort: - break - time.sleep(1); + waut = self.timerMgr.getMinTimeout() + time.sleep(waut) - if not self.is_restart: - self.receipe_sema.release() + self.rast_running = False + # Stirrer and plant + self.stirrer.deactivate() + self.plant.activate(False) + + self.receipe_sema.release() def rast(self, rast): # Temperature - theta = self.plant.getTemperature() - theta_soll = rast['temp'] - heatrate_soll = rast['heatRate'] - self.log("Target temperature {} °C".format(theta_soll)) + 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 - self.timer_ist = 0 timer_soll = 60*rast['time'] - # Kalman filter initial value - self.kalman.initial((theta, 0)) - self.theta_sm.initial(theta) - # ------------------------------ # The loop # ------------------------------ - doLoop = theta_soll > theta - if doLoop: - # Stirrer - self.stirrer.activate() - self.plant.activate(True) + if self.ctrl_theta_soll < self.ctrl_theta: + return True - self.stirrer.setCycleTime(self.stirrCycleTime) + controllerStateNext = self.controllerState + ovenStateNext = self.ovenState - while(doLoop): + # ----------------------------------------- + self.stirrer.process() - time_start = time.time() + ctrl_theta = self.ctrl_theta + ctrl_heatrate = self.ctrl_heatrate - controllerStateNext = self.controllerState - ovenStateNext = self.ovenState + ctrl_theta_err = self.ctrl_theta_soll - ctrl_theta + ctrl_heatrate_err = self.ctrl_heatrate_soll - ctrl_heatrate - # ----------------------------------------- - self.stirrer.process() - self.plant.process() + if self.ovenState == ovenStates.NOP: + if ctrl_theta < self.ctrl_theta_soll: + ovenStateNext = ovenStates.HEAT - theta_raw = self.plant.getTemperature() - self.theta_raw_v = np.append(self.theta_raw_v, theta_raw) + 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 = self.params['Heat']['Acqu']['thresh_leave'] + pid_thresh_track = self.params['Heat']['Track']['thresh_leave'] + 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 - # Process Kalman - Z = self.kalman.process_measurement((theta_raw, 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_raw) - 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 self.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: - ctrl_theta_err = self.theta_err_sm.process(theta_soll - theta_ist) - ctrl_heatrate_err = self.heatrate_err_sm.process(heatrate_soll - 60/self.dt * dtheta_ist) - 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 = self.params['Heat']['Acqu']['thresh_leave'] - pid_thresh_track = self.params['Heat']['Track']['thresh_leave'] - 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 = self.params['Heat']['Acqu']['thresh_leave'] - pid_thresh_track = self.params['Heat']['Track']['thresh_leave'] - 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_soll = max(self.params['P_min'], min(self.params['P_max'], self.params['P_max']*y)) - self.plant.setPower(power_soll) - power_ist = self.plant.getPower() - - self.time_v = np.append(self.time_v, self.time/60) - self.power_v = np.append(self.power_v, power_ist) - 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)) - self.pid.reset() - 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: + 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 = self.params['Heat']['Acqu']['thresh_leave'] + pid_thresh_track = self.params['Heat']['Track']['thresh_leave'] + if self.timer_ist == 0: + ovenStateNext = ovenStates.NOP doLoop = False - self.stirrer.deactivate() - self.plant.activate(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_soll = max(self.params['P_min'], min(self.params['P_max'], self.params['P_max']*y)) + 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_ist) + self.error_v = np.append(self.error_v, pid_err) + + # ----------------------------------------- + if ovenStateNext != self.ovenState: + self.log("{} -> {}".format(self.ovenState, ovenStateNext)) + self.pid.reset() + 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 + # ------------------------------ + self.time += self.dt + + if self.timer_ist > 0: + self.timer_ist -= self.dt + else: + self.timer_ist = 0 +