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

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