gekko - fstatus=1 时 MV 的 MEAS

Question

大家和约翰教授

我们正在使用 gekko 在 tclab 仿真模型上进行 MPC。我们尝试模拟现场由于执行器问题导致执行器偏离gekko计算的MV的情况。

如果偏差处于固定模式，例如相当大的恒定偏差会长时间发生并且可能会再次出现，那么很长一段时间内都可以正常工作。我们可以通过额外的逻辑来处理它来检测偏差并将偏差值添加到gekko计算的mv中。

有一天，我注意到当 fstatus = 1 时，可能会有 MV 的测量值。所以我试了一下。我希望gekko可以自己处理偏差。例如，如果来自 gekko 的 mv 为 10 并且测量值为 5 并且模式继续，则 gekko 可能会吐出高于 10 的 MV 值，例如 15 并且测量值为 10。

在模拟中，当我设置 MV 的 fstatus=1 时，MV 的曲线变为：

q1a 是手动偏差的 q1。在上图中，q1a == q1。看起来gekko在考虑MV的效果方面又迈出了一步。

在下图中，有两个时间范围，一个是“q1a == q1+20”，另一个是“q1a == q1 -20”。q1a 的值被馈送到 tclab 和 mv(q1) 的 meas。

我不明白为什么尽管 t1 远离 sp，但当 meas 偏离时，gekko 计算的 q1 会上升或下降。

编辑：示例代码

请参阅以下“正常”HMI 的屏幕截图。缓慢的 MV 消失了，所以可能是我的代码中的错误引起的。但仍然可以看到上升或下降。 请参阅下面的代码：

from random import random
from random import randrange

import tclab
from tclab import labtime
from tclab import TCLabModel

import numpy as np
import time
import matplotlib.pyplot as plt
from gekko import GEKKO
import json

from tclab import TCLabModel

make_mp4 = True
if make_mp4:
    import imageio  # required to make animation
    import os
    try:
        os.mkdir('./figures')
    except:
        pass


class tclab_heaterpipe():
    def __init__(self,d1,d2,model):
        if(d1 >= 1 and d2 >=1):
            self.delay_q1_step = int(d1)
            self.delay_q2_step = int(d2)
            self.q1_buffer = [0] * self.delay_q1_step
            self.q2_buffer = [0] * self.delay_q2_step
            self.m = model
        else:
            self.delay_q1_step =0 
            self.delay_q2_step =0
        return

    def Q1_delay(self,q1):
        if(self.delay_q1_step == 0):
            self.m.Q1(q1)
        self.q1_buffer.insert(0,q1)
        self.m.Q1(self.q1_buffer.pop())

    def Q2_delay(self,q2):
        if(self.delay_q2_step == 0):
            self.m.Q1(q2)
        self.q2_buffer.insert(0,q2)
        self.m.Q2(self.q2_buffer.pop())

# Connect to Arduino
connected = False
theta1 = 1 
theta2 = 1 
T = tclab.setup(connected)
a = T()
tclab_delay = tclab_heaterpipe(theta1,theta2,a)
# Turn LED on
print('LED On')
a.LED(100)

# Simulate a time delay
# Run time in minutes
run_time = 80.0
# Number of cycles
loops = int(60.0*run_time)

# Temperature (K)

t1sp = 45.0
t2sp = 35.0

#########################################################
# Initialize Model
#########################################################
# use remote=True for MacOS
m = GEKKO(name='tclab-mpc',remote=False)

m.time = np.linspace(0,400,41)
step = 10

T1 = np.ones(int(loops/step)+1) * a.T1 # temperature (degC)
T2 = np.ones(int(loops/step)+1) * a.T2 # temperature (degC)
Tsp1 = np.ones(int(loops/step)+1) * t1sp # set point (degC)
Tsp2 = np.ones(int(loops/step)+1) * t2sp # set point (degC)
# heater values
Q1s = np.ones(int(loops/step)+1) * 0.0 
Q2s = np.ones(int(loops/step)+1) * 0.0 

