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

ovenStates = Enum('ovenStates', 'NOP HEAT HOLD')
controllerStates = Enum('controllerStates', 'ACQU TRACK')


class Controller():

	def __init__(self, params, plant):
		print(json.dumps({'Controller': params}, indent=4, sort_keys=True))
		self.ovenState = ovenStates.NOP
		self.controllerState = controllerStates.ACQU
		self.plant = plant
		self.dt = params['dt']
		self.params = params
		self.pid = Pid()
		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.theta_err_v = np.empty(0)
		self.heatrate_err_v = np.empty(0)
		self.theta_ist = 0
		self.time = 0
		pass

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

		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.theta_err_v, 'b-', linewidth=1)
		title('Temperature 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):
		temp_soll = rast['temp']
		heatrate_soll = rast['heatRate']
		print("Target temperature {} °C".format(temp_soll))
		doLoop = True
		timer_ist = 0
		heatrate_ist = 0
		timer_soll = 60*rast['time']/self.dt
		heatRateMeasureInterval = 1
		heatRateMeasureCount = 1

		theta_err_sm = Smoother(0.5)
		heatrate_err_sm = Smoother(0.5)

		# ------------------------------
		# The loop
		# ------------------------------
		while(doLoop):
			controllerStateNext = self.controllerState
			ovenStateNext = self.ovenState

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

			if heatRateMeasureCount <= 0:
				heatrate_ist = 60 * (theta_ist - self.theta_ist) / heatRateMeasureInterval;
				heatRateMeasureCount = heatRateMeasureInterval;
				theta_last = theta_ist;

			heatRateMeasureCount = heatRateMeasureCount - self.dt;
#			print ("{}: Temp IST = {:0.2f} °C".format(timer_ist, temp_ist))

			theta_err = temp_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 < temp_soll:
					ovenStateNext = ovenStates.HEAT

			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']
				if theta_ist >= temp_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']
				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(theta_err) < 0.2:
						controllerStateNext = controllerStates.TRACK


				if self.controllerState == controllerStates.TRACK:
					pid_params = pid_params_track
					if abs(theta_err) > 0.5:
						controllerStateNext = controllerStates.ACQU

				self.pid.process(pid_params, pid_err)

			y = self.pid.get_y()
#			print ("theta_err = {:0.2f} °C".format(theta_err))

			power = max(self.params['P_min'], min(self.params['P_max'], self.params['P_max']*y))
#			print ("Power = {:0.2f} W".format(power))
			self.plant.setPower(power)

			self.time += self.dt
			self.theta_ist = theta_ist

			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.theta_err_v = np.append(self.theta_err_v, theta_err_sm.get_y())
			self.heatrate_err_v = np.append(self.heatrate_err_v, heatrate_err_sm.get_y())

			# -----------------------------------------
			if controllerStateNext != self.controllerState:
				print("{} -> {}".format(self.controllerState, controllerStateNext))

			if ovenStateNext != self.ovenState:
				print("{} -> {}".format(self.ovenState, ovenStateNext))

			self.controllerState = controllerStateNext
			self.ovenState = ovenStateNext

		# ------------------------------