- Controller switched to timer mode

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

Control/brewpi/brewpi.py

@@ -5,7 +5,7 @@ import json
 from controller import Controller
 from mass import Mass
 from stirrer import Stirrer
-from pololu1376 import Pololu1376
+#from pololu1376 import Pololu1376
 
 from matplotlib.pyplot import figure, clf, plot, xlabel, ylabel, xlim, ylim, title, grid, axes, show, subplot
 
@@ -15,17 +15,16 @@ def results_plot(self):
 
 	for n in range(0, len(self.time_v)):
 		d = self.time_v[n]
-		v1 = self.theta_raw_v[n]
+		v1 = self.theta_v[n]
-		v2 = self.theta_k_v[n]
+		v2 = self.heatrate_v[n]
 		v3 = self.power_v[n]
 		v4 = self.error_v[n]
-		v5 = self.heatrate_k_v[n]
+		fp.write("{:6.3f} {:6.3f} {:6.3f} {:6.3f} {:6.3f}\n".format(d, v1, v2, v3, v4,))
-		fp.write("{:6.3f} {:6.3f} {:6.3f} {:6.3f} {:6.3f} {:6.3f}\n".format(d, v1, v2, v3, v4, v5))
 
 	fp.close()
 	figure(1)
 	subplot(4, 1, 1)
-	plot(self.time_v, self.theta_raw_v, 'g-', self.time_v, self.theta_k_v, 'r-', linewidth=1)
+	plot(self.time_v, self.theta_v, 'r-', linewidth=1)
 	title('Temperature')
 	grid(True)
 	ylabel('°C')
@@ -40,7 +39,7 @@ def results_plot(self):
 	grid(True)
 	ylabel('°C')
 	subplot(4, 1, 4)
-	plot(self.time_v, self.heatrate_k_v, 'r-', linewidth=1)
+	plot(self.time_v, self.heatrate_v, 'r-', linewidth=1)
 	title('Heatrate')
 	grid(True)
 	ylabel('°C/min')

Control/brewpi/controller.py

@@ -6,15 +6,18 @@ from utils import Smoother, Stable
 import time
 import threading
 from kalman import Kalman
+from timer import Timer, TimerManager, TimerListener
+
 
 ovenStates = Enum('ovenStates', 'NOP HEAT HOLD')
 controllerStates = Enum('controllerStates', 'ACQU TRACK')
 
 
-class Controller():
+class Controller(TimerListener):
 
 	def __init__(self, params, plant, stirrer):
 		print(json.dumps({'Controller': params}, indent=4, sort_keys=True))
+		self.sim_warp_factor = params['sim_warp_factor']
 		self.ovenState = ovenStates.NOP
 		self.controllerState = controllerStates.ACQU
 		self.plant = plant
@@ -23,8 +26,7 @@ class Controller():
 		self.params = params
 		self.pid = Pid()
 		self.timer_ist = 0
-		self.theta = 0
+		self.sensorTime_v = np.empty(0)
-		self.heatrate = 0
 		self.theta_raw_v = np.empty(0)
 		self.theta_v = np.empty(0)
 		self.theta_k_v = np.empty(0)
@@ -34,9 +36,7 @@ class Controller():
 		self.power_v = np.empty(0)
 		self.error_v = np.empty(0)
 		self.time = 0
-		self.sim_warp_factor = params['sim_warp_factor']
 		self.report_interval = 1.0
-		self.report_last_time = 0
 		self.thread = None
 		self.rast_abort = False
 		self.receipe_sema = threading.Semaphore(0)
@@ -49,29 +49,68 @@ class Controller():
 		self.heatrate_err_sm = Smoother(1.0)
 		self.useKalman = params['useKalman']
 		self.kalman = Kalman(params['kalman'])
