- reworked

git-svn-id: http://moon:8086/svn/projects/HendiControl@113 fda53097-d464-4ada-af97-ba876c37ca34

Control/brewpi/Rezept-001.json

@@ -0,0 +1,13 @@
+{
+	"Name" : "Rezept-001",
+	"Schuettung_kg" : 6,
+	"Wasser_kg" : 18,
+	"Rasten" :
+	[
+		{"time" :  15, "temp" :  50.0, "heatRate" :  0.5, "stirDutyCycle" :  0.5, "waitForUser" : true},
+		{"time" :  20, "temp" :  60.0, "heatRate" :  0.5, "stirDutyCycle" :  0.5, "waitForUser" : false},
+		{"time" :  30, "temp" :  70.0, "heatRate" :  0.5, "stirDutyCycle" :  0.5, "waitForUser" : false},
+		{"time" :   5, "temp" :  80.0, "heatRate" :  0.5, "stirDutyCycle" :  0.5, "waitForUser" : true}
+	]
+}
+

Control/brewpi/aplant.py

@@ -0,0 +1,20 @@
+import numpy as np
+import abc
+
+
+class APlant(abc.ABC):
+	def __init__(self, params):
+		pass
+
+	@abc.abstractmethod
+	def process(self, dt):
+		pass
+
+	@abc.abstractmethod
+	def setPower(self, power_W):
+		pass
+
+	@abc.abstractmethod
+	def getTemperature(self):
+		return None
+

Control/brewpi/brewpi.cfg.json

@@ -1,32 +1,59 @@
 {
-	"Global" : 
-	{
-		"samplerate_Hz" : 1.0
-	},
 
 	"Controller" :
 	{
-		"cont_ctrl"				: true,
+		"dt"			: 1.0,
-		"P_min_W"				: 0,
+		"cont_ctrl"		: true,
-		"P_max_W"				: 3000,	
+		"P_min"			: 0,
-		"P_q_W"					: 100,
+		"P_max"			: 3000,
-		"theta_lock_K"			: 0.5,
+		"P_q_W"			: 100,
-		"kp"					: 2,
+		"theta_lock_K"	: 0.5,
-		"ki"					: 0.02,
+		"Hold" : {
-		"kd"					: 400,
+			"Acqu": {
-		"pid_rho"				: 0.999,
+				"Pid" : {
-		"heatrate_KPerMin"		: 0.5
+					"kp"	: 2,
+					"ki"	: 0.02,
+					"kd"	: 400,
+					"rho"	: 0.999
+				}
+			},
+			"Track": {
+				"Pid" : {
+					"kp"	: 2,
+					"ki"	: 0.02,
+					"kd"	: 400,
+					"rho"	: 0.999
+				}
+			}
+		},
+		"Heat" : {
+			"Acqu": {
+				"Pid" : {
+					"kp"	: 4,
+					"ki"	: 0.02,
+					"kd"	: 4,
+					"rho"	: 0.999
+				}
+			},
+			"Track": {
+				"Pid" : {
+					"kp"	: 8,
+					"ki"	: 0.03,
+					"kd"	: 8,
+					"rho"	: 0.999
+				}
+			}
+		}
 	},
 
 	"Simulation" :
 	{
-		"theta_amb_degC"		: 20,
+		"theta_amb"		: 20,
-		"C_joulePerKperkg"		: 4190,
+		"C"				: 4190,
-		"M_kg"					: 20,
+		"M"				: 20,
-		"mass_kleak"			: 0.5,
+		"L"				: 0.5,
-		"mass_delay_s"			: 60,
+		"Td"			: 10,
-		"Td_s"					: 10,
+		"kn"			: 0
-		"k_noise"				: 0
 	}
 }
 

Control/brewpi/controller.py

@@ -0,0 +1,169 @@
+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')
+		xlabel('t/min')
+		subplot(4, 1, 2)
+		plot(self.time_v, self.power_v, 'r-', linewidth=1)
+		title('Power')
+		grid(True)
+		ylabel('W')
+		xlabel('t/min')
+		subplot(4, 1, 3)
+		plot(self.time_v, self.theta_err_v, 'b-', linewidth=1)
+		title('Err_{T}')
+		grid(True)
+		ylabel('°C')
+		xlabel('t/min')
+		subplot(4, 1, 4)
+		plot(self.time_v, self.heatrate_v, 'r-', linewidth=1)
+		title('Heatrate')
+		grid(True)
+		ylabel('°C')
+		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
+
+		# ------------------------------

Control/brewpi/main.py

