- reworked
git-svn-id: http://moon:8086/svn/projects/HendiControl@113 fda53097-d464-4ada-af97-ba876c37ca34
This commit is contained in:
@@ -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
|
||||
|
||||
# ------------------------------
|
||||
Reference in New Issue
Block a user