+		self.sensor_dt = params['kalman']['dt']
+		self.timerMgr = TimerManager()
+
+		self.rastTimer = Timer("Rast", self)
+		self.sensorTimer = Timer("Sensor", self)
+		self.reportTimer = Timer("Report", self)
+		self.timerMgr.registerTimer(self.sensorTimer)
+		self.timerMgr.registerTimer(self.reportTimer)
+		self.rast_running = False
+
+	def onTimer(self, timer):
+		if timer == self.sensorTimer:
+			self.sensorTime_v = np.append(self.sensorTime_v, timer.count*self.sensor_dt / 60)
+
+			# Process plant
+			self.plant.process()
+
+			# Temperature
+			self.theta_raw = self.plant.getTemperature()
+
+			if timer.count == 0:
+				# Kalman filter initial value
+				self.kalman.initial((self.theta_raw, 0))
+				self.theta_sm.initial(self.theta_raw)
+
+			self.theta_raw_v = np.append(self.theta_raw_v, self.theta_raw)
+
+			# Process Kalman
+			Z = self.kalman.process_measurement((self.theta_raw, 0))
+			xp = self.kalman.process(Z)
+			self.theta_ist_k = xp[0, 0]
+			self.heatrate_ist_k = xp[1, 0] * 60 / self.sensor_dt
+			self.theta_k_v = np.append(self.theta_k_v, self.theta_ist_k)
+			self.heatrate_k_v = np.append(self.heatrate_k_v, self.heatrate_ist_k)
+
+		if timer == self.reportTimer:
+			self.report()
 
 	def log(self, s):
 		now = time.time()
 		print("{:.2f}: {}".format(now, s))
 
 	def report(self):
-		now = time.time()
+		if self.rast_running:
-		if (now - self.report_last_time) >= self.report_interval:
+			self.log("Temp SOLL {} °C".format(self.ctrl_theta_soll))
-
+			self.log("Temp IST = {:0.1f} °C".format(self.ctrl_theta))
-			self.log("Temp IST = {:0.1f} °C".format(self.theta))
+			self.log("Heatrate SOLL = {:0.1f} °C/min.".format(self.ctrl_heatrate_soll))
-			self.log("Heatrate IST = {:0.1f} °C/min.".format(self.heatrate))
+			self.log("Heatrate IST = {:0.1f} °C/min.".format(self.ctrl_heatrate))
 			self.log("Power = {:0.0f} W".format(self.plant.getPower()))
 			if self.timer_ist > 0:
 				self.log("Rast remaining : {:0.1f} min.".format(self.timer_ist/60))
 
-			# self.log("theta_err = {:0.1f} °C".format(theta_err))
-			self.report_last_time = now
 
 	def start(self, recipe):
 		self.is_pause = False
 		self.receipe = recipe
 		print(json.dumps({recipe['Name'] : recipe}, indent=4, sort_keys=True))
 
+		self.sensorTimer.start(self.sensor_dt/self.sim_warp_factor)
+		self.reportTimer.start(self.report_interval)
+		self.rastTimer.start(self.dt/self.sim_warp_factor)
+
 		self.thread = threading.Thread(target=self.receipe_run, args=(recipe,))
 		self.thread.start()
 
@@ -112,178 +151,142 @@ class Controller():
 		self.stirrDutyRast = recipe['stirrDutyRast']
 		self.stirrCycleTime = recipe['stirrCycleTime']
 
+		# Stirrer and plant
+		self.stirrer.activate()
+		self.plant.activate(True)
+
+		self.stirrer.setCycleTime(self.stirrCycleTime)
+
 		self.time = 0
+		rasten = recipe['Rasten']
+		rasten.append(None)
+		rast_count = 0
+		self.timer_ist = 0
+		while(not self.rast_abort):
+			self.timerMgr.process()
+			if self.rastTimer.isElapsed():
+				rast = rasten[rast_count]
+				if rast is not None:
+					isFinished = self.rast(rast)
+					if isFinished:
+						rast_count += 1
+						self.timer_ist = 0
+				else:
+					break
+				self.rast_running = True
 
-		for rast in recipe['Rasten']:
+			waut = self.timerMgr.getMinTimeout()
-			self.rast(rast)
+			time.sleep(waut)
-			if self.rast_abort:
-				break
-			time.sleep(1);
 
-		if not self.is_restart:
+		self.rast_running = False
-			self.receipe_sema.release()
+		# Stirrer and plant
+		self.stirrer.deactivate()
+		self.plant.activate(False)
+
+		self.receipe_sema.release()
 
 	def rast(self, rast):
 
 		# Temperature
-		theta = self.plant.getTemperature()
+		self.ctrl_theta_soll = rast['temp']
-		theta_soll = rast['temp']
+		self.ctrl_theta = self.theta_ist_k
-		heatrate_soll = rast['heatRate']
+		self.ctrl_heatrate_soll = rast['heatRate']
-		self.log("Target temperature {} °C".format(theta_soll))
+		self.ctrl_heatrate = self.heatrate_ist_k
 
 		# Timer