# Parameters
Q1_ss = m.Param(value=0)
TC1_ss = m.Param(value=a.T1)
Q2_ss = m.Param(value=0)
TC2_ss = m.Param(value=a.T2)
Kp1 = m.Param(value= 0.7)
tau1 = m.Param(value=160.0)
Kp2 = m.Param(value=0.05)
tau2 = m.Param(value=160.0)
Kp3= m.Param(value=0.05)
tau3 = m.Param(value=160.0)
Kp4 = m.Param(value=0.4)
tau4 = m.Param(value=200.0)
sp1 = m.Param(value=a.T1)
sp2 = m.Param(value=a.T2)

# Manipulated variable
Q1 = m.MV(value=0, name='q1')
Q1.STATUS = 1  # use to control temperature
Q1.FSTATUS = 1 # no feedback measurement
Q1.LOWER = 0.0
Q1.UPPER = 100.0
Q1.DMAX = 10.0
Q1.DCOST = 5.0

Q2 = m.MV(value=0, name='q2')
Q2.STATUS = 1  # use to control temperature
Q2.FSTATUS = 1 # no feedback measurement
Q2.LOWER = 0.0
Q2.UPPER = 100.0
Q2.DMAX = 10.0
Q2.DCOST = 5.0

# Controlled variable
TC1 = m.CV(value=a.T1, name='tc1')
TC1.STATUS = 1     # minimize error with setpoint range
TC1.FSTATUS = 1    # receive measurement
TC1.TR_INIT = 2    # reference trajectory
# TC1.COST = 0.1
TC1.WSPHI = 20
TC1.WSPLO = 20
TC1.TAU = 50        # time constant for response
#TC1.TR_OPEN = 3

TC2 = m.CV(value=a.T2, name='tc2')
TC2.STATUS = 1     # minimize error with setpoint range
TC2.FSTATUS = 1    # receive measurement
TC2.TR_INIT = 2    # reference trajectory
# TC2.COST = 0.1
TC2.WSPHI = 20
TC2.WSPLO = 20
TC2.TAU = 30       # time constant for response
#kTC2.TR_OPEN = 3


# 添加延时
Q1d=m.Var()
m.delay(Q1, Q1d, theta1)
Q2d=m.Var()
m.delay(Q2, Q2d, theta2)
# Equation
#m.Equation(tau1 * TC1.dt() + (TC1 - TC1_ss) == Kp1 * (Q1d - Q1_ss))
# m.Equation(tau2 * TC2.dt() + (TC2 - TC2_ss)  == Kp2 * (Q1d - Q1_ss))
# m.Equation(tau3 * TC1.dt() + (TC1 - TC1_ss)  == Kp3 * (Q2d - Q2_ss))
# m.Equation(tau2 * TC2.dt() + (TC2 - TC2_ss) == Kp4 * (Q2d - Q2_ss))

m.Equation(0.5 * (tau1 * TC1.dt() + (TC1 - TC1_ss) + tau3 * TC1.dt() + (TC1 - TC1_ss)) == Kp1 * (Q1d - Q1_ss) + Kp3 * (Q2d -Q2_ss))
m.Equation(0.5 * (tau2 * TC2.dt() + (TC2 - TC2_ss) + tau4 * TC2.dt() + (TC2 - TC2_ss)) == Kp4 * (Q2d - Q2_ss) + Kp2 * (Q1d - Q1_ss))

# Steady-state initializations
m.options.IMODE = 1
m.options.SOLVER  = 1 # 1=APOPT, 3=IPOPT
m.solve()

sp1.VALUE = 45
sp2.VALUE = 35

# Global Options
m.options.IMODE   = 6 # MPC
m.options.CV_TYPE = 3 # Objective type
m.options.NODES   = 2 # Collocation nodes
m.options.MAX_TIME = 10
m.options.SOLVER  = 1 # 1=APOPT, 3=IPOPT
#m.options.CV_WGT_START = 2*theta 

#m.options.CV_WGT_SLOPE = theta
# m.options.MV_STEP_HOR = 5
##################################################################



