Kepler-Release/Venus/Venus_RT/Modules/TM/TMEntity.cs

using System;
using System.Diagnostics;
using System.Collections.Generic;
using Aitex.Core.RT.Fsm;
using Aitex.Core.RT.Log;
using Aitex.Core.Util;
using Venus_Core;
using Aitex.Sorter.Common;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.SubstrateTrackings;
using Venus_RT.Devices;
using Venus_RT.Modules.TM;
using Aitex.Core.RT.DataCenter;
using Aitex.Core.RT.OperationCenter;
using MECF.Framework.Common.Schedulers;
using MECF.Framework.Common.CommonData;
using Aitex.Core.RT.Device;
using System.Timers;
using Aitex.Core.RT.SCCore;
using Aitex.Core.Backend;
using System.Windows.Controls;
using MECF.Framework.Common.DataCenter;
using Venus_RT.Devices.VCE;
using Venus_RT.Modules.PMs;

namespace Venus_RT.Modules
{
    class TMEntity : Entity, IModuleEntity
    {
        public enum STATE
        {
            Unknown,
            Init,
            Initializing,
            InitializingRB,
            Idle,
            Error,
            Pumping,
            Venting,
            Purging,
            Leakchecking,
            Picking,
            Placing,
            Swaping,
            PMPicking,
            PMPlacing,
            PMSwaping,
            Aligning,
            Mapping,
            Extending,
            Retracting,
            Swapping,
            Gotoing,
            ControllingPressure
        }

        public enum MSG
        {
            Home,
            RobotHome,
            Online,
            Offline,
            Pump,
            Vent,
            Purge,
            CyclePurge,
            LeakCheck,
            Pick,
            Place,
            Swap,
            Goto,
            DoublePick,
            DoublePlace,
            DoubleSwap,
            PMPick,
            PMPlace,
            PMSwap,
            Extend,
            Retract,
            TMCycle,
            ControlPressure,
            Error,
            Abort,
            AbortControlPressure
        }


        public bool IsIdle
        {
            get { return fsm.State == (int)STATE.Idle; }
        }

        public bool IsError
        {
            get { return fsm.State == (int)STATE.Error; }
        }

        public bool IsInit
        {
            get { return fsm.State == (int)STATE.Unknown || fsm.State == (int)STATE.Init; }
        }

        public bool IsBusy
        {
            get { return !IsInit && !IsError && !IsIdle; }
        }

        public RState RobotStatus
        {
            get
            {
                if (_robot.Status != RState.Running)
                {
                    if (_robotWatch.ElapsedMilliseconds < 100)
                        return RState.Running;
                    else
                        return _robot.Status;
                }
                else
                    return RState.Running;
            }
        }

        public bool IsLLSlitDoorClosed(ModuleName ll) => ll == ModuleName.LLA ? _tm.IsLLAESlitDoorClosed : _tm.IsLLBESlitDoorClosed;
        public bool IsLLSlitDoorOpen(ModuleName ll) => ll == ModuleName.LLA ? _tm.IsLLAESlitDoorOpen : _tm.IsLLBESlitDoorOpen;

        public bool IsOnline { get; internal set; }

        public bool IsTMVac => _tm.IsTMVac;
        public bool IsTMATM => _tm.IsTMATM;

        public double TMPressure { get { return _tm.TMPressure; } }

        private readonly JetTM _tm;
        private readonly ITransferRobot _robot;

        private readonly MFHomeRoutine _homeRoutine;
        private readonly MFPumpRoutine _pumpingRoutine;
        private readonly MFVentRoutine _ventingRoutine;
        private readonly MFLeakCheckRoutine _leakCheckRoutine;
        private readonly MFPurgeRoutine _purgeRoutine;
        private readonly MFPickRoutine _pickRoutine;
        private readonly MFPlaceRoutine _placeRoutine;
        private readonly MFSwapRoutine _swapRoutine;
        private readonly MFPMPickRoutine _pmPickRoutine;
        private readonly MFPMPlaceRoutine _pmPlaceRoutine;
        private readonly MFPMSwapRoutine _pmSwapRoutine;


