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

using Aitex.Core.RT.Routine;
using Aitex.Core.RT.SCCore;
using Aitex.Sorter.Common;
using Venus_RT.Devices;
using MECF.Framework.Common.Routine;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.SubstrateTrackings;
using Venus_Core;
using Aitex.Core.RT.Log;
using Aitex.Core.Util;
using MECF.Framework.Common.Schedulers;
using System.Collections.Generic;

namespace Venus_RT.Modules.TM
{
    class MFPickRoutine : ModuleRoutineBase, IRoutine
    {
        private enum PickStep
        {
            WaitModuleReady,
            ModulePrepare,
            OpenSlitDoor,
            Picking,
            CloseSlitDoor,
            NotifyDone,
        }

        private readonly JetTM _JetTM;
        private readonly ITransferRobot _robot;

        private int _pickingTimeout = 120 * 1000;
        private ModuleName _targetModule;
        private LLEntity _llModule;
        int _targetSlot;
        Hand _hand;
        
        public MFPickRoutine(JetTM tm, ITransferRobot robot) :base(ModuleName.TM)
        {
            _JetTM = tm;
            _robot = robot;
            Name = "Pick";
        }
        public RState Start(params object[] objs)
        {
            if(!_robot.IsHomed)
            {
                LOG.Write(eEvent.ERR_TM, Module, $"TM Robot is not homed, please home it first");
                return RState.Failed;
            }


            var pickItem = (Queue<MoveItem>)objs[0];
            _targetModule = pickItem.Peek().SourceModule;
            _targetSlot = pickItem.Peek().SourceSlot;
            _hand = pickItem.Peek().RobotHand;

            if (ModuleHelper.IsLoadLock(_targetModule) && ModuleHelper.IsInstalled(_targetModule))
            {
                _llModule = Singleton<RouteManager>.Instance.GetLL(_targetModule);
            }
            else
            {
                LOG.Write(eEvent.ERR_TM, Module, $"Invalid target module : {_targetModule} for picking action");
                return RState.Failed;
            }

            if (WaferManager.Instance.CheckHasWafer(ModuleName.TM, (int)_hand))
            {
                LOG.Write(eEvent.ERR_TM, Module, $"Cannot pick as TM Robot Arm: {_hand} already has a wafer");
                return RState.Failed;
            }
            
            if (WaferManager.Instance.CheckNoWafer(_targetModule, _targetSlot))
            {
                LOG.Write(eEvent.ERR_TM, Module, $"Cannot pick as {_targetModule} Slot {_targetSlot} has no wafer");
                return RState.Failed;
            }

            Reset();
            _pickingTimeout = SC.GetValue<int>($"{Module}.PickTimeout") * 1000;

            return Runner.Start(Module, Name);
        }

        public RState Monitor()
        {
            Runner.Wait((int)PickStep.WaitModuleReady,  () => _llModule.IsIdle,     _delay_60s)
                .Run((int)PickStep.ModulePrepare,       ModulePrepare,              IsModulePrepareReady)
                .Run((int)PickStep.OpenSlitDoor,        OpenSlitDoor,               IsSlitDoorOpen)
                .Run((int)PickStep.Picking,             Picking,                    WaitPickDone)
                .Run((int)PickStep.CloseSlitDoor,       CloseSlitDoor,              IsSlitDoorClosed)
                .End((int)PickStep.NotifyDone,          NotifyLLDone,               _delay_50ms);         

            return Runner.Status;
        }

        private bool ModulePrepare()
        {
            _llModule.PostMsg(LLEntity.MSG.Prepare_TM);
            return true;
        }

        private bool IsModulePrepareReady()
        {
            return _llModule.Status == LLEntity.LLStatus.Ready_For_TM;
        }

        private bool OpenSlitDoor()
        {
            return _JetTM.TurnMFSlitDoor(_targetModule, true, out _);
        }

        private bool CloseSlitDoor()
        {
            return _JetTM.TurnMFSlitDoor(_targetModule, false, out _);
        }

        private bool IsSlitDoorOpen()
        {
            if (_targetModule == ModuleName.LLA) 
                return _JetTM.IsLLASlitDoorOpen;
            else 
                return _JetTM.IsLLBSlitDoorOpen;
        }

        private bool IsSlitDoorClosed()
        {
            if (_targetModule == ModuleName.LLA)
                return _JetTM.IsLLASlitDoorClosed;
            else
                return _JetTM.IsLLBSlitDoorClosed;
        }

        private bool Picking()
        {
            return _robot.Pick(_targetModule, _targetSlot, _hand);
        }

        private bool WaitPickDone()
        {
            if (_robot.Status == RState.Running)
            {
                return false;
            }
            else if (_robot.Status == RState.End)
            {
                //WaferManager.Instance.WaferMoved(ModuleName.TM, (int)_hand, _targetModule, _targetSlot);
                WaferManager.Instance.WaferMoved(_targetModule, _targetSlot, ModuleName.TM, (int)_hand);

                return true;
            }
            else
            {
                Runner.Stop($"TM Robot Picking failed, {_robot.Status}");
                return true;
            }
        }

        private bool NotifyLLDone()
        {
            _llModule.PostMsg(LLEntity.MSG.TM_Exchange_Ready);
            return true;
        }

        public void Abort()
        {
            _robot.Halt();
        }
    }
}