# Create plot
plt.figure()
plt.ion()
plt.show()


# Main Loop
a.Q1(0)
a.Q2(0)
Q2s[0:] = 0
start_time = time.time()

tm = np.linspace(1,loops,int(loops/step)+1)
j=0

try:
    time_start = time.time()
    labtime_start = labtime.time()
    if(not connected):
        labtime.set_rate(10)
    for i in tclab.clock(loops,adaptive=False):
        i = int(i)
        if(i == 0):
            continue
        print("-----------------------")
        t_real = time.time() - time_start
        t_lab  = labtime.time() - labtime_start
        print("real time = {0:4.1f}    lab time = {1:4.1f}    m.time = {1:4.1f}".format(t_real, t_lab,m.time))
        #print("real time = {0:4.1f}    m.time = {1:4.1f}".format(t_real, m.time))
       
        if(i%step != 0):
            continue

        j = i/step
        j = int(j)
        print(j)

        T1[j:] = a.T1
        T2[j:] = a.T2
        tm[j] = i
        ###############################
        ### MPC CONTROLLER          ###
        ###############################
        TC1.MEAS = T1[j] 
        TC2.MEAS = T2[j]
        print("T1 meas:{0:4.1f} ".format(a.T1))
        print("T2 meas:{0:4.1f} ".format(a.T2))
        
            
        # input setpoint with deadband +/- DT
        DT =0.5 
        TC1.SPHI = Tsp1[j] +DT 
        TC1.SPLO = Tsp1[j] -DT 
        TC2.SPHI = Tsp2[j] +DT 
        TC2.SPLO = Tsp2[j] -DT 

        try:
            # stop model time to solve MPC in cast the solver takes too much time
            if(not connected):
                labtime.stop()
            m.solve(disp=False)
            #start model time  
            if(not connected):
                labtime.start()
        except Exception as e:
            if(not connected):
                if(not labtime.running):
                    labtime.start()
            print("sovle's exception:")
            print(e)
            if(j != 0):
                Q1s[j] = Q1s[j-1]
                Q2s[j] = Q2s[j-1]
            continue
        # test for successful solution
        if (m.options.APPSTATUS==1):
            # retrieve the first Q value
            Q1s[j:] = np.ones(len(Q1s)-j) * Q1.NEWVAL
            Q2s[j:] = np.ones(len(Q2s)-j) * Q2.NEWVAL
            #a.Q1(Q1.NEWVAL)
            #a.Q2(Q2.NEWVAL)
            print("Q1 applied with delay: {0:4.1f}  ".format(Q1.NEWVAL))
            print("Q2 applied with delay: {0:4.1f}  ".format(Q2.NEWVAL))
            with open(m.path+'//results.json') as f:
                results = json.load(f)
        else:
            # not successful, set heater to zero
            print("APPSTATUS is not 1,set Q to 0")
            #Q1s[j] = 0
            #Q2s[j] = 0
        if i> 300 and i < 600:
          Q1s[j] = Q1s[j] - 20
          Q2s[j] = Q2s[j] - 20

        if i>= 600:
          Q1s[j] = Q1s[j] + 20
          Q2s[j] = Q2s[j] + 20

        Q1.meas= Q1s[j] 
        Q2.meas= Q2s[j]
        tclab_delay.Q1_delay(Q1s[j])
        tclab_delay.Q2_delay(Q2s[j])


        print("calc:"+str(Q1s[j]))
        print("calc:"+str(Q2s[j]))


        #apply disturbance on 50s, 200s,
        #if(i == 600):
        #    Q2s[j] = 100        
        #if(i == 1400):
        #    Q2s[j] = 0
            #Q2s[j] = 20 - randrange(20)  
        #Q2s[j:] = np.ones(len(Q2s)-j) * Q2s[j]

        #restore Q2 to 0
        #if(i == 300):
            #Q2s[j:] = 0

        #a.Q2(Q2s[j])
        #tclab_delay.Q2_delay(Q2s[j])


        #take Q2 to FV
        #Q2.MEAS = Q2s[j]


        if(not connected):
            labtime.stop()
        # Plot
        try:
            plt.clf()
            ax=plt.subplot(2,1,1)
            ax.grid()
            plt.plot(tm[0:j],T1[0:j],'ro',markersize=3,label=r'$T_1$')
            plt.plot(tm[0:j],Tsp1[0:j],'r-',markersize=3,label=r'$T_1 Setpoint$')
            plt.plot(tm[0:j],T2[0:j],'bo',markersize=3,label=r'$T_2$')
            plt.plot(tm[0:j],Tsp2[0:j],'b-',markersize=3,label=r'$T_2 Setpoint$')
        
            plt.plot(tm[j]+m.time,results['tc1.bcv'],'r-.',markersize=1,\
                     label=r'$T_1$ predicted',linewidth=1)

            plt.plot(tm[j]+m.time,results['tc2.bcv'],'b-.',markersize=1,\
                     label=r'$T_2$ predicted',linewidth=1)

            plt.plot(tm[j]+m.time,results['tc1.tr_hi'],'k--',\
                     label=r'$T_1$ trajectory')
            plt.plot(tm[j]+m.time,results['tc1.tr_lo'],'k--')

            
            plt.plot(tm[j]+m.time,results['tc2.tr_hi'],'k--',\
                     label=r'$T_2$ trajectory')
            plt.plot(tm[j]+m.time,results['tc2.tr_lo'],'k--')
            
            
            
            plt.ylabel('Temperature (degC)')
            plt.legend(loc='best')
            ax=plt.subplot(2,1,2)
            ax.grid()
            plt.plot(tm[0:j],Q1s[0:j],'r-',linewidth=3,label=r'$Q_1$')
            plt.plot(tm[0:j],Q2s[0:j],'b-',linewidth=3,label=r'$Q_2$')
            plt.plot(tm[j]+m.time,Q1.value,'r-.',\
                     label=r'$Q_1$ plan',linewidth=1)
            plt.plot(tm[j]+m.time,Q2.value,'b-.',\
                     label=r'$Q_2$ plan',linewidth=1)
            #plt.plot(tm[0:i],Q2s[0:i],'b:',LineWidth=3,label=r'$Q_2$')
            plt.ylabel('Heaters')
            plt.xlabel('Time (sec)')
            plt.legend(loc='best')
            plt.draw()
            plt.pause(0.05)
            if make_mp4:
                filename='./figures/plot_'+str(j+10000)+'.png'
                plt.savefig(filename)

        except Exception as e:
            print(e)
            pass

        if(not connected):
            labtime.start()

    # Turn off heaters
    a.Q1(0)
    a.Q2(0)
    print('Shutting down')
    input("Press Enter to continue...")
    a.close()

