- tempcontroller: added heat rate control

brewpi.py

@@ -33,9 +33,6 @@ class TempSensorTask(ATask):
 	def on_temp_changed(self, value):
 		asyncio.create_task(self.send({'Temp': value}))
 
-	def on_temp_changed_kalman(self, value):
-		pass
-
 	async def recv(self, data):
 		print(data)
 
@@ -44,17 +41,10 @@ class TempSensorTask(ATask):
 
 	async def on_process(self):
 		print("{}: Started with interval {} s".format(self.msg_handler.get_key(), self.interval))
-		self.kalman.initial((self.sensor.temperature(), 0))
 		temp = ValueChanged(self.on_temp_changed, prec=1)
-		temp_kalman = ValueChanged(self.on_temp_changed_kalman, prec=1)
 		self.sensor.set_on_changed("temp", temp.set)
 		while True:
 			self.sensor.process()
-			Z = self.kalman.process_measurement((self.sensor.temperature(), 0), 0)
-			xp = self.kalman.process(Z)
-			theta_ist = xp[0, 0]
-			heatrate_ist = xp[1, 0] * 60
-			temp_kalman.set(theta_ist)
 			await asyncio.sleep(self.interval)
 
 
@@ -190,12 +180,11 @@ if __name__ == '__main__':
 
 		# Kalman Filter
 		kalman_params = {
-			"dt"				: 1.0,
+			"dt"		: 1.0,
-			"var_P"				: 0.1,
+			"var_P"		: 1,
-			"var_Q"				: 0.001,
+			"var_Q"		: 0.0001,
-			"var_R"				: 0.1
+			"var_R"		: 1
 		}
-		kalman = kalman.Kalman(kalman_params)
 
 		# Sensor
 		sensor = TempSensorSim()
@@ -226,23 +215,23 @@ if __name__ == '__main__':
 			"dt": 1.0,
 			"Hold": {
 				"Pid": {
-					"kp": 0.8,
+					"kp": 0.5,
-					"ki": 0.008,
+					"ki": 0.005,
 					"kd": 0.0,
 					"rho": 1.0
 				}
 			},
 			"Heat": {
 				"Pid": {
-					"kp": 0.002,
+					"kp": 0.1,
-					"ki": 0.0002,
+					"ki": 0.01,
 					"kd": 0.0,
 					"rho": 1.0
 				}
 			}
 		}
-		tc = temp_controller.TempController(tc_params)
+		tc = temp_controller.TempController(tc_params, kalman_params)
-		tc.set_on_changed("y_hold", heater_task.actor)
+		tc.set_on_changed("y", heater_task.actor)
 		tc_task = TcTask(tc, TC_DT, dispatcher.msgio_get("TempCtrl"))
 		taskmgr.add(tc_task)
 		sensor.set_on_changed("temp", ValueChanged(tc.set_theta_ist, prec=2).set)

components/pid/kalman.py

@@ -105,7 +105,7 @@ if __name__ == '__main__':
 	params = {
 		'dt'	: dt,
 		'var_P'	: 1,
-		'var_Q'	: 0.001,
+		'var_Q'	: 0.0001,
 		'var_R'	: 1
 	}
 	k = Kalman(params)
@@ -115,7 +115,7 @@ if __name__ == '__main__':
 	_x2 = np.empty(0)
 	_y2 = np.empty(0)
 
-	N = int(100/dt)
+	N = int(1000/dt)
 	seqn = range(0, N)
 
 	_seqn = range(0, 2*N)

components/pid/pid.py

@@ -35,10 +35,6 @@ class Pid:
         yd = kd/dt*(err - self.err)
         yp = kp * err
 
-        # Reset yi on error sign changes
-        if yp > self.y_max or yp <= self.y_min:
-            self.yi = 0
-
         self.err = err
 
         y = yp + self.yi + yd

components/pid/temp_controller.py

@@ -1,52 +1,91 @@
-from components.pid.pid import Pid
+from components.pid import pid
+from components.pid import kalman
 from components.plant.pot import Pot
