Kepler-Release/Venus/Venus_RT/Modules/TM/VenusEntity/SETMEntity.cs

using Aitex.Core.RT.DataCenter;
using Aitex.Core.RT.Device;
using Aitex.Core.RT.Fsm;
using Aitex.Core.RT.Log;
using Aitex.Core.RT.OperationCenter;
using Aitex.Core.RT.SCCore;
using Aitex.Core.Util;
using Aitex.Sorter.Common;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.SubstrateTrackings;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Reflection;
using System.Text;
using System.Threading.Tasks;
using Venus_Core;
using Venus_RT.Devices;
using Venus_RT.Devices.PreAligner;
using Venus_RT.Devices.VCE;
using Venus_RT.Modules.PMs;
using static Mono.Security.X509.X520;

namespace Venus_RT.Modules.TM.VenusEntity
{
    public class SETMEntity : 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,
            Align,
            CreateJob,
            StartJob,
			Goto,
        }

        #region 公开变量
        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 bool VCEIsATM => _tm.IsVCEATM;
        public bool TMIsATM  => _tm.IsTMATM;

        public bool TMIsVAC => _tm.IsTMVAC;
        public bool VCEIsVAC => _tm.IsVCEVAC;

        
        public bool IsPMASlitDoorClosed => _tm.PMASlitDoorClosed;
        public bool IsPMBSlitDoorClosed => _tm.PMBSlitDoorClosed;
        public bool IsPMCSlitDoorClosed => _tm.PMCSlitDoorClosed;
        public bool IsVCESlitDoorClosed => _tm.VCESlitDoorClosed;

        public bool IsPMASlitDoorOpen => _tm.PMASlitDoorOpen;
        public bool IsPMBSlitDoorOpen => _tm.PMBSlitDoorOpen;
        public bool IsPMCSlitDoorOpen => _tm.PMCSlitDoorOpen;
        public bool IsVCESlitDoorOpen => _tm.VCESlitDoorOpen;

        public double VCEPressure => _tm.VCEPressure;

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

        private bool _IsOnline;
        public bool IsOnline => _IsOnline;

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

        #endregion

        #region 私有变量
        private readonly HongHuTM _tm;
        private readonly ITransferRobot _robot;
        private readonly IPreAlign _vpa;

        private readonly SEMFHomeRoutine _homeRoutine;
        private readonly SEMFPickRoutine _pickRoutine;
        private readonly SEMFPlaceRoutine _placeRoutine;
        private readonly SEMFVentRoutine _ventRoutine;
        private readonly SEMFPumpRoutine _pumpRoutine;
        private readonly SEMFPMPickRoutine _pickpmRoutine;
        private readonly SEMFPMPlaceRoutine _placepmRoutine;
        private readonly SEMFSwapRoutine _swaproutine;
        private readonly SEMFPMSwapRoutine _pmswaproutine;

        private readonly SEMFPMRetractRoutine _pmRetractRoutine;
        private readonly SEMFPMExtendRoutine _pmExtendRoutine;

        //private readonly
        //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;
        #endregion


        public SETMEntity()
        {
            _tm = DEVICE.GetDevice<HongHuTM>("SETM");
            if(ModuleHelper.IsInstalled(ModuleName.TMRobot))
                _robot = new HongHuVR();
            _vpa = new HongHuVPA(ModuleName.VPA);
            _robotWatch = new Stopwatch();
            
            _homeRoutine = new SEMFHomeRoutine(_tm,_robot, _vpa);
            _pickRoutine = new SEMFPickRoutine(_tm,_robot, _vpa);
            _placeRoutine = new SEMFPlaceRoutine(_tm, _robot, _vpa);
            _pumpRoutine = new SEMFPumpRoutine(_tm, ModuleName.SETM);
            _ventRoutine = new SEMFVentRoutine(_tm, ModuleName.SETM);
            _pickpmRoutine = new SEMFPMPickRoutine(_tm, _robot);
            _placepmRoutine = new SEMFPMPlaceRoutine(_tm, _robot);
            _swaproutine = new SEMFSwapRoutine(_tm, _robot);
            _pmswaproutine = new SEMFPMSwapRoutine(_tm, _robot);
            _pmExtendRoutine = new SEMFPMExtendRoutine(_tm, _robot, _vpa);
            _pmRetractRoutine = new SEMFPMRetractRoutine(_tm, _robot, _vpa);

            InitFsmMap();
        }


        protected override bool Init()
        {
            DATA.Subscribe($"SETM.FsmState", () => ((STATE)fsm.State).ToString());
            DATA.Subscribe("SETM.RobotMoveAction", () => _robot.TMRobotMoveInfo, SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe("SETM.RobotMoveAction.ArmTarget", () => _robot.TMRobotMoveInfo.ArmTarget.ToString(), SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe("SETM.RobotMoveAction.BladeTarget", () => _robot.TMRobotMoveInfo.BladeTarget, SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe("SETM.RobotMoveAction.RobotAction", () => _robot.TMRobotMoveInfo.Action.ToString(), SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe("SETM.IsOnline", () => IsOnline, SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe("SETM.IsOffline", () => !IsOnline, SubscriptionAttribute.FLAG.IgnoreSaveDB);

            OP.Subscribe("SETM.Goto", (cmd, args) => RobotGoto(args));
            OP.Subscribe("SETM.Home", (cmd, args) => { PostMsg(MSG.Home); return true; });
            OP.Subscribe("SETM.Abort", (cmd, args) => { PostMsg(MSG.Abort); return true; });
            OP.Subscribe("SETM.Pick", (cmd, args) => { PostMsg(MSG.Pick, args); return true; });
            OP.Subscribe("SETM.Place", (cmd, args) => { PostMsg(MSG.Place, args); return true; });
            OP.Subscribe("SETM.Extend", (cmd, args) => { PostMsg(MSG.Extend, args); return true; });
            OP.Subscribe("SETM.Retract", (cmd, args) => { PostMsg(MSG.Retract, args); return true; });
            
            OP.Subscribe("SETM.PMPick", (cmd, args) => { PostMsg(MSG.PMPick, args); return true; });
            OP.Subscribe("SETM.PMPlace", (cmd, args) => { PostMsg(MSG.PMPlace, args); return true; });
            OP.Subscribe("SETM.PumpDown", (cmd, args) => { PostMsg(MSG.Pump); return true; });
            OP.Subscribe("SETM.Vent", (cmd, args) => { PostMsg(MSG.Vent); return true; });
            OP.Subscribe("SETM.SetOnline", (cmd, args) =>
            {
                if (IsIdle)
                    _IsOnline = true;
                else
                    LOG.Write(eEvent.WARN_TM,ModuleName.TM,"cannot Set Online as TM is not idle");

                return true;
            });
            OP.Subscribe("SETM.SetOffline", (cmd, args) =>
            {
                _IsOnline = false;
                return true;
            });
            return true;
        }

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

            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,      fnHomeTimeout,      STATE.Idle);
            Transition(STATE.Initializing,      MSG.Abort,          fnAbortHome,        STATE.Init);

            //Pick
            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
            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);

            //Pump
            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);

            //Vent
            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);

            //PMPick
            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);

            //PMPlace
            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);

            //PA align
            Transition(STATE.Idle,              MSG.Align,          fnStartAlign,       STATE.Aligning);
            Transition(STATE.Aligning,          FSM_MSG.TIMER,      fnAlignTimeout,     STATE.Idle);
            Transition(STATE.Aligning,          MSG.Abort,          fnAbortAlign,       STATE.Idle);

            //Swap
            Transition(STATE.Idle,              MSG.Swap,           fnStartSwap,        STATE.Swapping);
            Transition(STATE.Swapping,          FSM_MSG.TIMER,      fnSwapTimeout,      STATE.Idle);
            Transition(STATE.Swapping,          MSG.Abort,          fnAbortSwap,        STATE.Idle);

            //PM Swap
            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);

            //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);

            //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);


            //Control Pressure
            AnyStateTransition(FSM_MSG.TIMER, ControlPressureTimer_Elapsed, FSM_STATE.SAME);
            Running = true;

        }
        private bool RobotGoto(object[] param)
        {
            return _robot.Goto((ModuleName)param[0], (int)param[1], (Hand)param[2]);
        }
        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 FnAbortExtend(object[] param)
        {
            _pmExtendRoutine.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 fnAbortPMSwap(object[] param)
        {
            _pmswaproutine.Abort();
            return true;
        }
        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 fnStartPMSwap(object[] param)
        {
            return _pmswaproutine.Start(param) == RState.Running;
        }

        private bool fnAbortSwap(object[] param)
        {
            _swaproutine.Abort();
            return true;
        }
        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 fnStartSwap(object[] param)
        {
            return _swaproutine.Start(param) == RState.Running;
        }

        private bool fnStartAlign(object[] param)
        {
            if (float.TryParse(param[0].ToString(), out float angle))
            {
                return _vpa.Align();
            }
            else
            {
                LOG.Write(eEvent.ERR_TM,ModuleName.VPA,$"wrong angle, value is {param[0]}.");
                return false;
            }
        }
        private bool fnAlignTimeout(object[] param)
        {
            if (_vpa.Status == RState.End)
            {
                return true;
            }
            else if (_vpa.Status != RState.Running)
            {
                LOG.Write(eEvent.ERR_TM, ModuleName.VPA, $"PreAligner align failed: {_vpa.Status}");
                return true;
            }

            return false;
        }
        private bool fnAbortAlign(object[] param)
        {
            return true;
        }

        private bool fnAbortPMPlace(object[] param)
        {
            _placepmRoutine.Abort();
            return true;
        }

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

            return ret == RState.End;
        }

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

        private bool fnAbortPMPick(object[] param)
        {
            _pickpmRoutine.Abort();
            return true;
        }

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

            return ret == RState.End;
        }

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

        

        private bool fnAbortVent(object[] param)
        {
            _ventRoutine.Abort();
            return true;
        }

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

            return ret == RState.End;
        }

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

        private bool fnAbortPump(object[] param)
        {
            _pumpRoutine.Abort();
            return true;
        }

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

            return ret == RState.End;
        }

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


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

        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 fnStartPlace(object[] param)
        {
            return _placeRoutine.Start(param) == RState.Running;
        }

        private bool fnAbortPick(object[] param)
        {
            _pickRoutine.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 fnAbortHome(object[] param)
        {
            _homeRoutine.Abort();
            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 fnHomeTimeout(object[] param)
        {
            RState ret = _homeRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                PostMsg(MSG.Error);
                return false;
            }

            return ret == RState.End;
        }

        private bool fnOffline(object[] param)
        {
            //IsOnline = false;
            //return true;
            throw new NotImplementedException();
        }

        private bool fnOnline(object[] param)
        {
            //IsOnline = true;
            //return true;
            throw new NotImplementedException();
        }

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

        private bool ControlPressureTimer_Elapsed(object[] param)
        {
            if (RouteManager.IsATMMode)
                return true;

            if (IsOnline && _tm.PumpingState == HongHuTM.PumpState.Idle)
            {
                if (SC.GetValue<bool>($"SETM.PressureControl.ControlWriteMode"))
                {
                    SC.SetItemValue("SETM.PressureControl.ControlWriteMode",false);
                }
            }
            else
            {
                if (!SC.GetValue<bool>($"SETM.PressureControl.ControlWriteMode"))
                {
                    SC.SetItemValue("SETM.PressureControl.ControlWriteMode", true);
                }
            }

            return true;
        }

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

        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 int Invoke(string function, params object[] args)
        {
            switch (function)
            {
                case "Home":
                    CheckToPostMessage((int)MSG.Home);
                    return (int)MSG.Home;
            }

            return (int)FSM_MSG.NONE;
        }
    }
}