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

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_ist = 0
		self.heatrate_ist = 0
		self.theta_v = np.empty(0)
		self.heatrate_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)

	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_ist))
			self.log("Heatrate IST = {:0.1f} °C/min.".format(self.heatrate_ist))
			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):
		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

	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)

		self.receipe_sema.release()

	def rast(self, rast):

		# Stirrer
		self.stirrer.activate()
		self.stirrer.setCycleTime(self.stirrCycleTime)

		# Temperature
		self.theta_ist = 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']

		# ------------------------------
		# The loop
		# ------------------------------
		doLoop = True
		theta_err_sm = Smoother(0.5)
		heatrate_err_sm = Smoother(0.5)
		while(doLoop):
			if self.rast_abort:
				break

			time_start = time.time()

			controllerStateNext = self.controllerState
			ovenStateNext = self.ovenState

			# -----------------------------------------
			self.stirrer.process()
			self.plant.process()
			theta_ist = self.plant.getTemperature()

			heatrate_ist = 60/self.dt * (theta_ist - self.theta_ist)

			theta_err = theta_soll - theta_ist
			heatrate_err = heatrate_soll - heatrate_ist

			theta_err_sm.process(theta_err)
			heatrate_err_sm.process(heatrate_err)

			if self.ovenState == ovenStates.NOP:
				if theta_ist < theta_soll:
					ovenStateNext = ovenStates.HEAT

			pid_err = 0
			if self.ovenState == ovenStates.HEAT:
				pid_err = heatrate_err_sm.get_y()
				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 (theta_ist + 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 = theta_err_sm.get_y()
				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.theta_v = np.append(self.theta_v, theta_ist)
			self.heatrate_v = np.append(self.heatrate_v, heatrate_ist)
			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_ist = theta_ist
			self.heatrate_ist = heatrate_ist

			if self.timer_ist > 0:
				self.timer_ist -= self.dt
			else:
				self.timer_ist = 0

			self.report()

			time.sleep(time_to_sleep)

		self.stirrer.deactivate()