- added Kalman to brewpi controller
git-svn-id: http://moon:8086/svn/projects/HendiControl@211 fda53097-d464-4ada-af97-ba876c37ca34
This commit is contained in:
@@ -13,12 +13,12 @@ def results_plot(self):
|
||||
|
||||
figure(1)
|
||||
subplot(4, 1, 1)
|
||||
plot(self.time_v, self.theta_v, 'b-', linewidth=1)
|
||||
plot(self.time_v, self.theta_v, 'b-', self.time_v, self.theta_k_v, 'r-', linewidth=1)
|
||||
title('Temperature')
|
||||
grid(True)
|
||||
ylabel('°C')
|
||||
subplot(4, 1, 2)
|
||||
plot(self.time_v, self.power_v, 'r-', linewidth=1)
|
||||
plot(self.time_v, self.power_v, 'b-', linewidth=1)
|
||||
title('Power')
|
||||
grid(True)
|
||||
ylabel('W')
|
||||
@@ -28,7 +28,7 @@ def results_plot(self):
|
||||
grid(True)
|
||||
ylabel('°C')
|
||||
subplot(4, 1, 4)
|
||||
plot(self.time_v, self.heatrate_v, 'r-', linewidth=1)
|
||||
plot(self.time_v, self.heatrate_v, 'b-', self.time_v, self.heatrate_k_v, 'r-', linewidth=1)
|
||||
title('Heatrate')
|
||||
grid(True)
|
||||
ylabel('°C/min')
|
||||
|
||||
@@ -5,6 +5,7 @@ from pid import Pid
|
||||
from utils import Smoother, Stable
|
||||
import time
|
||||
import threading
|
||||
from kalman import Kalman
|
||||
|
||||
ovenStates = Enum('ovenStates', 'NOP HEAT HOLD')
|
||||
controllerStates = Enum('controllerStates', 'ACQU TRACK')
|
||||
@@ -22,10 +23,12 @@ class Controller():
|
||||
self.params = params
|
||||
self.pid = Pid()
|
||||
self.timer_ist = 0
|
||||
self.theta_ist = 0
|
||||
self.heatrate_ist = 0
|
||||
self.theta = 0
|
||||
self.heatrate = 0
|
||||
self.theta_v = np.empty(0)
|
||||
self.theta_k_v = np.empty(0)
|
||||
self.heatrate_v = np.empty(0)
|
||||
self.heatrate_k_v = np.empty(0)
|
||||
self.time_v = np.empty(0)
|
||||
self.power_v = np.empty(0)
|
||||
self.error_v = np.empty(0)
|
||||
@@ -40,10 +43,19 @@ class Controller():
|
||||
self.is_restart = False
|
||||
self.receipe = None
|
||||
self.theta_sm = Smoother(1.0)
|
||||
self.heatrate_sm = Smoother(0.5)
|
||||
self.heatrate_sm = Smoother(0.01)
|
||||
self.theta_err_sm = Smoother(1.0)
|
||||
self.heatrate_err_sm = Smoother(1.0)
|
||||
|
||||
kparams = {
|
||||
'dt': 1,
|
||||
'var_P': 0,
|
||||
'var_Q': 0.00001,
|
||||
'var_R': 2,
|
||||
'var_Z': 0
|
||||
}
|
||||
self.kalman = Kalman(kparams)
|
||||
|
||||
def log(self, s):
|
||||
now = time.time()
|
||||
print("{:.2f}: {}".format(now, s))
|
||||
@@ -52,8 +64,8 @@ class Controller():
|
||||
now = time.time()
|
||||
if (now - self.report_last_time) >= self.report_interval:
|
||||
|
||||
self.log("Temp IST = {:0.1f} °C".format(self.theta_ist))
|
||||
self.log("Heatrate IST = {:0.1f} °C/min.".format(self.heatrate_ist))
|
||||
self.log("Temp IST = {:0.1f} °C".format(self.theta))
|
||||
self.log("Heatrate IST = {:0.1f} °C/min.".format(self.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))
|
||||
@@ -123,7 +135,7 @@ class Controller():
|
||||
self.stirrer.setCycleTime(self.stirrCycleTime)
|
||||
|
||||
# Temperature
|
||||
self.theta_ist = self.plant.getTemperature()
|
||||
self.theta = self.plant.getTemperature()
|
||||
theta_soll = rast['temp']
|
||||
heatrate_soll = rast['heatRate']
|
||||
self.log("Target temperature {} °C".format(theta_soll))
|
||||
@@ -132,10 +144,15 @@ class Controller():
|
||||
self.timer_ist = 0
|
||||
timer_soll = 60*rast['time']
|
||||
|
||||
# Kalman filter initial value
|
||||
self.kalman.initial((self.theta, 0))
|
||||
self.theta_sm.initial(self.theta)
|
||||
|
||||
# ------------------------------
|
||||
# The loop
|
||||
# ------------------------------
|
||||
doLoop = True
|
||||
useKalman = True
|
||||
while(doLoop):
|
||||
|
||||
time_start = time.time()
|
||||
@@ -148,35 +165,53 @@ class Controller():
|
||||
self.plant.process()
|
||||
|
||||
theta = self.plant.getTemperature()
|
||||
|
||||
# Process Kalman
|
||||
Z = self.kalman.process_measurement((theta, 0))
|
||||
xp = self.kalman.process(Z)
|
||||
theta_ist_k = xp[0, 0]
|
||||
dtheta_ist_k = xp[1, 0]
|
||||
self.theta_k_v = np.append(self.theta_k_v, theta_ist_k)
|
||||
self.heatrate_k_v = np.append(self.heatrate_k_v, dtheta_ist_k * 60 / self.dt)
|
||||
|
||||
# Process conventional
|
||||
theta_ist = self.theta_sm.process(theta)
|
||||
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)
|
||||
|
||||
heatrate = 60/self.dt * (theta_ist - self.theta_ist)
|
||||
heatrate_ist = self.heatrate_sm.process(heatrate)
|
||||
if 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:
|
||||
theta_err = theta_soll - theta_ist
|
||||
dtheta_err = heatrate_soll - dtheta_ist
|
||||
ctrl_theta_err = self.theta_err_sm.process(theta_err)
|
||||
ctrl_heatrate_err = self.heatrate_err_sm.process(60/self.dt * dtheta_err)
|
||||
ctrl_theta = theta_ist
|
||||
ctrl_dtheta = dtheta_ist
|
||||
|
||||
theta_err = theta_soll - theta_ist
|
||||
heatrate_err = heatrate_soll - heatrate_ist
|
||||
|
||||
self.theta_err_sm.process(theta_err)
|
||||
self.heatrate_err_sm.process(heatrate_err)
|
||||
|
||||
if self.ovenState == ovenStates.NOP:
|
||||
if theta_ist < theta_soll:
|
||||
if ctrl_theta < theta_soll:
|
||||
ovenStateNext = ovenStates.HEAT
|
||||
|
||||
pid_err = 0
|
||||
if self.ovenState == ovenStates.HEAT:
|
||||
pid_err = self.heatrate_err_sm.get_y()
|
||||
pid_err = ctrl_heatrate_err
|
||||
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 + 1.0) >= theta_soll:
|
||||
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 = self.theta_err_sm.get_y()
|
||||
pid_err = ctrl_theta_err
|
||||
pid_params_acqu = self.params['Hold']['Acqu']['Pid']
|
||||
pid_params_track = self.params['Hold']['Track']['Pid']
|
||||
pid_thresh_acqu = 0.2
|
||||
@@ -202,8 +237,6 @@ class Controller():
|
||||
power = max(self.params['P_min'], min(self.params['P_max'], self.params['P_max']*y))
|
||||
self.plant.setPower(power)
|
||||
|
||||
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)
|
||||
@@ -234,8 +267,8 @@ class Controller():
|
||||
time_to_sleep = 0
|
||||
|
||||
self.time += self.dt
|
||||
self.theta_ist = theta_ist
|
||||
self.heatrate_ist = heatrate_ist
|
||||
self.theta = ctrl_theta
|
||||
self.heatrate = ctrl_dtheta * 60 / self.dt
|
||||
|
||||
if self.timer_ist > 0:
|
||||
self.timer_ist -= self.dt
|
||||
|
||||
Reference in New Issue
Block a user