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;

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,
            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 < 500)
                        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 = true;

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

        //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.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 FnStartControlPressure(object[] param)
        {
            return _tmControlPressureRoutine.Start(param) == RState.Running;
        }
        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);
                    startControlPressureFlag = false;
                    stopControlPressureFlag = 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)
                {
                    _tmControlPressureRoutine.Abort();
                    startControlPressureFlag = true;
                    stopControlPressureFlag = false;
                }
               
            }
            
            return true;
        }
        private bool FnControlPressure(object[] param)
        {
            RState ret = _tmControlPressureRoutine.Monitor();
            if (ret == RState.Failed)
            {
                PostMsg(MSG.Error);
                return false;
            }
            return ret == RState.End;
        }
        private bool FnAbortControlPressure(object[] param)
        {
            _tmControlPressureRoutine.Abort();
            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);
            //}
        }
    }
}