jens
1a06a6dacc
git-svn-id: http://moon:8086/svn/projects/HendiControl@375 fda53097-d464-4ada-af97-ba876c37ca34
286 lines
8.8 KiB
Python
286 lines
8.8 KiB
Python
import numpy as np
|
|
import json
|
|
from enum import Enum
|
|
from pid import Pid
|
|
from utils import Smoother, Stable
|
|
import time
|
|
import threading
|
|
from kalman import Kalman
|
|
from timer import Timer, TimerManager, TimerListener
|
|
import queue
|
|
|
|
ovenStates = Enum('ovenStates', 'IDLE HEAT HOLD')
|
|
states = Enum('controllerStates', 'IDLE NORMAL PAUSE')
|
|
commands = Enum('controllerCommands', 'RUN PAUSE ABORT STIRR_OFF STIRR_ON')
|
|
|
|
class Controller(TimerListener):
|
|
|
|
def __init__(self, params, plant, stirrer, recipe):
|
|
print(json.dumps({'Controller': params}, indent=4, sort_keys=True))
|
|
self.sim_warp_factor = params['sim_warp_factor']
|
|
self.ovenState = ovenStates.IDLE
|
|
self.plant = plant
|
|
self.stirrer = stirrer
|
|
self.dt = params['dt']
|
|
self.params = params
|
|
self.pid_hold = Pid()
|
|
self.pid_rate = Pid()
|
|
self.sensorTime_v = np.empty(0)
|
|
self.theta_raw_v = np.empty(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_soll = 0
|
|
self.power_v = np.empty(0)
|
|
self.power_heat_v = np.empty(0)
|
|
self.power_hold_v = np.empty(0)
|
|
self.error_v = np.empty(0)
|
|
self.time = 0
|
|
self.report_interval = 1.0
|
|
self.thread = None
|
|
self.receipe_sema = threading.Semaphore(0)
|
|
self.receipe = recipe
|
|
self.rasten = recipe['Rasten']
|
|
self.rasten.append(None)
|
|
self.theta_sm = Smoother(1.0)
|
|
self.heatrate_sm = Smoother(0.01)
|
|
self.theta_err_sm = Smoother(1.0)
|
|
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.processTimer = Timer("Process", self)
|
|
self.rastTimer = Timer("Rast", self)
|
|
self.sensorTimer = Timer("Sensor", self)
|
|
self.plantTimer = Timer("Plant", self)
|
|
self.reportTimer = Timer("Report", self)
|
|
self.timerMgr.registerTimer(self.processTimer)
|
|
self.timerMgr.registerTimer(self.rastTimer)
|
|
self.timerMgr.registerTimer(self.sensorTimer)
|
|
self.timerMgr.registerTimer(self.plantTimer)
|
|
self.timerMgr.registerTimer(self.reportTimer)
|
|
self.power_hold_offset = 200
|
|
self.power_heat_offset = 1500
|
|
self.queue = queue.Queue()
|
|
self.rast_done = False
|
|
self.receipe_done = False
|
|
self.thread = threading.Thread(target=self.main)
|
|
self.thread.start()
|
|
self.fp_log = None
|
|
|
|
# Stirrer
|
|
self.stirrSpeedHeat = recipe['stirrSpeedHeat']
|
|
self.stirrSpeedRast = recipe['stirrSpeedRast']
|
|
self.stirrDutyRast = recipe['stirrDutyRast']
|
|
self.stirrCycleTime = recipe['stirrCycleTime']
|
|
|
|
print(json.dumps({recipe['Name'] : recipe}, indent=4, sort_keys=True))
|
|
|
|
def command(self, cmd):
|
|
self.queue.put_nowait(cmd)
|
|
|
|
def onTimer(self, timer):
|
|
if timer == self.sensorTimer:
|
|
self.sensorTime_v = np.append(self.sensorTime_v, timer.count*self.sensor_dt / 60)
|
|
|
|
# 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.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)
|
|
|
|
elif timer == self.plantTimer:
|
|
# Process plant
|
|
self.plant.process()
|
|
|
|
# Process stirrer
|
|
self.stirrer.process()
|
|
|
|
elif timer == self.reportTimer:
|
|
self.report()
|
|
|
|
elif timer == self.processTimer:
|
|
rast = self.rasten[self.rast_index]
|
|
if rast is not None:
|
|
self.ctrl_theta_soll = rast['temp']
|
|
self.ctrl_heatrate_soll = rast['heatRate']
|
|
isFinished = self.rast(rast)
|
|
if isFinished:
|
|
if not self.rastTimer.isActive and not self.rast_done:
|
|
rast_timer_soll = 60 * rast['time']
|
|
self.log ("Start rast timer with {} min.".format(rast_timer_soll/60))
|
|
self.rastTimer.start(rast_timer_soll / self.sim_warp_factor, mode='oneshot')
|
|
self.rast_done = True
|
|
else:
|
|
self.receipe_done = True
|
|
|
|
elif timer == self.rastTimer:
|
|
self.log("Rast timer elapsed")
|
|
rast = self.rasten[self.rast_index]
|
|
if not rast["waitForUser"]:
|
|
self.rast_index += 1
|
|
self.rast_done = False
|
|
self.ovenState = ovenStates.IDLE
|
|
else:
|
|
self.log("Wait user input!")
|
|
|
|
def log(self, s):
|
|
now = time.strftime("%a, %d %b %Y %H:%M:%S", time.localtime())
|
|
print("{}: {}".format(now, s))
|
|
self.fp_log.write("{}: {}\n".format(now, s))
|
|
|
|
def report(self):
|
|
self.log("Temp SOLL {} °C".format(self.ctrl_theta_soll))
|
|
self.log("Temp IST = {:0.1f} °C".format(self.ctrl_theta))
|
|
self.log("Heatrate SOLL = {:0.1f} °C/min.".format(self.ctrl_heatrate_soll))
|
|
self.log("Heatrate IST = {:0.1f} °C/min.".format(self.ctrl_heatrate))
|
|
self.log("Power = {:0.0f} W".format(self.power_soll))
|
|
if self.rastTimer.isActive:
|
|
self.log("Rast remaining : {:0.1f} min.".format(self.rastTimer.elapseDuration / 60))
|
|
|
|
def wait_finished(self):
|
|
self.receipe_sema.acquire()
|
|
pass
|
|
|
|
def main(self):
|
|
self.sensorTimer.start(self.sensor_dt/self.sim_warp_factor)
|
|
self.plantTimer.start(1/self.sim_warp_factor)
|
|
|
|
self.stirrer.deactivate()
|
|
self.plant.activate(False)
|
|
|
|
run = True
|
|
while(run and not self.receipe_done):
|
|
self.timerMgr.process()
|
|
wait = self.timerMgr.getMinTimeout(1.0/self.sim_warp_factor)
|
|
try:
|
|
msg = self.queue.get(block=True, timeout=wait)
|
|
print ("Received: ", msg)
|
|
except:
|
|
continue
|
|
|
|
if msg == commands.PAUSE:
|
|
pass
|
|
elif msg == commands.RUN:
|
|
self.stirrer.activate()
|
|
self.plant.activate(True)
|
|
if not self.processTimer.isActive:
|
|
self.fp_log = open(self.receipe['Name'] + '.log', "w")
|
|
self.time = 0
|
|
self.rast_index = 0
|
|
self.reportTimer.start(self.report_interval)
|
|
self.processTimer.start(self.dt / self.sim_warp_factor)
|
|
|
|
# Stirrer and plant
|
|
self.stirrer.setCycleTime(self.stirrCycleTime)
|
|
self.receipe_done = False
|
|
else:
|
|
self.rast_index += 1
|
|
self.rast_done = False
|
|
self.ovenState = ovenStates.IDLE
|
|
|
|
elif msg == commands.ABORT:
|
|
run = False
|
|
|
|
elif msg == commands.STIRR_ON:
|
|
self.stirrer.activate()
|
|
|
|
elif msg == commands.STIRR_OFF:
|
|
self.stirrer.deactivate()
|
|
|
|
# Stirrer and plant
|
|
self.stirrer.deactivate()
|
|
self.plant.activate(False)
|
|
self.fp_log.close()
|
|
self.receipe_sema.release()
|
|
|
|
def rast(self, rast):
|
|
|
|
# Temperature
|
|
self.ctrl_theta = self.theta_ist_k
|
|
self.ctrl_heatrate = self.heatrate_ist_k
|
|
|
|
ovenStateNext = self.ovenState
|
|
|
|
# -----------------------------------------
|
|
ctrl_theta = self.ctrl_theta
|
|
ctrl_heatrate = self.ctrl_heatrate
|
|
|
|
ctrl_theta_err = self.ctrl_theta_soll - ctrl_theta
|
|
ctrl_heatrate_err = self.ctrl_heatrate_soll - ctrl_heatrate
|
|
|
|
if self.ovenState != ovenStates.IDLE:
|
|
self.pid_hold.process(self.dt, self.params['Hold']['Pid'], ctrl_theta_err)
|
|
self.pid_rate.process(self.dt, self.params['Heat']['Pid'], ctrl_heatrate_err)
|
|
|
|
power_hold = self.power_hold_offset + self.params['P_max'] * self.pid_hold.get_y()
|
|
power_heat = self.power_heat_offset + self.params['P_max'] * self.pid_rate.get_y()
|
|
|
|
power_hold = max(self.params['P_min'], min(self.params['P_max'], power_hold))
|
|
power_heat = max(self.params['P_min'], min(self.params['P_max'], power_heat))
|
|
|
|
self.power_soll = power_hold
|
|
if self.ovenState == ovenStates.HEAT:
|
|
self.power_soll = power_heat
|
|
|
|
self.plant.setPower(self.power_soll)
|
|
|
|
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, self.power_soll)
|
|
self.power_hold_v = np.append(self.power_hold_v, power_hold)
|
|
self.power_heat_v = np.append(self.power_heat_v, power_heat)
|
|
self.error_v = np.append(self.error_v, ctrl_theta_err)
|
|
|
|
self.time += self.dt
|
|
|
|
# -----------------------------------------
|
|
rast_finished = False
|
|
|
|
if self.ovenState == ovenStates.IDLE:
|
|
if (ctrl_theta + 1.0) < self.ctrl_theta_soll:
|
|
ovenStateNext = ovenStates.HEAT
|
|
else:
|
|
ovenStateNext = ovenStates.HOLD
|
|
|
|
if self.ovenState == ovenStates.HEAT:
|
|
if (ctrl_theta + 1.0) >= self.ctrl_theta_soll:
|
|
ovenStateNext = ovenStates.HOLD
|
|
|
|
if ovenStateNext != self.ovenState:
|
|
self.log("{} -> {}".format(self.ovenState, ovenStateNext))
|
|
if ovenStateNext == ovenStates.HEAT:
|
|
self.pid_rate.reset()
|
|
self.stirrer.setSpeed(self.stirrSpeedHeat)
|
|
self.stirrer.setDutyCycle(1.0)
|
|
if self.ovenState == ovenStates.HOLD:
|
|
self.power_hold_offset = power_hold
|
|
|
|
if ovenStateNext == ovenStates.HOLD:
|
|
self.pid_hold.reset()
|
|
self.stirrer.setSpeed(self.stirrSpeedRast)
|
|
self.stirrer.setDutyCycle(self.stirrDutyRast)
|
|
if self.ovenState == ovenStates.HEAT:
|
|
self.power_heat_offset = power_heat
|
|
|
|
# ------------------------------
|
|
self.rast_running = True
|
|
self.ovenState = ovenStateNext
|
|
return self.ovenState == ovenStates.HOLD
|