        private readonly MFPMRetractRoutine _pmRetractRoutine;
        private readonly MFPMExtendRoutine _pmExtendRoutine;

        //private readonly MFControlPressureRoutine _tmControlPressureRoutine;
        private bool startControlPressureFlag = true;
        private bool stopControlPressureFlag = false;

        private Stopwatch _robotWatch = new Stopwatch();
        private R_TRIG _robotIdleTrigger = new R_TRIG();

        private int _controlPressureCheckPoint = 100;
        private int _controlPressureSetPoint = 90;
        private int _controlFlowSetPoint = 90;
        //private readonly Timer controlPressureTimer;
        public TMEntity()
        {
            //_tm                 = Singleton<JetTM>.Instance;
            _tm = DEVICE.GetDevice<JetTM>("TM");
            _robot = new SIASUNRobot();

            

            _pickRoutine = new MFPickRoutine(_tm, _robot);
            _placeRoutine = new MFPlaceRoutine(_tm, _robot);
            _swapRoutine = new MFSwapRoutine(_tm, _robot);

            _pmPickRoutine = new MFPMPickRoutine(_tm, _robot);
            _pmPlaceRoutine = new MFPMPlaceRoutine(_tm, _robot);
            _pmSwapRoutine = new MFPMSwapRoutine(_tm, _robot);

            _pumpingRoutine = new MFPumpRoutine(_tm, ModuleName.TM);

            _homeRoutine = new MFHomeRoutine(_tm, _robot, _pumpingRoutine);

            _ventingRoutine = new MFVentRoutine(_tm, ModuleName.TM);
            _leakCheckRoutine = new MFLeakCheckRoutine(_tm, ModuleName.TM);
            _purgeRoutine = new MFPurgeRoutine(_tm, ModuleName.TM);

            _pmRetractRoutine = new MFPMRetractRoutine(_tm, _robot);
            _pmExtendRoutine = new MFPMExtendRoutine(_tm, _robot);

            //_tmControlPressureRoutine = new MFControlPressureRoutine(_tm, _pumpingRoutine);

            WaferManager.Instance.SubscribeLocation(ModuleName.TMRobot, 2);

            InitFsmMap();
            //controlPressureTimer = new Timer(50);
            //controlPressureTimer.Elapsed += ControlPressureTimer_Elapsed;
        }



