HendiControlFirmware/Control/brewpi/controller.py

import numpy as np
import json
from enum import Enum
from pid import Pid
from matplotlib.pyplot import figure, clf, plot, xlabel, ylabel, xlim, ylim, title, grid, axes, show, subplot
from utils import Smoother, Stable
import time

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.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 = 10000
		self.log_report_interval = 1.0
		self.log_last_time = 0

	def log(self, s):
		now = time.time()
		print("{:.2f}: {}".format(now, s))

	def report(self):
		now = time.time()
		if (now - self.log_last_time) >= self.log_report_interval:

			self.log("Temp IST = {:0.2f} °C".format(self.theta_ist))
			self.log("Heatrate IST = {:0.2f} °C/min.".format(self.heatrate_ist))
			self.log("Power = {:0.2f} W".format(self.plant.getPower()))
			# self.log("theta_err = {:0.2f} °C".format(theta_err))
			self.log_last_time = now

	def start(self, recipe):
		print(json.dumps({recipe['Name'] : recipe}, indent=4, sort_keys=True))

		# Stirrer
		self.stirrRpmHeat = recipe['stirrRpmHeat']
		self.stirrRpmRast = recipe['stirrRpmRast']
		self.stirrDutyRast = recipe['stirrDutyRast']
		self.stirrCycleTime = recipe['stirrCycleTime']

		self.time = 0

		for rast in recipe['Rasten']:
			self.rast(rast)

	def stop(self):
		figure(1)
		subplot(4, 1, 1)
		plot(self.time_v, self.theta_v, 'b-', linewidth=1)
		title('Temperature')
		grid(True)
		ylabel('°C')
		subplot(4, 1, 2)
		plot(self.time_v, self.power_v, 'r-', linewidth=1)
		title('Power')
		grid(True)
		ylabel('W')
		subplot(4, 1, 3)
		plot(self.time_v, self.error_v, 'b-', linewidth=1)
		title('Error')
		grid(True)
		ylabel('°C')
		subplot(4, 1, 4)
		plot(self.time_v, self.heatrate_v, 'r-', linewidth=1)
		title('Heatrate')
		grid(True)
		ylabel('°C/min')
		xlabel('t/min')
		show()

	def rast(self, rast):

		# Stirrer
		self.stirrer.start()
		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
		timer_ist = 0
		timer_soll = 60*rast['time']/self.dt

		# ------------------------------
		# The loop
		# ------------------------------
		doLoop = True
		theta_err_sm = Smoother(0.5)
		heatrate_err_sm = Smoother(0.5)
		while(doLoop):
			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
					timer_ist = 0

			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
				timer_ist += self.dt
				if timer_ist >= timer_soll:
					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.getRpm())


			# -----------------------------------------
			if ovenStateNext != self.ovenState:
				self.log("{} -> {}".format(self.ovenState, ovenStateNext))
				if ovenStateNext == ovenStates.HEAT:
					self.stirrer.setRpm(self.stirrRpmHeat)
					self.stirrer.setDutyCycle(1.0)

				if ovenStateNext == ovenStates.HOLD:
					self.stirrer.setRpm(self.stirrRpmRast)
					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

			self.report()

			time.sleep(time_to_sleep)

		self.stirrer.stop()