# Allow user to end loop with Ctrl-C
except KeyboardInterrupt:
    # Disconnect from Arduino
    a.Q1(0)
    a.Q2(0)
    print('Shutting down')
    a.close()
    if make_mp4:
        images = []
        iset = 0
        for i in range(1,int(loops/step)+1):
            filename='./figures/plot_'+str(i+10000)+'.png'
            if os.path.exists(filename):
                images.append(imageio.imread(filename))
                if ((i+1)%350)==0:
                    imageio.mimsave('results_'+str(iset)+'.mp4', images)
                    iset += 1
                    images = []
        if images!=[]:
            imageio.mimsave('results_'+str(iset)+'.mp4', images)

# Make sure serial connection still closes when there's an error
except:
    # Disconnect from Arduino
    a.Q1(0)
    a.Q2(0)
    print('Error: Shutting down')
    a.close()
    raise

问候蒂巴尔特

标签： gekko

Answer 1

CV是否FSTATUS也为 ON，例如t1.FSTATUS=1？如果您更新测量值，例如：

t1.MEAS = lab.T1
t2.MEAS = lab.T2

然后这会更新BIASfort1和t2（BIAS 文档）。这应该通过将加热器任意增加或减少 20% 来解决您引入的任何过程/模型不匹配。如果t1.FSTATUS为 OFF (0)，则无法补偿失配。