        protected override bool Init()
        {
            OP.Subscribe("TM.Home", (cmd, args) => CheckToPostMessage((int)MSG.Home, args));
            OP.Subscribe("TM.RobotHome", (cmd, args) => CheckToPostMessage((int)MSG.RobotHome, args));
            OP.Subscribe($"TM.{RtOperation.LLPick}", (cmd, args) => CheckToPostMessage((int)MSG.Pick, args));
            OP.Subscribe($"TM.{RtOperation.LLPlace}", (cmd, args) => CheckToPostMessage((int)MSG.Place, args));
            OP.Subscribe($"TM.{RtOperation.PMPick}", (cmd, args) => CheckToPostMessage((int)MSG.PMPick, args));
            OP.Subscribe($"TM.{RtOperation.PMPlace}", (cmd, args) => CheckToPostMessage((int)MSG.PMPlace, args));

            OP.Subscribe($"TM.{RtOperation.Extend}", (cmd, args) => CheckToPostMessage((int)MSG.Extend, args));
            OP.Subscribe($"TM.{RtOperation.Retract}", (cmd, args) => CheckToPostMessage((int)MSG.Retract, args));

            OP.Subscribe($"TM.{RtOperation.Goto}", (cmd, args) => RobotGoto(args));


            OP.Subscribe($"TM.{RtOperation.Cycle}", (cmd, args) => CheckToPostMessage((int)MSG.Swap, args));

            //OP.Subscribe($"TM.{RtOperation.LLPlace}", (cmd, args) => CheckToPostMessage((int)MSG.Place, args));
            OP.Subscribe($"TM.{RtOperation.Pump}", (cmd, args) => CheckToPostMessage((int)MSG.Pump));
            OP.Subscribe($"TM.{RtOperation.Vent}", (cmd, args) => CheckToPostMessage((int)MSG.Vent));
            OP.Subscribe($"TM.{RtOperation.LeakCheck}", (cmd, args) => CheckToPostMessage((int)MSG.LeakCheck));
            OP.Subscribe($"TM.{RtOperation.Purge}", (cmd, args) => CheckToPostMessage((int)MSG.Purge));
            OP.Subscribe($"TM.{RtOperation.Abort}", (cmd, args) => CheckToPostMessage((int)MSG.Abort));

            OP.Subscribe($"TM.{RtOperation.Online}", (cmd, args) => CheckToPostMessage((int)MSG.Online));
            OP.Subscribe($"TM.{RtOperation.Offline}", (cmd, args) => CheckToPostMessage((int)MSG.Offline));
            OP.Subscribe($"TM.{RtOperation.ControlPressure}", (cmd, args) => CheckToPostMessage((int)MSG.ControlPressure));
            OP.Subscribe($"TM.{RtOperation.AbortControlPressure}", (cmd, args) => CheckToPostMessage((int)MSG.AbortControlPressure));




            DATA.Subscribe("TM.FsmState", () => (((STATE)fsm.State).ToString()), SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe("TM.FsmPrevState", () => (((PMState)fsm.PrevState).ToString()), SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe("TM.FsmLastMessage", () => (((MSG)fsm.LastMsg).ToString()), SubscriptionAttribute.FLAG.IgnoreSaveDB);


            DATA.Subscribe("TM.RobotMoveAction", () => _robot.TMRobotMoveInfo, SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe("TM.RobotMoveAction.ArmTarget", () => _robot.TMRobotMoveInfo.ArmTarget.ToString(), SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe("TM.RobotMoveAction.BladeTarget", () => _robot.TMRobotMoveInfo.BladeTarget, SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe("TM.RobotMoveAction.RobotAction", () => _robot.TMRobotMoveInfo.Action.ToString(), SubscriptionAttribute.FLAG.IgnoreSaveDB);

            DATA.Subscribe("TM.IsOnline", () => IsOnline);
            DATA.Subscribe("TM.Home.StepNo", () => _homeRoutine.currentStepNo, SubscriptionAttribute.FLAG.IgnoreSaveDB);

            _robotWatch.Restart();

            return true;
        }

        private void InitFsmMap()
        {
            fsm = new StateMachine<TMEntity>("TM", (int)STATE.Init, 50);

            //AnyStateTransition(FSM_MSG.TIMER,   fnMonitor,      FSM_STATE.SAME); 


            AnyStateTransition(MSG.Error, fnError, STATE.Error);
            AnyStateTransition(MSG.Online, fnOnline, FSM_STATE.SAME);
            AnyStateTransition(MSG.Offline, fnOffline, FSM_STATE.SAME);
            AnyStateTransition(MSG.Home, fnHome, STATE.Initializing);


            // Home
            Transition(STATE.Initializing, FSM_MSG.TIMER, fnHoming, STATE.Idle);
            Transition(STATE.Initializing, MSG.Abort, FnAbortHome, STATE.Idle);

            // Robot Home
            Transition(STATE.Idle, MSG.RobotHome, fnHome, STATE.InitializingRB);
            Transition(STATE.InitializingRB, FSM_MSG.TIMER, fnHoming, STATE.Idle);
            Transition(STATE.InitializingRB, MSG.Abort, FnAbortHome, STATE.Idle);
            //Transition(STATE.Idle,              FSM_MSG.TIMER,      fnMonitor,          STATE.Idle);
            //Transition(STATE.Init,              FSM_MSG.TIMER,      fnMonitor,          STATE.Init);

            // Vent sequence
            Transition(STATE.Idle, MSG.Vent, FnStartVent, STATE.Venting);
            Transition(STATE.Venting, FSM_MSG.TIMER, FnVentTimeout, STATE.Idle);
            Transition(STATE.Venting, MSG.Abort, FnAbortVent, STATE.Idle);

            // Pump sequence
            Transition(STATE.Idle, MSG.Pump, FnStartPump, STATE.Pumping);
            Transition(STATE.Pumping, FSM_MSG.TIMER, FnPumpTimeout, STATE.Idle);
            Transition(STATE.Pumping, MSG.Abort, FnAbortPump, STATE.Idle);

            // Purge sequence
            Transition(STATE.Idle, MSG.Purge, FnStartPurge, STATE.Purging);
            Transition(STATE.Purging, FSM_MSG.TIMER, FnPurgeTimeout, STATE.Idle);
            Transition(STATE.Purging, MSG.Abort, FnAbortPurge, STATE.Idle);

            // Leak check sequence
            Transition(STATE.Idle, MSG.LeakCheck, FnStartLeakCheck, STATE.Leakchecking);
            Transition(STATE.Leakchecking, FSM_MSG.TIMER, FnLeakCheckTimeout, STATE.Idle);
            Transition(STATE.Leakchecking, MSG.Abort, FnAbortLeakCheck, STATE.Idle);

            // Pick wafer from LL sequence
            Transition(STATE.Idle, MSG.Pick, FnStartPick, STATE.Picking);
            Transition(STATE.Picking, FSM_MSG.TIMER, FnPickTimeout, STATE.Idle);
            Transition(STATE.Picking, MSG.Abort, FnAbortPick, STATE.Idle);

            // Place wafer to LL sequence
            Transition(STATE.Idle, MSG.Place, FnStartPlace, STATE.Placing);
            Transition(STATE.Placing, FSM_MSG.TIMER, FnPlaceTimeout, STATE.Idle);
            Transition(STATE.Placing, MSG.Abort, FnAbortPlace, STATE.Idle);

            // Swap wafer with LL sequence
            Transition(STATE.Idle, MSG.Swap, FnStartSwap, STATE.Swaping);
            Transition(STATE.Swaping, FSM_MSG.TIMER, FnSwapTimeout, STATE.Idle);
            Transition(STATE.Swaping, MSG.Abort, FnAbortSwap, STATE.Idle);

            // Pick wafer from PM sequence
            Transition(STATE.Idle, MSG.PMPick, FnStartPMPick, STATE.PMPicking);
            Transition(STATE.PMPicking, FSM_MSG.TIMER, FnPMPickTimeout, STATE.Idle);
            Transition(STATE.PMPicking, MSG.Abort, FnAbortPMPick, STATE.Idle);

            // Place wafer to PM sequence
            Transition(STATE.Idle, MSG.PMPlace, FnStartPMPlace, STATE.PMPlacing);
            Transition(STATE.PMPlacing, FSM_MSG.TIMER, FnPMPlaceTimeout, STATE.Idle);
            Transition(STATE.PMPlacing, MSG.Abort, FnAbortPMPlace, STATE.Idle);

            // Swap wafer with PM sequence
            Transition(STATE.Idle, MSG.PMSwap, FnStartPMSwap, STATE.PMSwaping);
            Transition(STATE.PMSwaping, FSM_MSG.TIMER, FnPMSwapTimeout, STATE.Idle);
            Transition(STATE.PMSwaping, MSG.Abort, FnAbortPMSwap, STATE.Idle);


            //Retract
            Transition(STATE.Idle, MSG.Retract, FnStartRetract, STATE.Retracting);
            Transition(STATE.Retracting, FSM_MSG.TIMER, FnRetract, STATE.Idle);
            Transition(STATE.Retracting, MSG.Abort, FnAbortRetract, STATE.Idle);

            //Extend
            Transition(STATE.Idle, MSG.Extend, FnStartExtend, STATE.Extending);
            Transition(STATE.Extending, FSM_MSG.TIMER, FnExtend, STATE.Idle);
            Transition(STATE.Extending, MSG.Abort, FnAbortExtend, STATE.Idle);

            //Transition(RtState.Init, MSG.TMCycle, FsmStartTMCycle, RtState.TMCycle);
            //Transition(RtState.TMCycle, FSM_MSG.TIMER, FsmMonitorTMCycle, RtState.Idle);
            //Extend

            //Control Pressure
            AnyStateTransition(FSM_MSG.TIMER, ControlPressureTimer_Elapsed, FSM_STATE.SAME);
            //Transition(STATE.ControllingPressure,        FSM_MSG.TIMER,               FnControlPressure,          STATE.Idle);
            //AnyStateTransition(MSG.AbortControlPressure, FnAbortControlPressure, FSM_STATE.SAME);


            Running = true;
        }

        private bool fnMonitor(object[] param)
        {
            _debugRoutine();
            return true;
        }

        private bool fnError(object[] param)
        {
            IsOnline = false;
            return true;
        }

        private bool fnOnline(object[] param)
        {
            //controlPressureTimer.Start();
            IsOnline = true;
            return true;
        }

        private bool fnOffline(object[] param)
        {
            //controlPressureTimer.Stop();
            IsOnline = false;
            return true;
        }

        private bool fnAbort(object[] param)
        {
            _robot.Halt();
            return true;
        }

        private bool fnHome(object[] param)
        {
            if (fsm.State == (int)STATE.Init && param.Length > 0)//带参home
            {
                return false;
            }
            else
                return _homeRoutine.Start(param) == RState.Running;
        }

        private bool fnHoming(object[] param)
        {
            RState ret = _homeRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                PostMsg(MSG.Error);
                return false;
            }

            return ret == RState.End;
        }

        private bool FnStartVent(object[] param)
        {
            return _ventingRoutine.Start() == RState.Running;
        }

        private bool FnVentTimeout(object[] param)
        {
            RState ret = _ventingRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                _ventingRoutine.Abort();
                PostMsg(MSG.Error);
                return false;
            }

            return ret == RState.End;
        }

        private bool FnAbortVent(object[] param)
        {
            _ventingRoutine.Abort();
            return true;
        }

        private bool FnStartPump(object[] param)
        {
            return _pumpingRoutine.Start() == RState.Running;
        }

        private bool FnPumpTimeout(object[] param)
        {
            RState ret = _pumpingRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                _pumpingRoutine.Abort();
                PostMsg(MSG.Error);
                return false;
            }

            return ret == RState.End;
        }

        private bool FnAbortPump(object[] param)
        {
            _pumpingRoutine.Abort();
            return true;
        }

        private bool FnStartPurge(object[] param)
        {
            return _purgeRoutine.Start() == RState.Running;
        }

        private bool FnPurgeTimeout(object[] param)
        {
            RState ret = _purgeRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                PostMsg(MSG.Error);
                return false;
            }

            return ret == RState.End;
        }

        private bool FnAbortPurge(object[] param)
        {
            _purgeRoutine.Abort();
            return true;
        }

        private bool FnStartLeakCheck(object[] param)
        {
            return _leakCheckRoutine.Start() == RState.Running;
        }

        private bool FnLeakCheckTimeout(object[] param)
        {
            RState ret = _leakCheckRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                PostMsg(MSG.Error);
                return false;
            }

            return ret == RState.End;
        }