-from matplotlib.pyplot import plot, figure, subplot, title, xlabel, ylabel, grid, show
+from matplotlib.pyplot import plot, figure, subplot, title, xlabel, ylabel, grid, show, legend
 import numpy as np
 from utils.value import AttributeChange
+from enum import Enum
+
+
+class States(Enum):
+	IDLE = 0,
+	HEAT = 1,
+	HOLD = 2
 
 
 class TempController(AttributeChange):
-	def __init__(self, params):
+	def __init__(self, params, params_kalman):
 		AttributeChange.__init__(self)
 
 		self.dt = params['dt']
-		self.pid_hold = Pid()
+		self.pid_hold = pid.Pid()
-		self.pid_rate = Pid()
+		self.pid_rate = pid.Pid()
+		self.theta_ist_set = 0
+		self.theta_soll_set = 0
+		self.heatrate_ist_set = 0
+		self.heatrate_soll_set = 2.0
 		self.theta_ist = 0
-		self.theta_soll = 0
 		self.heatrate_ist = 0
-		self.heatrate_soll = 0
 		self.params = params
-		self.y_hold = 0
+		self.kalman = kalman.Kalman(params_kalman)
-		self.y_heat = 0
+		self.y = 0
+		self.state = States.HOLD
+		self.use_kalman = True
+		self.kalman.initial((20, 0))
 
 	def set_theta_ist(self, value):
-		self.theta_ist = value
+		self.theta_ist_set = value
 
 	def set_heatrate_ist(self, value):
-		self.heatrate_ist = value
+		self.heatrate_ist_set = value
 
 	def set_theta_soll(self, value):
-		self.theta_soll = value
+		self.theta_soll_set = value
 
 	def set_heatrate_soll(self, value):
-		self.heatrate_soll = value
+		self.heatrate_soll_set = value
 
 	def process(self):
-		theta_err = self.theta_soll - self.theta_ist
+		# Process Kalman
-		heatrate_err = self.heatrate_soll - self.heatrate_ist
+		if self.use_kalman:
+			Z = self.kalman.process_measurement((self.theta_ist_set, 0), 0.0)
+			xp = self.kalman.process(Z)
+			self.theta_ist = xp[0, 0]
+			self.heatrate_ist = xp[1, 0] * 60
+		else:
+			self.theta_ist = self.theta_ist_set
+			self.heatrate_ist = self.heatrate_ist_set
 
-		self.pid_hold.process(self.dt, self.params['Hold']['Pid'], theta_err)
+		theta_err = self.theta_soll_set - self.theta_ist
-		self.pid_rate.process(self.dt, self.params['Heat']['Pid'], heatrate_err)
+		heatrate_err = self.heatrate_soll_set - self.heatrate_ist
 
-		self.y_hold = self.pid_hold.get_y()
+		# Process state
-		self.y_heat = self.pid_rate.get_y()
+		state_next = self.state
+		if self.state == States.HOLD:
+			if (self.theta_ist + 1.0) < self.theta_soll_set:
+				state_next = States.HEAT
+				self.pid_rate.reset()
+		elif self.state == States.HEAT:
+			if (self.theta_ist + 1.0) >= self.theta_soll_set:
+				state_next = States.HOLD
+				self.pid_hold.reset()
 
-	def get_power_hold(self):
+		if self.state != States.IDLE:
-		return self.y_hold
+			self.pid_hold.process(self.dt, self.params['Hold']['Pid'], theta_err)
+			self.pid_rate.process(self.dt, self.params['Heat']['Pid'], heatrate_err)
 
-	def get_power_heat(self):
+		if self.state == States.HEAT:
-		return self.y_heat
+			self.y = self.pid_rate.get_y()
+		else:
+			self.y = self.pid_hold.get_y()
+
+		if state_next != self.state:
+			print("New state = {}".format(state_next))
+
+		self.state = state_next
+
+	def get_power(self):
+		return self.y
 
 
 if __name__ == '__main__':