另一件事是调整参考轨迹。如果TAU太高，控制器可能会显得迟钝。这是一个使用MPC 和线性模型的示例应用程序。

补偿失配的另一种方法是使用移动地平线估计，如此处所示。

看起来您已经创建了一个不错的界面！

对编辑的回应

感谢您添加代码。问题是Q1.DMAX=10和Q2.DMAX=10。当Q1和Q2值在每个循环中向上移动 20 时，控制器最多可以向下移动，20-10=10因此控制器看起来是在错误的方向上倾斜。更改以DMAX=100解决问题。仍然存在与设定点的偏移量，因为推荐 值在每个循环中都会发生偏移Q1。Q2真正的推荐值从未实施。要尝试的另一件事是对测量值施加偏移，例如TC1.MEAS = T1[j] + 20. 在这种情况下，模型偏差将消除偏移量。

from random import random
from random import randrange

import tclab
from tclab import labtime
from tclab import TCLabModel

import numpy as np
import time
import matplotlib.pyplot as plt
from gekko import GEKKO
import json

from tclab import TCLabModel

make_gif = True
make_mp4 = True
if make_gif or make_mp4:
    # pip install imageio-ffmpeg with imageio to make MP4
    import imageio  # required to make animation
    import os
    try:
        os.mkdir('./figures')
    except:
        pass

class tclab_heaterpipe():
    def __init__(self,d1,d2,model):
        if(d1 >= 1 and d2 >=1):
            self.delay_q1_step = int(d1)
            self.delay_q2_step = int(d2)
            self.q1_buffer = [0] * self.delay_q1_step
            self.q2_buffer = [0] * self.delay_q2_step
            self.m = model
        else:
            self.delay_q1_step =0 
            self.delay_q2_step =0
        return

    def Q1_delay(self,q1):
        if(self.delay_q1_step == 0):
            self.m.Q1(q1)
        self.q1_buffer.insert(0,q1)
        self.m.Q1(self.q1_buffer.pop())

    def Q2_delay(self,q2):
        if(self.delay_q2_step == 0):
            self.m.Q1(q2)
        self.q2_buffer.insert(0,q2)
        self.m.Q2(self.q2_buffer.pop())

# Connect to Arduino
connected = False  # switch to connected=True with physical hardware
theta1 = 1 
theta2 = 1 
T = tclab.setup(connected)
a = T()
tclab_delay = tclab_heaterpipe(theta1,theta2,a)
# Turn LED on
print('LED On')
a.LED(100)

# Simulate a time delay
# Run time in minutes
run_time = 20.0
# Number of cycles
loops = int(60.0*run_time)

# Temperature (K)
t1sp = 45.0
t2sp = 35.0

#########################################################
# Initialize Model
#########################################################
# use remote=True for MacOS
m = GEKKO(name='tclab-mpc',remote=False)

m.time = np.linspace(0,400,41)
step = 10

T1 = np.ones(int(loops/step)+1) * a.T1 # temperature (degC)
T2 = np.ones(int(loops/step)+1) * a.T2 # temperature (degC)
Tsp1 = np.ones(int(loops/step)+1) * t1sp # set point (degC)
Tsp2 = np.ones(int(loops/step)+1) * t2sp # set point (degC)
# heater values
Q1s = np.ones(int(loops/step)+1) * 0.0 
Q2s = np.ones(int(loops/step)+1) * 0.0 

# Parameters
Q1_ss = m.Param(value=0)
TC1_ss = m.Param(value=a.T1)
Q2_ss = m.Param(value=0)
TC2_ss = m.Param(value=a.T2)
Kp1 = m.Param(value= 0.7)
tau1 = m.Param(value=160.0)
Kp2 = m.Param(value=0.05)
tau2 = m.Param(value=160.0)
Kp3= m.Param(value=0.05)
tau3 = m.Param(value=160.0)
Kp4 = m.Param(value=0.4)
tau4 = m.Param(value=200.0)
sp1 = m.Param(value=a.T1)
sp2 = m.Param(value=a.T2)

