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


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