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

using Aitex.Core.RT.Log;
using Aitex.Core.RT.Routine;
using Aitex.Core.RT.SCCore;
using Aitex.Core.Util;
using Aitex.Sorter.Common;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.Schedulers;
using MECF.Framework.Common.SubstrateTrackings;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Venus_Core;
using Venus_RT.Devices;
using Venus_RT.Devices.TM;
using Venus_RT.Modules.VCE;

namespace Venus_RT.Modules.TM.VenusEntity
{
    public class SEMFSwapRoutine : ModuleRoutineBase, IRoutine
    {
        private enum SwapStep
        {
            WaitModuleReady,
            //PreRotation,
            SEDoorOpen,
            ModulePrepare,
            OpenSlitDoor,
            MoveWafer,
            VCEGoto,
            CloseSlitDoor,
            NotifyDone,
        }

        private readonly TMBase _TM;
        private readonly ITransferRobot _robot;

        private int _swapTimeout = 120 * 1000;
        private ModuleName _targetModule;
        private VceEntity _vceModule;


        Queue<MoveItem> _actionList = new Queue<MoveItem>();
        MoveItem _currentAction;

        public SEMFSwapRoutine(TMBase tm, ITransferRobot robot,ModuleName module) : base(module)
        {
            _TM = tm;
            _robot = robot;

            Name = "Swap";
        }
        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;
            }


            _actionList.Clear();
            foreach (var item in (Queue<MoveItem>)objs[0])
            {
                _actionList.Enqueue(new MoveItem(item.SourceModule, item.SourceSlot, item.DestinationModule, item.DestinationSlot, item.RobotHand));
            }
            var firtItem = _actionList.Peek();
            if (ModuleHelper.IsLoadPort(firtItem.SourceModule))
            {
                _targetModule = firtItem.SourceModule;
            }
            else if (ModuleHelper.IsLoadPort(firtItem.DestinationModule))
            {
                _targetModule = firtItem.DestinationModule;
            }
            else
            {
                LOG.Write(eEvent.ERR_TM, Module, $"Invalid move parameter: {firtItem.SourceModule},{firtItem.SourceSlot + 1} => {firtItem.DestinationModule},{firtItem.DestinationSlot + 1} ");
                return RState.Failed;
            }

            if (ModuleHelper.IsVCE(VCE2LP.QueryLP2VCE(_targetModule)))
            {
                _vceModule = Singleton<RouteManager>.Instance.GetVCE(VCE2LP.QueryLP2VCE(_targetModule));
            }
            else
            {
                LOG.Write(eEvent.ERR_TM, Module, $"Invalid target module : {_targetModule} ,which cannot find VCE2LP");
                return RState.Failed;
            }

            if (_vceModule == null)
            {
                LOG.Write(eEvent.ERR_TM, Module, $"Invalid vce: {_targetModule}, maybe not installed");
                return RState.Failed;
            }

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

