components/pid/temp_controller.py

@@ -1,64 +1,18 @@
 from components.plant.pot import Pot
 from matplotlib.pyplot import plot, figure, subplot, grid, show, legend
-from components import APid
-from components.pid import Pid, Kalman
+from components.pid import Kalman
+from components.pid.temp_controller_base import TempControllerBase
 from components.pid.tc_constants import *
 import numpy as np
 
 
-class TempController(APid):
-	def __init__(self, dt, params):
-		APid.__init__(self)
-		self.pid_hold = Pid(dt)
-		self.pid_rate = Pid(dt)
-		self.theta_ist_set = 0
-		self.theta_soll_set = 0
-		self.heatrate_ist_set = 0
-		self.heatrate_soll_set = 1.0
-		self.heatrate_soll = 1.0
-		self.theta_ist = 0
-		self.heatrate_ist = 0
-		self.params = params
+class TempController(TempControllerBase):
+	def __init__(self, dt, params, model_params=None):
+		TempControllerBase.__init__(self, dt, params, model_params)
 		self.kalman = Kalman(dt, params['Kalman'])
-		self.y = -1
-		self.state = States.INIT
-		self.use_kalman = True
-		self.pid_hold.set_params(params['Hold'])
-		self.pid_rate.set_params(params['Heat'])
-		self.is_startup = True
 
-	def set_theta_ist(self, value):
-		self.theta_ist_set = value
-		if self.is_startup:
-			self.is_startup = False
-			self.kalman.initial((value, 0))
-
-	def get_theta_ist(self):
-		return self.theta_ist
-
-	def set_heatrate_ist(self, value):
-		self.heatrate_ist_set = value
-
-	def get_heatrate_ist(self):
-		return self.heatrate_ist
-
-	def set_theta_soll(self, value):
-		self.theta_soll_set = value
-
-	def get_theta_soll(self):
-		return self.theta_soll
-
-	def get_theta_soll_set(self):
-		return self.theta_soll_set
-
-	def set_heatrate_soll(self, value):
-		self.heatrate_soll_set = value
-
-	def get_heatrate_soll(self):
-		return self.heatrate_soll
-
-	def get_heatrate_soll_set(self):
-		return self.heatrate_soll_set
+	def init_kalman(self, value):
+		self.kalman.initial((value, 0))
 
 	def process(self):
 		# Process Kalman
@@ -83,48 +37,6 @@ class TempController(APid):
 		self.process_fsm(diff)
 		self.process_pid(theta_err, heatrate_err)
 
-	def process_fsm(self, diff):
-		# Process state
-		state_next = self.state
-		if self.state == States.INIT:
-			if not self.is_startup:
-				state_next = States.IDLE
-		elif self.state == States.IDLE:
-			if diff >= THRESH_IDLE_HEAT:
-				state_next = States.HEAT
-				self.pid_rate.reset()
-			elif diff >= -THRESH_IDLE_HOLD:
-				state_next = States.HOLD
-				self.pid_rate.reset()
-		elif self.state == States.HOLD:
-			if diff >= THRESH_HOLD_HEAT:
-				state_next = States.HEAT
-				self.pid_rate.reset()
-			elif diff <= -THRESH_HOLD_IDLE:
-				state_next = States.IDLE
-		elif self.state == States.HEAT:
-			if diff <= -THRESH_HEAT_IDLE:
-				state_next = States.IDLE
-			elif diff <= THRESH_HEAT_HOLD:
-				state_next = States.HOLD
-				self.pid_hold.reset()
-
-		if state_next != self.state:
-			self.state = state_next
-			print("New state = {}".format(state_next))
-
-	def process_pid(self, theta_err, heatrate_err):
-		self.pid_hold.process(theta_err, -self.theta_ist)
-		self.pid_rate.process(heatrate_err, -self.heatrate_ist)
-
-		if self.state == States.IDLE:
-			self.y = 0
-		else:
-			self.y = self.pid_rate.get_y()
-
-	def get_power(self):
-		return self.y
-
 
 if __name__ == '__main__':
 	dt = 1.0
@@ -196,4 +108,3 @@ if __name__ == '__main__':
 	show()
 
 	print("End of program")
-

components/pid/temp_controller_base.py