@@ -3,29 +3,25 @@
 import time
 import json
 import configparser
+from pid import Pid
+from controller import Controller
+from mass import Mass
 
-def getValue(s):
-
-	v = s.split(' ')[0]
-
-	return v
-
-def getUnit(s):
-
-	u = s.split(' ')[1]
-
-	return u
 
 if __name__ == '__main__':
-	config = configparser.RawConfigParser()
-	config.read('brewpi.cfg')
-	print (getValue(config.get('Global', 'samplerate')))
-	print (getValue(config.get('Controller', 'cont_ctrl')))
-	print (getValue(config.get('Controller', 'heatrate')))
 
 	fp = open("brewpi.cfg.json")
 	configJson = json.load(fp)
-	print (json.dumps({'config' : configJson}, indent=4, sort_keys=True))
+
+	plant = Mass(configJson["Simulation"])
+	temp = plant.getTemperature()
+
+	fp = open("Rezept-001.json")
+	recipeJson = json.load(fp)
+
+	ablauf = Controller(configJson["Controller"], plant)
+	ablauf.start(recipeJson)
+	ablauf.stop()
 
 	print ("End of program.")
 

Control/brewpi/mass.py

@@ -0,0 +1,35 @@
+import numpy as np
+import json
+from aplant import APlant
+
+class Mass(APlant):
+	def __init__(self, params):
+		print(json.dumps({'Mass': params}, indent=4, sort_keys=True))
+		self.e = 0
+		self.x = 0
+		self.gain = 0.8
+
+		self.C = params['C']
+		self.M = params['M']
+		self.L = params['L']
+		self.Td = params['Td']
+		self.theta_amb = params['theta_amb']
+
+		self.theta = 0
+		self.P = 0
+
+	def process(self, dt):
+		alpha = 1.0
+		if self.Td > 0:
+			alpha = dt/self.Td
+
+		self.e = self.e*(1 - ((self.L*self.theta)*dt)/(self.M*self.C))
+		self.x = (1-alpha)*self.x + self.gain*alpha*self.P*dt
+		self.e += self.x
+		self.theta = self.e/(self.M*self.C)
+
+	def setPower(self, power_w):
+		self.P = power_w
+
+	def getTemperature(self):
+		return self.theta + self.theta_amb

Control/brewpi/pid.py

@@ -0,0 +1,58 @@
+import numpy as np;
+
+
+class Pid():
+    def __init__(self):
+        # Integrator
+        self.yi = 0
+
+        # Differentiator
+        self.xd = 0
+
+        # Auto windup
+        self.y_min = 0.0
+        self.y_max = 80.0
+        self.diff_aw = 0.0
+
+        # Output
+        self.y = 0
+        self.yc = 0
+
+    @staticmethod
+    def params(kp, ki, kd, rho):
+        p = dict(kp=kp, ki=ki, kd=kd, rho=rho)
+        return p
+
+    def process(self, params, err):
+        kp = params['kp']
+        ki = params['ki']
+        kd = params['kd']
+        rho = params['rho']
+
+        err_aw = err + self.diff_aw
+        yi = rho*self.yi + err_aw
+        yd = err - self.xd
+
+        _yp = kp * err
+        _yi = ki * yi
+        _yd = kd * yd
+
+        self.y = _yp + _yi + _yd
+
+        yaw = yi
+        if yaw < self.y_min:
+            self.diff_aw = abs(yaw - self.y_min)
+
+        if yaw > self.y_max:
+            self.diff_aw = -abs(yaw - self.y_max)
+
+        self.yc = max(self.y_min, min(self.y_max, self.y))
+
+        self.yi = yi
+        self.xd = err
+
+    def get_y(self):
+        return self.y
+
+    def get_yc(self):
+        return self.yc

Control/brewpi/utils.py

@@ -0,0 +1,29 @@
+class Smoother:
+	def __init__(self, alpha):
+		self.a = alpha
+		self.b = 1-alpha
+		self.y = 0
+
+	def process(self, x):
+		self.y = self.b*self.y + self.a*x
+
+	def get_y(self):
+		return self.y
+
+class Stable:
+	def __init__(self, stable_count, err_max):
+		self.stable_time = stable_count
+		self.count = stable_count
+		self.err_max = err_max
+		self.x = 0
+
+	def process(self, x):
+		if self.count > 0:
+			self.count -= 1
+
+		if abs(x - self.x) >= self.err_max:
+			self.count = self.stable_time
+
+		is_stable = (self.count == 0)
+
+		return is_stable