-		self.timer_ist = 0
 		timer_soll = 60*rast['time']
 
-		# Kalman filter initial value
-		self.kalman.initial((theta, 0))
-		self.theta_sm.initial(theta)
-
 		# ------------------------------
 		# The loop
 		# ------------------------------
-		doLoop = theta_soll > theta
+		if self.ctrl_theta_soll < self.ctrl_theta:
-		if doLoop:
+			return True
-			# Stirrer
-			self.stirrer.activate()
-			self.plant.activate(True)
 
-			self.stirrer.setCycleTime(self.stirrCycleTime)
+		controllerStateNext = self.controllerState
+		ovenStateNext = self.ovenState
 
-		while(doLoop):
+		# -----------------------------------------
+		self.stirrer.process()
 
-			time_start = time.time()
+		ctrl_theta = self.ctrl_theta
+		ctrl_heatrate = self.ctrl_heatrate
 
-			controllerStateNext = self.controllerState
+		ctrl_theta_err = self.ctrl_theta_soll - ctrl_theta
-			ovenStateNext = self.ovenState
+		ctrl_heatrate_err = self.ctrl_heatrate_soll - ctrl_heatrate
 
-			# -----------------------------------------
+		if self.ovenState == ovenStates.NOP:
-			self.stirrer.process()
+			if ctrl_theta < self.ctrl_theta_soll:
-			self.plant.process()
+				ovenStateNext = ovenStates.HEAT
 
-			theta_raw = self.plant.getTemperature()
+		pid_err = 0
-			self.theta_raw_v = np.append(self.theta_raw_v, theta_raw)
+		if self.ovenState == ovenStates.HEAT:
+			pid_err = ctrl_heatrate_err
+			pid_params_acqu = self.params['Heat']['Acqu']['Pid']
+			pid_params_track = self.params['Heat']['Track']['Pid']
+			pid_thresh_acqu = self.params['Heat']['Acqu']['thresh_leave']
+			pid_thresh_track = self.params['Heat']['Track']['thresh_leave']
+			if (ctrl_theta + 1.0) >= self.ctrl_theta_soll:
+				ovenStateNext = ovenStates.HOLD
+				self.log("Set rast timer to {:0.1f} min.".format(timer_soll/60))
+				self.timer_ist = timer_soll
 