@@ -0,0 +1,112 @@
+from components import APid
+from components.pid.pid import Pid
+from components.pid.tc_constants import States, THRESH_HOLD_IDLE, THRESH_HOLD_HEAT, \
+	THRESH_IDLE_HEAT, THRESH_IDLE_HOLD, THRESH_HEAT_HOLD, THRESH_HEAT_IDLE
+
+
+class TempControllerBase(APid):
+	def __init__(self, dt, params, model_params=None):
+		APid.__init__(self)
+		self.pid_hold = Pid(dt)
+		self.pid_rate = Pid(dt)
+		self.theta_ist_set = 0
+		self.theta_soll_set = 0
+		self.heatrate_ist_set = 0
+		self.heatrate_soll_set = 1.0
+		self.heatrate_soll = 1.0
+		self.theta_ist = 0
+		self.heatrate_ist = 0
+		self.params = params
+		self.model_params = model_params
+		self.y = -1
+		self.state = States.INIT
+		self.use_kalman = True
+		self.pid_hold.set_params(params['Hold'])
+		self.pid_rate.set_params(params['Heat'])
+		self.is_startup = True
+
+	def init_kalman(self, value):
+		raise NotImplementedError
+
+	def on_state_entered(self, state):
+		pass
+
+	def post_pid(self):
+		pass
+
+	def set_theta_ist(self, value):
+		self.theta_ist_set = value
+		if self.is_startup:
+			self.is_startup = False
+			self.init_kalman(value)
+
+	def get_theta_ist(self):
+		return self.theta_ist
+
+	def set_heatrate_ist(self, value):
+		self.heatrate_ist_set = value
+
+	def get_heatrate_ist(self):
+		return self.heatrate_ist
+
+	def set_theta_soll(self, value):
+		self.theta_soll_set = value
+
+	def get_theta_soll(self):
+		return self.theta_soll
+
+	def get_theta_soll_set(self):
+		return self.theta_soll_set
+
+	def set_heatrate_soll(self, value):
+		self.heatrate_soll_set = value
+
+	def get_heatrate_soll(self):
+		return self.heatrate_soll
+
+	def get_heatrate_soll_set(self):
+		return self.heatrate_soll_set
+
+	def process_fsm(self, diff):
+		state_next = self.state
+		if self.state == States.INIT:
+			if not self.is_startup:
+				state_next = States.IDLE
+		elif self.state == States.IDLE:
+			if diff >= THRESH_IDLE_HEAT:
+				state_next = States.HEAT
+				self.pid_rate.reset()
+			elif diff >= -THRESH_IDLE_HOLD:
+				state_next = States.HOLD
+				self.pid_rate.reset()
+		elif self.state == States.HOLD:
+			if diff >= THRESH_HOLD_HEAT:
+				state_next = States.HEAT
+				self.pid_rate.reset()
+			elif diff <= -THRESH_HOLD_IDLE:
+				state_next = States.IDLE
+		elif self.state == States.HEAT:
+			if diff <= -THRESH_HEAT_IDLE:
+				state_next = States.IDLE
+			elif diff <= THRESH_HEAT_HOLD:
+				state_next = States.HOLD
+				self.pid_hold.reset()
+
+		if state_next != self.state:
+			self.state = state_next
+			print("New state = {}".format(state_next))
+			self.on_state_entered(state_next)
+
+	def process_pid(self, theta_err, heatrate_err):
+		self.pid_hold.process(theta_err, -self.theta_ist)
+		self.pid_rate.process(heatrate_err, -self.heatrate_ist)
+
+		if self.state == States.IDLE:
+			self.y = 0
+		else:
+			self.y = self.pid_rate.get_y()
+
+		self.post_pid()
+
+	def get_power(self):
+		return self.y

components/pid/temp_controller_smith.py

@@ -1,73 +1,42 @@
 from components.plant.pot import Pot
 from matplotlib.pyplot import plot, figure, subplot, grid, show, legend
-from components import APid
-from components.pid import Pid, Kalman
+from components.pid import Kalman
+from components.pid.temp_controller_base import TempControllerBase
 from components.pid.tc_constants import *
 import numpy as np
 
 
-class TempController(APid):
+class TempController(TempControllerBase):
 	def __init__(self, dt, params, model_params):
-		APid.__init__(self)
-		self.pid_hold = Pid(dt)
-		self.pid_rate = Pid(dt)
-		self.theta_ist_set = 0
-		self.theta_soll_set = 0
-		self.heatrate_ist_set = 0
-		self.heatrate_soll_set = 1.0
-		self.heatrate_soll = 1.0
-		self.theta_ist = 0
-		self.heatrate_ist = 0
-		self.params = params
-		self.model_params = model_params
+		TempControllerBase.__init__(self, dt, params, model_params)
 		self.kalman_model = Kalman(dt, params['Kalman'])
 		self.kalman_model_delay = Kalman(dt, params['Kalman'])
 		self.kalman_plant = Kalman(dt, params['Kalman'])
