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, 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_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

	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
		theta_last = self.plant.getTemperature()
		theta_soll = rast['temp']
		heatrate_soll = rast['heatRate']
		print("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):
			controllerStateNext = self.controllerState
			ovenStateNext = self.ovenState

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

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

			# print ("{}: Temp IST = {:0.2f} °C".format(timer_ist, temp_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 >= 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.time += self.dt
			theta_last = 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.error_v = np.append(self.error_v, pid_err)
			#self.error_v = np.append(self.error_v, self.stirrer.getRpm())

			# print ("theta_err = {:0.2f} °C".format(theta_err))
			# print ("Power = {:0.2f} W".format(power))

			# -----------------------------------------
			if ovenStateNext != self.ovenState:
				print("{} -> {}".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:
				print("{} -> {}".format(self.controllerState, controllerStateNext))

			self.ovenState = ovenStateNext
			self.controllerState = controllerStateNext
			# ------------------------------

		self.stirrer.stop()