# Manipulated variable
Q1 = m.MV(value=0, name='q1')
Q1.STATUS = 1  # use to control temperature
Q1.FSTATUS = 1 # no feedback measurement
Q1.LOWER = 0.0
Q1.UPPER = 100.0
Q1.DMAX = 100.0
Q1.DCOST = 1e-3

Q2 = m.MV(value=0, name='q2')
Q2.STATUS = 1  # use to control temperature
Q2.FSTATUS = 1 # no feedback measurement
Q2.LOWER = 0.0
Q2.UPPER = 100.0
Q2.DMAX = 100.0
Q2.DCOST = 1e-3

# Controlled variable
TC1 = m.CV(value=a.T1, name='tc1')
TC1.STATUS = 1     # minimize error with setpoint range
TC1.FSTATUS = 1    # receive measurement
TC1.TR_INIT = 2    # reference trajectory
# TC1.COST = 0.1
TC1.WSPHI = 20
TC1.WSPLO = 20
TC1.TAU = 50        # time constant for response
#TC1.TR_OPEN = 3

TC2 = m.CV(value=a.T2, name='tc2')
TC2.STATUS = 1     # minimize error with setpoint range
TC2.FSTATUS = 1    # receive measurement
TC2.TR_INIT = 2    # reference trajectory
# TC2.COST = 0.1
TC2.WSPHI = 20
TC2.WSPLO = 20
TC2.TAU = 30       # time constant for response
#kTC2.TR_OPEN = 3

# 添加延时
Q1d=m.Var()
m.delay(Q1, Q1d, theta1)
Q2d=m.Var()
m.delay(Q2, Q2d, theta2)
# Equation
#m.Equation(tau1 * TC1.dt() + (TC1 - TC1_ss) == Kp1 * (Q1d - Q1_ss))
# m.Equation(tau2 * TC2.dt() + (TC2 - TC2_ss)  == Kp2 * (Q1d - Q1_ss))
# m.Equation(tau3 * TC1.dt() + (TC1 - TC1_ss)  == Kp3 * (Q2d - Q2_ss))
# m.Equation(tau2 * TC2.dt() + (TC2 - TC2_ss) == Kp4 * (Q2d - Q2_ss))

m.Equation(0.5 * (tau1 * TC1.dt() + (TC1 - TC1_ss) + tau3 * TC1.dt() + (TC1 - TC1_ss)) == Kp1 * (Q1d - Q1_ss) + Kp3 * (Q2d -Q2_ss))
m.Equation(0.5 * (tau2 * TC2.dt() + (TC2 - TC2_ss) + tau4 * TC2.dt() + (TC2 - TC2_ss)) == Kp4 * (Q2d - Q2_ss) + Kp2 * (Q1d - Q1_ss))

# Steady-state initializations
m.options.IMODE = 1
m.options.SOLVER  = 1 # 1=APOPT, 3=IPOPT
m.solve()

sp1.VALUE = 45
sp2.VALUE = 35

# Global Options
m.options.IMODE   = 6 # MPC
m.options.CV_TYPE = 3 # Objective type
m.options.NODES   = 2 # Collocation nodes
m.options.MAX_TIME = 10
m.options.SOLVER  = 1 # 1=APOPT, 3=IPOPT
#m.options.CV_WGT_START = 2*theta 

#m.options.CV_WGT_SLOPE = theta
# m.options.MV_STEP_HOR = 5
##################################################################
# Create plot
plt.figure(figsize=(12,8))
plt.ion()
plt.show()

# Main Loop
a.Q1(0)
a.Q2(0)
Q2s[0:] = 0
start_time = time.time()

tm = np.linspace(1,loops,int(loops/step)+1)
j=0