-		self.y = -1
-		self.state = States.INIT
-		self.use_kalman = True
-		self.pid_hold.set_params(params['Hold'])
-		self.pid_rate.set_params(params['Heat'])
 		self.model = Pot(dt, model_params)
-		self.is_startup = True
+
+		self.theta_ist_plant = 0
+		self.dtheta_ist_plant = 0
+		self.theta_ist_model = 0
+		self.dtheta_ist_model = 0
+		self.theta_ist_model_delay = 0
+		self.dtheta_ist_model_delay = 0
 
 	def set_model_power(self, power):
 		self.model.set_power(power)
 
-	def set_theta_ist(self, value):
-		self.theta_ist_set = value
-		if self.is_startup:
-			self.is_startup = False
-			self.kalman_model.initial((value, 0))
-			self.kalman_model_delay.initial((value, 0))
-			self.kalman_plant.initial((value, 0))
+	def init_kalman(self, value):
+		self.kalman_model.initial((value, 0))
+		self.kalman_model_delay.initial((value, 0))
+		self.kalman_plant.initial((value, 0))
 
-	def get_theta_ist(self):
-		return self.theta_ist
+	def on_state_entered(self, state):
+		if state == States.HEAT:
+			self.model.initial(self.theta_ist)
+			self.kalman_model.initial((self.theta_ist, 0))
+			self.kalman_model_delay.initial((self.theta_ist, 0))
 
-	def set_heatrate_ist(self, value):
-		self.heatrate_ist_set = value
-
-	def get_heatrate_ist(self):
-		return self.heatrate_ist
-
-	def set_theta_soll(self, value):
-		self.theta_soll_set = value
-
-	def get_theta_soll(self):
-		return self.theta_soll
-
-	def get_theta_soll_set(self):
-		return self.theta_soll_set
-
-	def set_heatrate_soll(self, value):
-		self.heatrate_soll_set = value
-
-	def get_heatrate_soll(self):
-		return self.heatrate_soll
-
-	def get_heatrate_soll_set(self):
-		return self.heatrate_soll_set
+	def post_pid(self):
+		self.model.process()
 
 	def process(self):
 		# Process Kalman of Plant
@@ -114,54 +83,6 @@ class TempController(APid):
 		self.process_fsm(diff)
 		self.process_pid(theta_err, heatrate_err)
 
-	def process_fsm(self, diff):
-		# Process state
-		state_next = self.state
-		if self.state == States.INIT:
-			if not self.is_startup:
-				state_next = States.IDLE
-		elif self.state == States.IDLE:
-			if diff >= THRESH_IDLE_HEAT:
-				state_next = States.HEAT
-				self.pid_rate.reset()
-			elif diff >= -THRESH_IDLE_HOLD:
-				state_next = States.HOLD
-				self.pid_rate.reset()
-		elif self.state == States.HOLD:
-			if diff >= THRESH_HOLD_HEAT:
-				state_next = States.HEAT
-				self.pid_rate.reset()
-			elif diff <= -THRESH_HOLD_IDLE:
-				state_next = States.IDLE
-		elif self.state == States.HEAT:
-			if diff <= -THRESH_HEAT_IDLE:
-				state_next = States.IDLE
-			elif diff <= THRESH_HEAT_HOLD:
-				state_next = States.HOLD
-				self.pid_hold.reset()
-
-		if state_next != self.state:
-			self.state = state_next
-			print("New state = {}".format(state_next))
-			if state_next == States.HEAT:
-				self.model.initial(self.theta_ist)
-				self.kalman_model.initial((self.theta_ist, 0))
-				self.kalman_model_delay.initial((self.theta_ist, 0))
-
-	def process_pid(self, theta_err, heatrate_err):
-		self.pid_hold.process(theta_err, -self.theta_ist)
-		self.pid_rate.process(heatrate_err, -self.heatrate_ist)
-
-		if self.state == States.IDLE:
-			self.y = 0
-		else:
-			self.y = self.pid_rate.get_y()
-
-		self.model.process()
-
-	def get_power(self):
-		return self.y
-
 
 if __name__ == '__main__':
 	dt = 1.0
@@ -246,4 +167,3 @@ if __name__ == '__main__':
 	show()
 
 	print("End of program")
-