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

using Aitex.Core.RT.Device;
using Aitex.Core.RT.Fsm;
using Aitex.Core.RT.Log;
using Aitex.Core.RT.OperationCenter;
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;

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
        }

        #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 => true;
        public bool TMIsATM  => true;


        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 IsOnline { get; internal set; }

        //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
        private readonly Stopwatch _robotWatch = new Stopwatch();

        #endregion

        
        public SETMEntity()
        {
            _tm = DEVICE.GetDevice<HongHuTM>("SETM");
            _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);
            //routine


            InitFsmMap();
        }

        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.Idle);

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

            //Transition(STATE.Initializing,      MSG.Abort,          fnAbortHome,        STATE.Idle);
            //Transition(STATE.Initializing,      MSG.Abort,          fnAbortHome,        STATE.Idle);
            //Transition(STATE.Initializing,      MSG.Abort,          fnAbortHome,        STATE.Idle);
            //Transition(STATE.Initializing,      MSG.Abort,          fnAbortHome,        STATE.Idle);
            //Transition(STATE.Initializing,      MSG.Abort,          fnAbortHome,        STATE.Idle);
            //Transition(STATE.Initializing,      MSG.Abort,          fnAbortHome,        STATE.Idle);
            //Transition(STATE.Initializing,      MSG.Abort,          fnAbortHome,        STATE.Idle);
            //Transition(STATE.Initializing,      MSG.Abort,          fnAbortHome,        STATE.Idle);
            //Transition(STATE.Initializing,      MSG.Abort,          fnAbortHome,        STATE.Idle);

            Running = true;

        }

        protected override bool Init()
        {
            OP.Subscribe("SETM.Home", (cmd, args) => { PostMsg(MSG.Home); return true; });
            OP.Subscribe("SETM.Pick", (cmd, args) => { PostMsg(MSG.Pick, args[0], args[1],Hand.Blade1); return true; });
            OP.Subscribe("SETM.Place", (cmd, args) => { PostMsg(MSG.Place, args[0], args[1], Hand.Blade1); return true; });
            return true;
        }

        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)
        {
            throw new NotImplementedException();
        }

        private bool fnOnline(object[] param)
        {
            throw new NotImplementedException();
        }

        private bool fnError(object[] param)
        {
            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;
        }
    }
}