try:
    time_start = time.time()
    labtime_start = labtime.time()
    if(not connected):
        labtime.set_rate(10)
    for i in tclab.clock(loops,adaptive=False):
        i = int(i)
        if(i == 0):
            continue
        print("-----------------------")
        t_real = time.time() - time_start
        t_lab  = labtime.time() - labtime_start
        print("real time = {0:4.1f}    lab time = {1:4.1f}    m.time = {1:4.1f}".format(t_real, t_lab,m.time))
        #print("real time = {0:4.1f}    m.time = {1:4.1f}".format(t_real, m.time))

        if(i%step != 0):
            continue

        j = i/step
        j = int(j)
        print(j)

        T1[j:] = a.T1
        T2[j:] = a.T2
        tm[j] = i
        ###############################
        ### MPC CONTROLLER          ###
        ###############################
        TC1.MEAS = T1[j] 
        TC2.MEAS = T2[j]
        print("T1 meas:{0:4.1f} ".format(a.T1))
        print("T2 meas:{0:4.1f} ".format(a.T2))
        
        # input setpoint with deadband +/- DT
        DT =0.5 
        TC1.SPHI = Tsp1[j] +DT 
        TC1.SPLO = Tsp1[j] -DT 
        TC2.SPHI = Tsp2[j] +DT 
        TC2.SPLO = Tsp2[j] -DT 

        try:
            # stop model time to solve MPC in cast the solver takes too much time
            if(not connected):
                labtime.stop()
            m.solve(disp=False)
            #start model time  
            if(not connected):
                labtime.start()
        except Exception as e:
            if(not connected):
                if(not labtime.running):
                    labtime.start()
            print("sovle's exception:")
            print(e)
            if(j != 0):
                Q1s[j] = Q1s[j-1]
                Q2s[j] = Q2s[j-1]
            continue
        # test for successful solution
        if (m.options.APPSTATUS==1):
            # retrieve the first Q value
            Q1s[j:] = np.ones(len(Q1s)-j) * Q1.NEWVAL
            Q2s[j:] = np.ones(len(Q2s)-j) * Q2.NEWVAL
            #a.Q1(Q1.NEWVAL)
            #a.Q2(Q2.NEWVAL)
            print("Q1 applied with delay: {0:4.1f}  ".format(Q1.NEWVAL))
            print("Q2 applied with delay: {0:4.1f}  ".format(Q2.NEWVAL))
            with open(m.path+'//results.json') as f:
                results = json.load(f)
        else:
            # not successful, set heater to zero
            print("APPSTATUS is not 1,set Q to 0")
            #Q1s[j] = 0
            #Q2s[j] = 0
        if i> 300 and i < 600:
          Q1s[j] = max(0,Q1s[j] - 20)
          Q2s[j] = max(0,Q2s[j] - 20)

        if i>= 600:
          Q1s[j] = min(100,Q1s[j] + 20)
          Q2s[j] = min(100,Q2s[j] + 20)

        Q1.meas= Q1s[j] 
        Q2.meas= Q2s[j]
        tclab_delay.Q1_delay(Q1s[j])
        tclab_delay.Q2_delay(Q2s[j])


        print("calc:"+str(Q1s[j]))
        print("calc:"+str(Q2s[j]))

        if(not connected):
            labtime.stop()
        # Plot
        try:
            plt.clf()
            ax=plt.subplot(2,1,1)
            ax.grid()
            plt.plot(tm[0:j],T1[0:j],'ro',markersize=3,label=r'$T_1$')
            plt.plot(tm[0:j],Tsp1[0:j],'r-',markersize=3,label=r'$T_1 Setpoint$')
            plt.plot(tm[0:j],T2[0:j],'bo',markersize=3,label=r'$T_2$')
            plt.plot(tm[0:j],Tsp2[0:j],'b-',markersize=3,label=r'$T_2 Setpoint$')
        
            plt.plot(tm[j]+m.time,results['tc1.bcv'],'r-.',markersize=1,\
                     label=r'$T_1$ predicted',linewidth=1)

            plt.plot(tm[j]+m.time,results['tc2.bcv'],'b-.',markersize=1,\
                     label=r'$T_2$ predicted',linewidth=1)

            plt.plot(tm[j]+m.time,results['tc1.tr_hi'],'k--',\
                     label=r'$T_1$ trajectory')
            plt.plot(tm[j]+m.time,results['tc1.tr_lo'],'k--')

            
            plt.plot(tm[j]+m.time,results['tc2.tr_hi'],'k--',\
                     label=r'$T_2$ trajectory')
            plt.plot(tm[j]+m.time,results['tc2.tr_lo'],'k--')
            
            plt.ylabel('Temperature (degC)')
            plt.legend(loc=1)
            ax=plt.subplot(2,1,2)
            ax.grid()
            plt.plot(tm[0:j],Q1s[0:j],'r-',linewidth=3,label=r'$Q_1$')
            plt.plot(tm[0:j],Q2s[0:j],'b-',linewidth=3,label=r'$Q_2$')
            plt.plot(tm[j]+m.time,Q1.value,'r-.',\
                     label=r'$Q_1$ plan',linewidth=1)
            plt.plot(tm[j]+m.time,Q2.value,'b-.',\
                     label=r'$Q_2$ plan',linewidth=1)
            #plt.plot(tm[0:i],Q2s[0:i],'b:',LineWidth=3,label=r'$Q_2$')
            plt.ylabel('Heaters')
            plt.xlabel('Time (sec)')
            plt.legend(loc=1)
            plt.draw()
            plt.pause(0.05)
            if make_mp4:
                filename='./figures/plot_'+str(j+10000)+'.png'
                plt.savefig(filename)

        except Exception as e:
            print(e)
            pass

        if(not connected):
            labtime.start()

    # Turn off heaters
    a.Q1(0)
    a.Q2(0)
    print('Shutting down')
    input("Press Enter to continue...")
    a.close()

    # make gif
    if make_gif:
        images = []
        iset = 0
        for i in range(1,int(loops/step)+1):
            filename='./figures/plot_'+str(i+10000)+'.png'
            if os.path.exists(filename):
                images.append(imageio.imread(filename))
                if ((i+1)%350)==0:
                    imageio.mimsave('results_'+str(iset)+'.gif', images)
                    iset += 1
                    images = []
        if images!=[]:
            imageio.mimsave('results_'+str(iset)+'.gif', images)

    if make_mp4:
        images = []
        iset = 0
        for i in range(1,int(loops/step)+1):
            filename='./figures/plot_'+str(i+10000)+'.png'
            if os.path.exists(filename):
                images.append(imageio.imread(filename))
                if ((i+1)%350)==0:
                    imageio.mimsave('results_'+str(iset)+'.gif', images)
                    iset += 1
                    images = []
        if images!=[]:
            imageio.mimsave('results_'+str(iset)+'.gif', images)