        private bool FnAbortLeakCheck(object[] param)
        {
            _leakCheckRoutine.Abort();
            return true;
        }

        private bool FnStartPick(object[] param)
        {
            return _pickRoutine.Start(param) == RState.Running;
        }

        private bool FnPickTimeout(object[] param)
        {
            RState ret = _pickRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                PostMsg(MSG.Error);
                return false;
            }

            return ret == RState.End;
        }

        private bool FnAbortPick(object[] param)
        {
            _pickRoutine.Abort();
            return true;
        }
        private bool FnStartPlace(object[] param)
        {
            return _placeRoutine.Start(param) == RState.Running;
        }

        private bool FnPlaceTimeout(object[] param)
        {
            RState ret = _placeRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                PostMsg(MSG.Error);
                return false;
            }

            return ret == RState.End;
        }

        private bool FnAbortPlace(object[] param)
        {
            _placeRoutine.Abort();
            return true;
        }

        private bool FnStartSwap(object[] param)
        {
            return _swapRoutine.Start(param) == RState.Running;
        }

        private bool FnSwapTimeout(object[] param)
        {
            RState ret = _swapRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                PostMsg(MSG.Error);
                return false;
            }

            return ret == RState.End;
        }

        private bool FnAbortSwap(object[] param)
        {
            _swapRoutine.Abort();
            return true;
        }


        private bool FnStartPMPick(object[] param)
        {
            return _pmPickRoutine.Start(param) == RState.Running;
        }

        private bool FnPMPickTimeout(object[] param)
        {
            RState ret = _pmPickRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                PostMsg(MSG.Error);
                return false;
            }

            return ret == RState.End;
        }

        private bool FnAbortPMPick(object[] param)
        {
            _pmPickRoutine.Abort();
            return true;
        }
        private bool FnStartPMPlace(object[] param)
        {
            return _pmPlaceRoutine.Start(param) == RState.Running;
        }

        private bool FnPMPlaceTimeout(object[] param)
        {
            RState ret = _pmPlaceRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                PostMsg(MSG.Error);
                return false;
            }

            return ret == RState.End;
        }

        private bool FnAbortPMPlace(object[] param)
        {
            _pmPlaceRoutine.Abort();
            return true;
        }

        private bool FnStartPMSwap(object[] param)
        {
            return _pmSwapRoutine.Start(param) == RState.Running;
        }

        private bool FnPMSwapTimeout(object[] param)
        {
            RState ret = _pmSwapRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                PostMsg(MSG.Error);
                return false;
            }

            return ret == RState.End;
        }

        private bool FnAbortPMSwap(object[] param)
        {
            _pmSwapRoutine.Abort();
            return true;
        }


        private bool FnStartRetract(object[] param)
        {
            return _pmRetractRoutine.Start(param) == RState.Running;
        }
        private bool FnRetract(object[] param)
        {
            RState ret = _pmRetractRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                PostMsg(MSG.Error);
                return false;
            }

            return ret == RState.End;
        }
        private bool FnAbortRetract(object[] param)
        {
            _pmRetractRoutine.Abort();
            return true;
        }

        private bool FnStartExtend(object[] param)
        {
            return _pmExtendRoutine.Start(param) == RState.Running;
        }
        private bool FnExtend(object[] param)
        {
            RState ret = _pmExtendRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                PostMsg(MSG.Error);
                return false;
            }

            return ret == RState.End;
        }

        private bool FnAbortHome(object[] param)
        {
            _homeRoutine.Abort();
            return true;
        }

        private bool FnAbortHomeRB(object[] param)
        {
            _homeRoutine.Abort();
            return true;
        }

        private bool FnAbortExtend(object[] param)
        {
            _pmExtendRoutine.Abort();
            return true;
        }

        private bool RobotGoto(object[] param)
        {
            return _robot.Goto((ModuleName)param[0], (int)param[1], (Hand)param[2]);
        }
        private bool ControlPressureTimer_Elapsed(object[] param)
        {
            // robot idle check
            _robotIdleTrigger.CLK = _robot.Status != RState.Running;
            if (_robotIdleTrigger.Q)
            {
                _robotWatch.Restart();
            }

            if (RouteManager.IsATMMode)
            {
                return true;
            }
            if (IsOnline == true)
            {
                if (startControlPressureFlag == true)
                {
                    //_tmControlPressureRoutine.Start(param);
                    _controlPressureCheckPoint = SC.GetValue<int>($"TM.ControlPressureCheckPoint");
                    _controlPressureSetPoint = SC.GetValue<int>($"TM.ControlPressureSetPoint");
                    _controlFlowSetPoint = SC.GetValue<int>($"TM.TM_MFC1.DefaultSetPoint");
                    startControlPressureFlag = false;
                    stopControlPressureFlag = false;
                    _pumpingRoutine.Start();
                }
                RState ret = _pumpingRoutine.Monitor();
                if (ret == RState.End &&_tm.TMPressure <= _controlPressureCheckPoint)
                {
                    if (stopControlPressureFlag == false)
                    {
                        stopControlPressureFlag=true;
                        _tm.TurnFastPumpValve(ModuleName.TM, true);
                        _tm.TurnN2Valve(true);
                        _tm.TurnPurgeValve(ModuleName.TM, true);
                        if (_tm.AllPMSlitDoorClosed)
                        {
                            _tm.SwitchTMPressureMode(true);
                            _tm.SetTMPressure(_controlPressureSetPoint);
                        }
                        else
                        {
                            _tm.SwitchTMPressureMode(false);
                            _tm.SetTMFlow(_controlFlowSetPoint);
                        }
                        if (_tm.PMASlitDoorClosed == false || _tm.PMBSlitDoorClosed == false || _tm.PMCSlitDoorClosed == false || _tm.PMDSlitDoorClosed == false)
                        {
                            _tm.SwitchTMPressureMode(false);
                        }
                        else
                        {
                            _tm.SwitchTMPressureMode(true);
                        }
                    }
                    
                }
                //RState ret = _tmControlPressureRoutine.Monitor();

                //if (ret == RState.End)
                //{
                //    if (_tm.PMASlitDoorClosed == false || _tm.PMBSlitDoorClosed == false || _tm.PMCSlitDoorClosed == false || _tm.PMDSlitDoorClosed == false)
                //    {
                //        _tm.SwitchTMPressureMode(false);
                //    }
                //    else
                //    {
                //        _tm.SwitchTMPressureMode(true);
                //    }
                //}
            }
            else
            {
                if (stopControlPressureFlag == true)
                {
                    _tm.TurnFastPumpValve(ModuleName.TM, false);
                    _tm.TurnN2Valve(false);
                    _tm.TurnPurgeValve(ModuleName.TM, false);
                    _tm.SetTMPressure(0);
                    _tm.SetTMFlow(0);
                    startControlPressureFlag = true;
                    stopControlPressureFlag = false;
                }
            }
            return true;
        }


        public bool Check(int msg, out string reason, params object[] args)
        {
            reason = "";
            return true;
        }

        public int Invoke(string function, params object[] args)
        {
            switch (function)
            {
                case "Home":
                    CheckToPostMessage((int)MSG.Home);
                    return (int)MSG.Home;
            }

            return (int)FSM_MSG.NONE;
        }

        public bool CheckAcked(int msg)
        {
            return fsm.CheckExecuted(msg);
        }

        public bool CheckToPostMessage(int msg, params object[] args)
        {
            if (!fsm.FindTransition(fsm.State, msg))
            {
                LOG.Write(eEvent.WARN_FSM_WARN, ModuleName.TM, $"TM is in {(STATE)fsm.State} state，can not do {(MSG)msg}");
                return false;
            }

            Running = true;
            fsm.PostMsg(msg, args);

            return true;
        }

        public bool TurnEFEMSlitDoor(ModuleName loadlock, bool open, out string reason)
        {
            return _tm.TurnEFEMSlitDoor(loadlock, open, out reason);
        }
        //private bool FsmStartTMCycle(object[] objs)
        //{
        //    return _TMCycle.Start(objs) == RState.Running;
        //}

        //private bool FsmMonitorTMCycle(object[] objs)
        //{
        //    RState ret = _TMCycle.Monitor();
        //    if (ret == RState.Failed || ret == RState.Timeout)
        //    {
        //        PostMsg(MSG.Error);
        //        return false;
        //    }

        //    return ret == RState.End;
        //}
        private void _debugRoutine()
        {
            int flag = 0;
            // Test Home routine
            if (flag == 1)
            {
                PostMsg(MSG.Home);
            }
            else if (flag == 2)
            {
                PostMsg(MSG.Vent);
            }
            else if (flag == 3)
            {
                PostMsg(MSG.Pump);
            }
            else if (flag == 4)
            {
                PostMsg(MSG.Pick, ModuleName.LLA, 0, 0);
            }
            else if (flag == 5)
            {
                PostMsg(MSG.Place, ModuleName.LLA, 0, 0);
            }
            else if (flag == 6)
            {
                Queue<MoveItem> items = new Queue<MoveItem>();
                items.Enqueue(new MoveItem(ModuleName.TMRobot, 0, ModuleName.LLA, 0, Hand.Blade1));
                items.Enqueue(new MoveItem(ModuleName.TMRobot, 1, ModuleName.LLA, 1, Hand.Blade2));
                items.Enqueue(new MoveItem(ModuleName.LLA, 0, ModuleName.TMRobot, 0, Hand.Blade1));
                items.Enqueue(new MoveItem(ModuleName.LLA, 1, ModuleName.TMRobot, 1, Hand.Blade2));
                PostMsg(MSG.Swap, items);
            }
            else if (flag == 7)
            {
                PostMsg(MSG.PMPick, ModuleName.PMA, 0, 0);
            }
            else if (flag == 8)
            {
                PostMsg(MSG.PMPlace, ModuleName.PMA, 0, 0);
            }
            else if (flag == 9)
            {
                PostMsg(MSG.PMSwap, ModuleName.PMA, 0, 0, 0, 0);
            }

            //else if (flag == 4)
            //{
            //    PostMsg(MSG.PumpLoadLock);
            //}
            //else if (flag == 5)
            //{
            //    PostMsg(MSG.VentLoadLock);
            //}
            //else if (flag == 6)
            //{
            //    PostMsg(MSG.PurgeLoadLock);
            //}
            //else if (flag == 7)
            //{
            //    PostMsg(MSG.LaunchPump);
            //}
            //else if (flag == 8)
            //{
            //    PostMsg(MSG.LaunchTurboPump);
            //}
            //else if (flag == 9)
            //{
            //    PostMsg(MSG.LoadLockLeakCheck);
            //}
            //else if (flag == 10)
            //{
            //    PostMsg(MSG.CyclePurge);
            //}
            //else if (flag == 11)
            //{
            //    PostMsg(MSG.GasLinePurge);
            //}
            //else if (flag == 12)
            //{
            //    PostMsg(MSG.LeakCheck);
            //}
            //else if (flag == 13)
            //{
            //    PostMsg(MSG.GasLeakCheck);
            //}
            //else if (flag == 14)
            //{
            //    PostMsg(MSG.LLPlace);
            //}
            //else if (flag == 15)
            //{
            //    PostMsg(MSG.LLPick);
            //}
            //else if (flag == 16)
            //{
            //    PostMsg(MSG.RunRecipe, "7777");
            //}
            //else if (flag == 17)
            //{
            //    PostMsg(MSG.MFCVerification, "MFC2", (double)50, 10);
            //}
        }
    }
}