@@ -54,16 +93,16 @@ if __name__ == '__main__':
 		"dt": 1.0,
 		"Hold": {
 			"Pid": {
-				"kp": 0.8,
+				"kp": 0.5,
-				"ki": 0.008,
+				"ki": 0.005,
 				"kd": 0.0,
 				"rho": 1.0
 			}
 		},
 		"Heat": {
 			"Pid": {
-				"kp": 0.002,
+				"kp": 0.1,
-				"ki": 0.0002,
+				"ki": 0.01,
 				"kd": 0.0,
 				"rho": 1.0
 			}
@@ -80,19 +119,28 @@ if __name__ == '__main__':
 		"kn"			: 0.2
 	}
 
+	# Kalman Filter
+	kalman_params = {
+		"dt"			: 1.0,
+		"var_P"				: 1,
+		"var_Q"				: 0.0001,
+		"var_R"				: 1
+	}
 	temp_ist = 0
 	temp_soll = 20
-	ctrl = TempController(params)
+	ctrl = TempController(params, kalman_params)
 	plant = Pot(pot_params)
 	_temp_ist = np.empty(0)
 	_temp_soll = np.empty(0)
 	_y = np.empty(0)
 	_fb = np.empty(0)
 	_t = np.empty(0)
+	_heatrate_ist_kalman = np.empty(0)
+	_temp_ist_kalman = np.empty(0)
 	a = 0.5
 	fb = 0
 	rho = 0.02
-	temps = [{'Temp': 20, 'Duration': 1000}, {'Temp': 40, 'Duration': 1000}, {'Temp': 50, 'Duration': 1000}, {'Temp': 60, 'Duration': 1000}, {'Temp': 70, 'Duration': 1000}, {'Temp': 80, 'Duration': 1000}]
+	temps = [{'Temp': 20, 'Duration': 1000}, {'Temp': 40, 'Duration': 1000}, {'Temp': 50, 'Duration': 1000}, {'Temp': 60, 'Duration': 1000}, {'Temp': 70, 'Duration': 1000}, {'Temp': 80, 'Duration': 1000}, {'Temp': 78, 'Duration': 1000}]
 
 	t = 0
 	for temp in temps:
@@ -100,6 +148,7 @@ if __name__ == '__main__':
 		hold_counter = temp['Duration']
 		hold = False
 		ctrl.set_theta_soll(temp_soll)
+		ctrl.set_heatrate_soll(2.0)
 
 		while True:
 			if hold:
@@ -108,11 +157,13 @@ if __name__ == '__main__':
 				hold_counter -= 1
 			ctrl.process()
 			plant.process()
-			y = max(0, 3500*ctrl.get_power_hold())
+			temp_ist = plant.getTemperature() + 0.0*np.random.randn()
-			plant.setPower(y)
-			temp_ist = round(plant.getTemperature(), 1)
-			fb = plant.getPower()
 			ctrl.set_theta_ist(temp_ist)
+
+			y = 3500*ctrl.get_power()
+			power = max(0, 0+y)
+			plant.setPower(power)
+			fb = plant.getPower()
 			if abs(temp_ist - temp_soll) < 0.1:
 				hold = True
 
@@ -122,14 +173,22 @@ if __name__ == '__main__':
 			_y = np.append(_y, y)
 			_fb = np.append(_fb, fb)
 			_t = np.append(_t, t)
+			_heatrate_ist_kalman = np.append(_heatrate_ist_kalman, min(3000, ctrl.heatrate_ist))
+			_temp_ist_kalman = np.append(_temp_ist, ctrl.theta_ist)
 			t += 1
 
 	figure(1)
-	subplot(2, 1, 1)
+	subplot(3, 1, 1)
 	plot(_t, _temp_ist, _t, _temp_soll, 'r-', linewidth=1)
+	legend(["ist", "soll"])
 	grid(True)
-	subplot(2, 1, 2)
+	subplot(3, 1, 2)
-	plot(_t, _y, 'bx', _t, _fb, '-r', linewidth=1)
+	plot(_t, _y, '-b', _t, _fb, '-r', linewidth=1)
+	legend(["y", "pot"])
+	grid(True)
+	subplot(3, 1, 3)
+	plot(_t, _heatrate_ist_kalman, '-b', linewidth=1)
+	legend(["heatrate"])
 	grid(True)
 	show()