# Allow user to end loop with Ctrl-C
except KeyboardInterrupt:
    # Disconnect from Arduino
    a.Q1(0)
    a.Q2(0)
    print('Shutting down')
    a.close()
    if make_gif:
        images = []
        iset = 0
        for i in range(1,int(loops/step)+1):
            filename='./figures/plot_'+str(i+10000)+'.png'
            if os.path.exists(filename):
                images.append(imageio.imread(filename))
                if ((i+1)%350)==0:
                    imageio.mimsave('results_'+str(iset)+'.gif', images)
                    iset += 1
                    images = []
        if images!=[]:
            imageio.mimsave('results_'+str(iset)+'.gif', images)
    if make_mp4:
        images = []
        iset = 0
        for i in range(1,int(loops/step)+1):
            filename='./figures/plot_'+str(i+10000)+'.png'
            if os.path.exists(filename):
                images.append(imageio.imread(filename))
                if ((i+1)%350)==0:
                    imageio.mimsave('results_'+str(iset)+'.mp4', images)
                    iset += 1
                    images = []
        if images!=[]:
            imageio.mimsave('results_'+str(iset)+'.mp4', images)

# Make sure serial connection still closes when there's an error
except:
    # Disconnect from Arduino
    a.Q1(0)
    a.Q2(0)
    print('Error: Shutting down')
    a.close()
    raise