            return Runner.Start(Module, $"Swap with {_targetModule}");
        }

        public RState Monitor()
        {
            Runner.Wait(SwapStep.WaitModuleReady,               () => _vceModule.IsIdle,            _delay_60s)
                .RunIf(SwapStep.SEDoorOpen,                     ModuleHelper.IsLoadPort(_targetModule), VCEDoorOpen, CheckVCEDoorOpen)
                .LoopStart(SwapStep.VCEGoto,                    loopName(),                         _actionList.Count,      VCEGoto,         VCEGoReady)
                .LoopEnd(SwapStep.MoveWafer,                    MoveWafer,                          WaitWaferMoved)
                .End(SwapStep.NotifyDone,                       NullFun,                            _delay_50ms);

            return Runner.Status;
        }

        private bool VCEDoorOpen()
        {
            return _TM.TurnSlitDoor(VCE2LP.QueryLP2VCE(_targetModule), true);
        }
        private bool CheckVCEDoorOpen()
        {
            return _TM.CheckSlitValveOpen(VCE2LP.QueryLP2VCE(_targetModule));
        }
        private string loopName()
        {
            return "VCE Swap";
        }

        private bool VCEGoto()
        {
            _currentAction = _actionList.Peek();
            if (ModuleHelper.IsVCE(VCE2LP.QueryLP2VCE(_currentAction.SourceModule)) && ModuleHelper.IsTMRobot(_currentAction.DestinationModule))
            {
                return _vceModule.CheckToPostMessage((int)VceMSG.Goto, _currentAction.SourceSlot);
            }
            else if (ModuleHelper.IsTMRobot(_currentAction.SourceModule) && ModuleHelper.IsVCE(VCE2LP.QueryLP2VCE(_currentAction.DestinationModule)))
            {
                return _vceModule.CheckToPostMessage((int)VceMSG.Goto, _currentAction.DestinationSlot);
            }
            else
            {
                LOG.Write(eEvent.ERR_TM_ROBOT, ModuleName.VCE1, $"Invalid move parameter, source:{_currentAction.SourceModule},{_currentAction.SourceSlot}, destination: {_currentAction.DestinationModule}, {_currentAction.DestinationSlot}");
                return false;
            }
        }

        private bool VCEGoReady()
        {
            _currentAction = _actionList.Peek();
            if (ModuleHelper.IsVCE(VCE2LP.QueryLP2VCE(_currentAction.SourceModule)) && ModuleHelper.IsTMRobot(_currentAction.DestinationModule))
            {
                return _vceModule.IsIdle && _vceModule.CurrentSlot == _currentAction.SourceSlot;
            }
            if (ModuleHelper.IsTMRobot(_currentAction.SourceModule) && ModuleHelper.IsVCE(VCE2LP.QueryLP2VCE(_currentAction.DestinationModule)))
            {
                return _vceModule.IsIdle && _vceModule.CurrentSlot == _currentAction.DestinationSlot;
            }
            return false;
        }

        private bool VerifyWaferExistence(MoveItem item)
        {
            if (WaferManager.Instance.CheckHasWafer(item.DestinationModule, item.DestinationSlot))
            {
                LOG.Write(eEvent.ERR_TM, Module, $"Cannot move wafer as desitination {_currentAction.DestinationModule},{_currentAction.DestinationSlot + 1} already has a wafer: ");
                return false;
            }

            if (WaferManager.Instance.CheckNoWafer(_currentAction.SourceModule, _currentAction.SourceSlot))
            {
                LOG.Write(eEvent.ERR_TM, Module, $"Cannot move wafer as source {_currentAction.SourceModule}, {_currentAction.SourceSlot + 1} has no wafer");
                return false;
            }

            return true;
        }

        private bool MoveWafer()
        {
            _currentAction = _actionList.Dequeue();

            if (!VerifyWaferExistence(_currentAction))
                return false;

            var wafer = WaferManager.Instance.GetWafer(_currentAction.SourceModule, _currentAction.SourceSlot);
            LOG.Write(eEvent.INFO_TM_ROBOT, ModuleName.TMRobot, $"{wafer.WaferOrigin} will be move from {_currentAction.SourceModule} {_currentAction.SourceSlot + 1}  to {_currentAction.DestinationModule} {_currentAction.DestinationSlot + 1}");

            if (ModuleHelper.IsVCE(VCE2LP.QueryLP2VCE(_currentAction.SourceModule)) && ModuleHelper.IsTMRobot(_currentAction.DestinationModule))
            {
                return _robot.Pick(VCE2LP.QueryLP2VCE(_currentAction.SourceModule), _currentAction.SourceSlot, (Hand)_currentAction.DestinationSlot);
            }
            else if (ModuleHelper.IsTMRobot(_currentAction.SourceModule) && ModuleHelper.IsVCE(VCE2LP.QueryLP2VCE(_currentAction.DestinationModule)))
            {
                return _robot.Place(VCE2LP.QueryLP2VCE(_currentAction.DestinationModule), _currentAction.DestinationSlot, (Hand)_currentAction.SourceSlot);
            }
            else
            {
                LOG.Write(eEvent.ERR_TM_ROBOT, ModuleName.TMRobot, $"Invalid move parameter, source:{_currentAction.SourceModule},{_currentAction.SourceSlot}, destination: {_currentAction.DestinationModule}, {_currentAction.DestinationSlot}");
                return false;
            }
        }

        private bool WaitWaferMoved()
        {
            if (_robot.Status == RState.Running)
            {
                return false;
            }
            else if (_robot.Status == RState.End)
            {
                WaferManager.Instance.WaferMoved(_currentAction.SourceModule, _currentAction.SourceSlot, _currentAction.DestinationModule, _currentAction.DestinationSlot);
                return true;
            }
            else
            {
                Runner.Stop($"TM Robot moving wafer failed, {_robot.Status}");
                return true;
            }
        }

        


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

    }
}