-			# Process Kalman
+		if self.ovenState == ovenStates.HOLD:
-			Z = self.kalman.process_measurement((theta_raw, 0))
+			pid_err = ctrl_theta_err
-			xp = self.kalman.process(Z)
+			pid_params_acqu = self.params['Hold']['Acqu']['Pid']
-			theta_ist_k = xp[0, 0]
+			pid_params_track = self.params['Hold']['Track']['Pid']
-			dtheta_ist_k = xp[1, 0]
+			pid_thresh_acqu = self.params['Heat']['Acqu']['thresh_leave']
-			self.theta_k_v = np.append(self.theta_k_v, theta_ist_k)
+			pid_thresh_track = self.params['Heat']['Track']['thresh_leave']
-			self.heatrate_k_v = np.append(self.heatrate_k_v, dtheta_ist_k * 60 / self.dt)
+			if self.timer_ist == 0:
-
+				ovenStateNext = ovenStates.NOP
-			# Process conventional
-			theta_ist = self.theta_sm.process(theta_raw)
-			dtheta_ist = self.heatrate_sm.process(theta_ist - self.theta)
-			self.theta_v = np.append(self.theta_v, theta_ist)
-			self.heatrate_v = np.append(self.heatrate_v, dtheta_ist * 60/self.dt)
-
-			if self.useKalman:
-				ctrl_theta_err = theta_soll - theta_ist_k
-				ctrl_heatrate_err = heatrate_soll - dtheta_ist_k * 60/self.dt
-				ctrl_theta = theta_ist_k
-				ctrl_dtheta = dtheta_ist_k
-			else:
-				ctrl_theta_err = self.theta_err_sm.process(theta_soll - theta_ist)
-				ctrl_heatrate_err = self.heatrate_err_sm.process(heatrate_soll - 60/self.dt * dtheta_ist)
-				ctrl_theta = theta_ist
-				ctrl_dtheta = dtheta_ist
-
-
-			if self.ovenState == ovenStates.NOP:
-				if ctrl_theta < theta_soll:
-					ovenStateNext = ovenStates.HEAT
-
-			pid_err = 0
-			if self.ovenState == ovenStates.HEAT:
-				pid_err = ctrl_heatrate_err
-				pid_params_acqu = self.params['Heat']['Acqu']['Pid']
-				pid_params_track = self.params['Heat']['Track']['Pid']
-				pid_thresh_acqu = self.params['Heat']['Acqu']['thresh_leave']
-				pid_thresh_track = self.params['Heat']['Track']['thresh_leave']
-				if (ctrl_theta + 1.0) >= theta_soll:
-					ovenStateNext = ovenStates.HOLD
-					self.log("Set rast timer to {:0.1f} min.".format(timer_soll/60))
-					self.timer_ist = timer_soll
-
-			if self.ovenState == ovenStates.HOLD:
-				pid_err = ctrl_theta_err
-				pid_params_acqu = self.params['Hold']['Acqu']['Pid']
-				pid_params_track = self.params['Hold']['Track']['Pid']
-				pid_thresh_acqu = self.params['Heat']['Acqu']['thresh_leave']
-				pid_thresh_track = self.params['Heat']['Track']['thresh_leave']
-				if self.timer_ist == 0:
-					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_soll = max(self.params['P_min'], min(self.params['P_max'], self.params['P_max']*y))
-			self.plant.setPower(power_soll)
-			power_ist = self.plant.getPower()
-
-			self.time_v = np.append(self.time_v, self.time/60)
-			self.power_v = np.append(self.power_v, power_ist)
-			self.error_v = np.append(self.error_v, pid_err)
-			#self.error_v = np.append(self.error_v, self.stirrer.getSpeed())
-
-
-			# -----------------------------------------
-			if ovenStateNext != self.ovenState:
-				self.log("{} -> {}".format(self.ovenState, ovenStateNext))
-				self.pid.reset()
-				if ovenStateNext == ovenStates.HEAT:
-					self.stirrer.setSpeed(self.stirrSpeedHeat)
-					self.stirrer.setDutyCycle(1.0)
-
-				if ovenStateNext == ovenStates.HOLD:
-					self.stirrer.setSpeed(self.stirrSpeedRast)
-					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 = ctrl_theta
-			self.heatrate = ctrl_dtheta * 60 / self.dt
-
-			if self.timer_ist > 0:
-				self.timer_ist -= self.dt
-			else:
-				self.timer_ist = 0
-
-			self.report()
-
-			while(doLoop):
-				time.sleep(time_to_sleep)
-				if self.is_pause == False:
-					break
-				if self.rast_abort:
-					break
-			if self.rast_abort:
 				doLoop = False
 
-		self.stirrer.deactivate()
+		if self.ovenState != ovenStates.NOP:
-		self.plant.activate(False)
+			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_soll = max(self.params['P_min'], min(self.params['P_max'], self.params['P_max']*y))
+		self.plant.setPower(power_soll)
+		power_ist = self.plant.getPower()
+
+		self.time_v = np.append(self.time_v, self.time/60)
+		self.theta_v = np.append(self.theta_v, ctrl_theta)
+		self.heatrate_v = np.append(self.heatrate_v, ctrl_heatrate)
+		self.power_v = np.append(self.power_v, power_ist)
+		self.error_v = np.append(self.error_v, pid_err)
+
+		# -----------------------------------------
+		if ovenStateNext != self.ovenState:
+			self.log("{} -> {}".format(self.ovenState, ovenStateNext))
+			self.pid.reset()
+			if ovenStateNext == ovenStates.HEAT:
+				self.stirrer.setSpeed(self.stirrSpeedHeat)
+				self.stirrer.setDutyCycle(1.0)
+
+			if ovenStateNext == ovenStates.HOLD:
+				self.stirrer.setSpeed(self.stirrSpeedRast)
+				self.stirrer.setDutyCycle(self.stirrDutyRast)
+
+		if controllerStateNext != self.controllerState:
+			self.log("{} -> {}".format(self.controllerState, controllerStateNext))
+
+		self.ovenState = ovenStateNext
+		self.controllerState = controllerStateNext
+		# ------------------------------
+		self.time += self.dt
+
+		if self.timer_ist > 0:
+			self.timer_ist -= self.dt
+		else:
+			self.timer_ist = 0
+