HendiControlFirmware/Control/brewpi/controller.py

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_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.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):
		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))


	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()

	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)

		power_hold = max(self.params['P_min'], min(self.params['P_max'], self.params['P_max'] * self.pid_hold.get_y()))
		power_heat = max(self.params['P_min'], min(self.params['P_max'], self.params['P_max'] * self.pid_rate.get_y()))
		power_soll = min(power_hold, 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_ist)
		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)

		# -----------------------------------------
		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