678 lines
22 KiB
C#
678 lines
22 KiB
C#
using Aitex.Core.RT.Device;
|
||
using Aitex.Core.RT.Device.Devices;
|
||
using Aitex.Core.RT.Device.Unit;
|
||
using Aitex.Core.RT.Event;
|
||
using Aitex.Core.RT.Routine;
|
||
using Aitex.Core.RT.SCCore;
|
||
using Aitex.Core.Util;
|
||
using MECF.Framework.Common.Equipment;
|
||
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.TMs;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using System.Text;
|
||
using System.Threading;
|
||
using System.Threading.Tasks;
|
||
|
||
namespace Mainframe.TMs
|
||
{
|
||
public class TMPressureBalancePidRoutine : TMBaseRoutine
|
||
{
|
||
enum RoutineStep
|
||
{
|
||
OpenValve,
|
||
RunPid,
|
||
SetM40,
|
||
CloseValve,
|
||
OpenSlowVent,
|
||
CloseSlowVent,
|
||
OpenSlowPump,
|
||
CloseSlowPump,
|
||
Delay1,
|
||
}
|
||
|
||
|
||
private bool _pidAlways = false;
|
||
private SicTM _tm;
|
||
private IoMFC _m40;
|
||
|
||
private IoValve _TMTOLLVent;
|
||
private ModuleName _paramTarget;
|
||
private double _pidP;
|
||
private double _pidI;
|
||
private double _pidD;
|
||
|
||
//private bool _needMfcVent; //是否需要Vent操作
|
||
//private double _ventMfcFlow; //MFC在Vent时设置的值
|
||
private double _balancePressure; //目标压力值
|
||
private int _ventTimeout; //MFC 超时
|
||
private double _pidMaxDiffPressure; //PID与目标压力的差
|
||
|
||
//private int _waitPressureTimeout; //不需要Vent操作时，等待压力平衡时间
|
||
|
||
private bool _isPidSucceed = false;
|
||
private bool _isPidRunning = true;
|
||
private int _pidSuccessCount; //压力到达范围内比较多少次就认为PID成功
|
||
private int _pidSleepTime;
|
||
private bool _pressureAboveTarget = false;
|
||
private bool _needVentOrPump = false;
|
||
|
||
List<double> lstPressure = new List<double>();
|
||
|
||
public TMPressureBalancePidRoutine()
|
||
{
|
||
Module = ModuleName.TM.ToString();
|
||
Name = "Pressure Balance PID";
|
||
_tm = DEVICE.GetDevice<SicTM>($"{ ModuleName.System.ToString()}.{ Module}");
|
||
_m40 = DEVICE.GetDevice<IoMFC>($"TM.Mfc40");
|
||
}
|
||
|
||
public void Init(ModuleName target)
|
||
{
|
||
Name = "Pressure Balance PID with " + target;
|
||
_paramTarget = target;
|
||
_pidAlways = false;
|
||
}
|
||
|
||
public void Init(ModuleName target, bool alwaysPID)
|
||
{
|
||
Name = "Pressure Balance PID with " + target;
|
||
_paramTarget = target;
|
||
_pidAlways = alwaysPID;
|
||
}
|
||
|
||
public override Result Start(params object[] objs)
|
||
{
|
||
if (ModuleHelper.IsPm(_paramTarget))
|
||
{
|
||
if (!SC.ContainsItem($"TM.PressureBalance.BalancePressure"))
|
||
{
|
||
EV.PostAlarmLog(Module, $"did not define balance pressure for {_paramTarget}");
|
||
return Result.FAIL;
|
||
}
|
||
_balancePressure = SC.GetValue<double>($"TM.PressureBalance.BalancePressure") + SC.GetValue<double>("TM.TMPressureBigThanPM"); //TM的压力需要大于PM数值
|
||
}
|
||
else
|
||
{
|
||
_balancePressure = SC.GetValue<double>("TM.PressureBalance.BalancePressure");
|
||
}
|
||
|
||
_ventTimeout = SC.GetValue<int>("TM.PressureBalance.VentTimeout");
|
||
|
||
_pidP = SC.GetValue<double>("TM.PressureBalance.PidP");
|
||
_pidI = SC.GetValue<double>("TM.PressureBalance.PidI");
|
||
_pidD = SC.GetValue<double>("TM.PressureBalance.PidD");
|
||
|
||
_pidMaxDiffPressure = SC.GetValue<double>("TM.PressureBalance.BalanceMaxDiffPressure");
|
||
_pidSuccessCount = SC.GetValue<int>("TM.PressureBalance.SuccessCount");
|
||
_pidSleepTime = SC.GetValue<int>("TM.PressureBalance.SleepTime");
|
||
|
||
if (_balancePressure <= 1)
|
||
{
|
||
return Result.DONE;
|
||
}
|
||
|
||
_isPidSucceed = false;
|
||
|
||
_needVentOrPump = true;
|
||
if (Math.Abs(_tm.ChamberPressure - _balancePressure) < 10)
|
||
{
|
||
_needVentOrPump = false;
|
||
}
|
||
else
|
||
{
|
||
if (_tm.ChamberPressure > _balancePressure)
|
||
{
|
||
_pressureAboveTarget = true;
|
||
}
|
||
else
|
||
{
|
||
_pressureAboveTarget = false;
|
||
}
|
||
}
|
||
|
||
_tm.SetVentMfc(10, out string reasonxx);
|
||
|
||
Notify("Start");
|
||
return Result.RUN;
|
||
}
|
||
|
||
public override Result Monitor()
|
||
{
|
||
try
|
||
{
|
||
if (SC.GetValue<bool>("System.IsATMMode"))
|
||
{
|
||
return Result.DONE;
|
||
}
|
||
|
||
if (_needVentOrPump)
|
||
{
|
||
if (_pressureAboveTarget)
|
||
{
|
||
OpenSlowPumpToTarget((int)RoutineStep.OpenSlowPump, _tm, _balancePressure, 200);
|
||
CloseSlowPump((int)RoutineStep.CloseSlowPump, _tm);
|
||
TimeDelay((int)RoutineStep.Delay1, 1);
|
||
}
|
||
else
|
||
{
|
||
OpenSlowVentToTarget((int)RoutineStep.OpenSlowVent,_tm, _balancePressure,200);
|
||
}
|
||
}
|
||
|
||
SetPIDValve((int)RoutineStep.OpenValve, _tm, true, 2);
|
||
|
||
RunPid((int)RoutineStep.RunPid, _ventTimeout);
|
||
|
||
//不需要关闭阀门
|
||
if (!_pidAlways)
|
||
{
|
||
SetPIDValve((int)RoutineStep.CloseValve, _tm, false, 2);
|
||
}
|
||
}
|
||
catch (RoutineBreakException)
|
||
{
|
||
return Result.RUN;
|
||
}
|
||
catch (RoutineFaildException)
|
||
{
|
||
return Result.FAIL;
|
||
}
|
||
|
||
return Result.DONE;
|
||
}
|
||
|
||
public override void Abort()
|
||
{
|
||
_isPidRunning = false;
|
||
_tm.SetPIDValve(false, out string reason);
|
||
|
||
_tm.SetSlowVentValve(false, out reason);
|
||
_tm.SetFastVentValve(false, out reason);
|
||
_tm.SetSlowPumpValve(false, out reason);
|
||
_tm.SetFastPumpValve(false, out reason);
|
||
}
|
||
|
||
|
||
|
||
private bool PidPressureReach()
|
||
{
|
||
if (lstPressure.Count >= _pidSuccessCount)
|
||
{
|
||
for (int i = 0; i < _pidSuccessCount; i++)
|
||
{
|
||
if (Math.Abs(lstPressure[i]) > _pidMaxDiffPressure)
|
||
{
|
||
return false;
|
||
}
|
||
}
|
||
return true;
|
||
}
|
||
else
|
||
{
|
||
return false;
|
||
}
|
||
}
|
||
|
||
public void RunPid(int id, int timeout)
|
||
{
|
||
Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
|
||
{
|
||
Notify($"Run PID for {_paramTarget}");
|
||
|
||
Task.Factory.StartNew(() => TMPressureBalancePid(timeout));
|
||
|
||
return true;
|
||
}, () =>
|
||
{
|
||
return _isPidSucceed;
|
||
|
||
}, timeout * 1000);
|
||
|
||
if (ret.Item1)
|
||
{
|
||
if (ret.Item2 == Result.TIMEOUT) //timeout
|
||
{
|
||
Stop($"Run PID for {_paramTarget} timeout, over {timeout} seconds");
|
||
throw (new RoutineFaildException());
|
||
}
|
||
else
|
||
throw (new RoutineBreakException());
|
||
}
|
||
}
|
||
|
||
public void TMPressureBalancePid(int timeOut)
|
||
{
|
||
bool firstReachTarget = false;
|
||
bool isLowThanTarget = _tm.ChamberPressure <_balancePressure;
|
||
|
||
DeviceTimer timer = new DeviceTimer();
|
||
timer.Start(timeOut * 1000);
|
||
|
||
double MP = 0;
|
||
double MI = 0;
|
||
double MD = 0;
|
||
double M = 0;
|
||
|
||
double LE = 0;
|
||
double E = _balancePressure - _tm.ChamberPressure;
|
||
|
||
while (_isPidRunning)
|
||
{
|
||
//第一次穿过目标值时,重新开始PID,以节省调节时间
|
||
if (firstReachTarget == false)
|
||
{
|
||
if (isLowThanTarget && _tm.ChamberPressure > _balancePressure)
|
||
{
|
||
MP = 0;
|
||
MI = 0;
|
||
MD = 0;
|
||
M = 0;
|
||
LE = 0;
|
||
E = _balancePressure - _tm.ChamberPressure;
|
||
lstPressure = new List<double>();
|
||
firstReachTarget = true;
|
||
}
|
||
else if(!isLowThanTarget && _tm.ChamberPressure < _balancePressure)
|
||
{
|
||
MP = 0;
|
||
MI = 0;
|
||
MD = 0;
|
||
M = 0;
|
||
LE = 0;
|
||
E = _balancePressure - _tm.ChamberPressure;
|
||
lstPressure = new List<double>();
|
||
firstReachTarget = true;
|
||
}
|
||
}
|
||
|
||
if (lstPressure.Count > _pidSuccessCount)
|
||
{
|
||
lstPressure.RemoveAt(0);
|
||
}
|
||
LE = E;
|
||
E = _balancePressure - _tm.ChamberPressure;
|
||
lstPressure.Add(E);
|
||
|
||
MP = _pidP * E;
|
||
MI += _pidI * E;
|
||
MD = _pidD * (E - LE);
|
||
M = MP + MI + MD;
|
||
|
||
_tm.SetVentMfc(M, out string reason);
|
||
|
||
|
||
_isPidRunning = !PidPressureReach();
|
||
if (timer.IsTimeout())
|
||
{
|
||
_isPidSucceed = false;
|
||
_isPidRunning = false;
|
||
}
|
||
|
||
Thread.Sleep(_pidSleepTime);
|
||
}
|
||
|
||
_isPidSucceed = true;
|
||
}
|
||
|
||
public void SetM40(int id, IoMFC m40)
|
||
{
|
||
Tuple<bool, Result> ret = Execute(id, () =>
|
||
{
|
||
Notify($"Set M40 flow default value");
|
||
|
||
m40.Terminate();
|
||
|
||
return true;
|
||
});
|
||
|
||
if (ret.Item1)
|
||
{
|
||
if (ret.Item2 == Result.FAIL)
|
||
{
|
||
throw (new RoutineFaildException());
|
||
}
|
||
else
|
||
throw (new RoutineBreakException());
|
||
}
|
||
}
|
||
|
||
public void SetPIDValve(int id, SicTM tm, bool isOpen, int timeout)
|
||
{
|
||
Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
|
||
{
|
||
Notify($"Open Slow Vent Valve and V121 for PID");
|
||
|
||
if (!tm.SetPIDValve(isOpen, out string reason))
|
||
{
|
||
Stop(reason);
|
||
return false;
|
||
}
|
||
|
||
return true;
|
||
}, () =>
|
||
{
|
||
return tm.CheckPIDValve(isOpen);
|
||
|
||
}, timeout * 1000);
|
||
|
||
if (ret.Item1)
|
||
{
|
||
if (ret.Item2 == Result.TIMEOUT) //timeout
|
||
{
|
||
Stop($"Set PID valve timeout, over {timeout} seconds");
|
||
throw (new RoutineFaildException());
|
||
}
|
||
else
|
||
throw (new RoutineBreakException());
|
||
}
|
||
}
|
||
|
||
|
||
//public void RunPid(int id, int timeout)
|
||
//{
|
||
// Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
|
||
// {
|
||
// Notify($"Run PID for {_paramTarget}");
|
||
|
||
// Task.Factory.StartNew(() => TMPressureBalancePid(timeout));
|
||
|
||
// return true;
|
||
// }, () =>
|
||
// {
|
||
// return _isPidSucceed;
|
||
|
||
// }, timeout * 1000);
|
||
|
||
// if (ret.Item1)
|
||
// {
|
||
// if (ret.Item2 == Result.TIMEOUT) //timeout
|
||
// {
|
||
// Stop($"Run PID for {_paramTarget} timeout, over {timeout} seconds");
|
||
// throw (new RoutineFaildException());
|
||
// }
|
||
// else
|
||
// throw (new RoutineBreakException());
|
||
// }
|
||
//}
|
||
|
||
//private void TMPressureBalancePid(int timeout)
|
||
//{
|
||
// string reason = string.Empty;
|
||
|
||
// try
|
||
// {
|
||
// _isPidRunning = true;
|
||
// double tolerance = _maxPressureDiffOpenSlitValve;
|
||
// double lowLimit = _balancePressure - 0.5;
|
||
// double upLimit = _balancePressure + tolerance - 1;
|
||
|
||
// double FP, FD;
|
||
// double FI = _tm.ChamberPressure;
|
||
// double Pn = FI;
|
||
// double Fn;
|
||
|
||
// int step = 0;
|
||
// double mfcFlow = 0;
|
||
|
||
// List<double> lstPressure = new List<double>();
|
||
|
||
// DeviceTimer timer = new DeviceTimer();
|
||
// timer.Start(timeout * 1000);
|
||
|
||
// while (_isPidRunning)
|
||
// {
|
||
// if (_tm.ChamberPressure >= lowLimit && _tm.ChamberPressure < upLimit)
|
||
// {
|
||
// lstPressure.Add(_tm.ChamberPressure);
|
||
|
||
// if (lstPressure.Count >= 100)
|
||
// {
|
||
// double avg = lstPressure.Average();
|
||
// lstPressure.RemoveAt(0);
|
||
|
||
// if (avg >= lowLimit && avg < upLimit)
|
||
// {
|
||
// if (!_isPidSucceed)
|
||
// {
|
||
// _isPidSucceed = true;
|
||
// break;
|
||
// }
|
||
// }
|
||
// }
|
||
// }
|
||
|
||
// switch (step)
|
||
// {
|
||
// case 0:
|
||
// if (_tm.ChamberPressure >= _balancePressure + 2)
|
||
// {
|
||
// _tm.SetVentMfc(0, out reason);
|
||
// _tm.SetMfcVentValve(false, out reason);
|
||
|
||
// if (!_tm.SetFastPumpValve(true, out reason))
|
||
// {
|
||
// Stop(reason);
|
||
// step = 10;
|
||
// }
|
||
// else
|
||
// {
|
||
// step = 2; // 1 为了防止气流不稳，不能关闭Pump阀门
|
||
// }
|
||
// }
|
||
// else
|
||
// {
|
||
// step = 2; // 1 为了防止气流不稳，不能关闭Pump阀门
|
||
// }
|
||
// break;
|
||
// case 2:
|
||
// if (_tm.ChamberPressure < _balancePressure - 1)
|
||
// {
|
||
// if (mfcFlow < _ventMfcFlow)
|
||
// {
|
||
// mfcFlow += _ventMfcStepFlow;
|
||
// }
|
||
// else
|
||
// {
|
||
// mfcFlow = 0;
|
||
// }
|
||
// _tm.SetVentMfc(mfcFlow, out reason);
|
||
// _tm.SetMfcVentValve(true, out reason);
|
||
// }
|
||
// else
|
||
// {
|
||
// step = 4; // 3，case 3 没用了
|
||
// }
|
||
|
||
// break;
|
||
// case 4:
|
||
// FP = _pidP * (_balancePressure - _tm.ChamberPressure);
|
||
// if (Math.Abs(Pn - _tm.ChamberPressure) < 3.0 * tolerance)
|
||
// {
|
||
// FI = FI + _pidI * (_balancePressure - _tm.ChamberPressure);
|
||
// }
|
||
// FD = _pidD * (Pn - _tm.ChamberPressure);
|
||
// Fn = FP + FI + FD;
|
||
|
||
// Fn = Math.Min(Fn, 7000.0);
|
||
// Fn = Math.Max(Fn, 5000.0);
|
||
|
||
// if (Fn < 0)
|
||
// {
|
||
// Fn = 0;
|
||
// }
|
||
// else if (Fn > _ventMfcFlow)
|
||
// {
|
||
// Fn = _ventMfcFlow;
|
||
// }
|
||
|
||
// _tm.SetVentMfc(Fn, out reason);
|
||
// _tm.SetMfcVentValve(true, out reason);
|
||
|
||
// Pn = _tm.ChamberPressure;
|
||
|
||
// step = _tm.ChamberPressure > (_balancePressure + 3) ? 0 : 2; // 0 : 1 为了防止气流不稳，不能关闭Pump阀门
|
||
|
||
// if (timer.IsTimeout())
|
||
// {
|
||
// step = 10;
|
||
// }
|
||
// break;
|
||
// case 10:
|
||
// _isPidRunning = false;
|
||
// _isPidSucceed = false;
|
||
// break;
|
||
// }
|
||
// Thread.Sleep(100);
|
||
// }
|
||
// }
|
||
// catch (Exception ex)
|
||
// {
|
||
// _isPidRunning = false;
|
||
// _isPidSucceed = false;
|
||
// LOG.Write(ex, $"Run PID for {_paramTarget} failed");
|
||
// }
|
||
|
||
// _tm.SetVentMfc(0, out reason);
|
||
// _tm.SetMfcVentValve(false, out reason);
|
||
// _tm.SetFastPumpValve(false, out reason);
|
||
|
||
// if (_isPidSucceed)
|
||
// {
|
||
// Notify($"Succeed");
|
||
// }
|
||
// else
|
||
// {
|
||
// Notify($"Failed");
|
||
// }
|
||
//}
|
||
|
||
public void OpenSlowPumpToTarget(int id, TM tm, double basePressure, int timeout)
|
||
{
|
||
Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
|
||
{
|
||
Notify($"Open {tm.Name} slow pump valve");
|
||
|
||
if (!tm.SetSlowPumpValve(true, out string reason))
|
||
{
|
||
Stop(reason);
|
||
return false;
|
||
}
|
||
|
||
return true;
|
||
}, () =>
|
||
{
|
||
return Math.Abs(tm.ChamberPressure - basePressure) < 10;
|
||
|
||
}, timeout * 1000);
|
||
|
||
if (ret.Item1)
|
||
{
|
||
if (ret.Item2 == Result.FAIL)
|
||
{
|
||
throw (new RoutineFaildException());
|
||
}
|
||
else if (ret.Item2 == Result.TIMEOUT) //timeout
|
||
{
|
||
tm.SetFastPumpValve(false, out string _);
|
||
|
||
Stop($"{tm.Name} pressure can not pump to {basePressure} in {timeout} seconds");
|
||
throw (new RoutineFaildException());
|
||
}
|
||
else
|
||
throw (new RoutineBreakException());
|
||
}
|
||
}
|
||
|
||
public void CloseSlowPump(int id, TM tm)
|
||
{
|
||
Tuple<bool, Result> ret = Execute(id, () =>
|
||
{
|
||
Notify($"Close {tm.Name} pump valves");
|
||
|
||
if (!_tm.SetSlowPumpValve(false, out string reason))
|
||
{
|
||
Stop(reason);
|
||
return false;
|
||
}
|
||
|
||
return true;
|
||
});
|
||
|
||
if (ret.Item1)
|
||
{
|
||
if (ret.Item2 == Result.FAIL)
|
||
{
|
||
throw (new RoutineFaildException());
|
||
}
|
||
else
|
||
throw (new RoutineBreakException());
|
||
}
|
||
}
|
||
|
||
public void OpenSlowVentToTarget(int id, TM tm, double basePressure, int timeout)
|
||
{
|
||
Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
|
||
{
|
||
Notify($"Open {tm.Name} slow vent valve");
|
||
|
||
if (!tm.SetSlowVentValve(true, out string reason))
|
||
{
|
||
Stop(reason);
|
||
return false;
|
||
}
|
||
|
||
return true;
|
||
}, () =>
|
||
{
|
||
return Math.Abs(tm.ChamberPressure - basePressure) < 10;
|
||
|
||
}, timeout * 1000);
|
||
|
||
if (ret.Item1)
|
||
{
|
||
if (ret.Item2 == Result.FAIL)
|
||
{
|
||
throw (new RoutineFaildException());
|
||
}
|
||
else if (ret.Item2 == Result.TIMEOUT) //timeout
|
||
{
|
||
tm.SetFastPumpValve(false, out string _);
|
||
|
||
Stop($"{tm.Name} pressure can not vent to {basePressure} in {timeout} seconds");
|
||
throw (new RoutineFaildException());
|
||
}
|
||
else
|
||
throw (new RoutineBreakException());
|
||
}
|
||
}
|
||
|
||
public void CloseSlowVent(int id, TM tm)
|
||
{
|
||
Tuple<bool, Result> ret = Execute(id, () =>
|
||
{
|
||
Notify($"Close {tm.Name} vent valves");
|
||
|
||
if (!_tm.SetSlowVentValve(false, out string reason))
|
||
{
|
||
Stop(reason);
|
||
return false;
|
||
}
|
||
|
||
return true;
|
||
});
|
||
|
||
if (ret.Item1)
|
||
{
|
||
if (ret.Item2 == Result.FAIL)
|
||
{
|
||
throw (new RoutineFaildException());
|
||
}
|
||
else
|
||
throw (new RoutineBreakException());
|
||
}
|
||
}
|
||
|
||
}
|
||
}
|