PunkHPX8/PunkHPX8_RT/Modules/EFEM/EfemPickRoutine.cs

using Aitex.Core.RT.Routine;
using Aitex.Core.RT.SCCore;
using Aitex.Sorter.Common;
using PunkHPX8_RT.Devices;
using MECF.Framework.Common.Routine;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.SubstrateTrackings;
using PunkHPX8_Core;
using Aitex.Core.RT.Log;
using Aitex.Core.Util;
using MECF.Framework.Common.Schedulers;
using System.Collections.Generic;
using PunkHPX8_RT.Devices.EFEM;
using PunkHPX8_RT.Modules.LPs;
using PunkHPX8_RT.Modules.SRD;
using System.Runtime.InteropServices;
using Aitex.Core.RT.Device;
using PunkHPX8_RT.Devices.AXIS;
using MECF.Framework.Common.Utilities;
using PunkHPX8_RT.Modules.Transporter;
using System;
using Aitex.Core.UI.ControlDataContext;
using Aitex.Core.Common;
using PunkHPX8_RT.Devices.PlatingCell;
using PunkHPX8_RT.Modules.PlatingCell;

namespace PunkHPX8_RT.Modules.EFEM
{
    class EfemPickRoutine : RoutineBase, IRoutine
    {
        private enum PickStep
        {
            WaitModuleReady,
            PufVacuumOff,
            Picking1,
            Picking2,
            End,
        }

        private int _moveTimeout = 20 * 1000;
        private ModuleName _targetModule = ModuleName.System;
        int _targetSlot;
        int _targetSlot2;
        string _targetPufSlot;
        Hand _hand;
        Hand _hand2;
        Flip _flip;
        EfemBase _efem;
        bool _bDoublePick = false;
        private SRDEntity _srdModule;
        private Queue<MoveItem> _moveQueue;
        public EfemPickRoutine(EfemBase efem) : base(ModuleName.EfemRobot.ToString())
        {
            _efem = efem;
        }

        public RState Start(params object[] objs)
        {
            _bDoublePick = false;
            _moveQueue = (Queue<MoveItem>)objs[0];
            MoveItem moveItem = _moveQueue.Peek();
            _targetModule = moveItem.SourceModule;
            _targetSlot = moveItem.SourceSlot;
            _hand = moveItem.RobotHand;
            _flip = moveItem.PickRobotFlip;

            if(_moveQueue.Count >= 2)
            {
                _bDoublePick = true;
            }
            if (!CheckPreCondition())
            {
                return RState.Failed;
            }
            _moveTimeout = SC.GetValue<int>($"EFEM.MotionTimeout") * 1000;
            return Runner.Start(Module, $"Pick from {_targetModule}");
        }

        private Loadport GetLoadPort(ModuleName station)
        {
            LoadPortModule loadPortModule = Singleton<RouteManager>.Instance.EFEM.GetLoadportModule(station - ModuleName.LP1);
            return loadPortModule.LPDevice;
        }

        private bool CheckPreCondition()
        {
            //LoadPort状态判断
            if (ModuleHelper.IsLoadPort(_targetModule) && ModuleHelper.IsInstalled(_targetModule))
            {
                Loadport loadPort=GetLoadPort(_targetModule);
                if (loadPort == null)
                {
                    return false;
                }
                if (!loadPort.IsLoaded)
                {
                    NotifyError(eEvent.ERR_EFEM_ROBOT,  $"LoadPort not load, cannot do the pick action",-1);
                    return false;
                }
            }
            if (ModuleHelper.IsDummy(_targetModule)||ModuleHelper.IsAligner(_targetModule)|| ModuleHelper.IsLoadPort(_targetModule))
            {
                if (_hand == Hand.Blade2)
                {
                    NotifyError(eEvent.ERR_EFEM_ROBOT, $"cannot use upper arm to pick from {_targetModule}", -1);
                    return false;
                }
            }
            if (ModuleHelper.IsPlatingCell(_targetModule))
            {
                if (_hand == Hand.Blade1)
                {
                    NotifyError(eEvent.ERR_EFEM_ROBOT, $"cannot use lower arm to pick from {_targetModule}", -1);
                    return false;
                }
                PlatingCellDevice platingCellDevice = DEVICE.GetDevice<PlatingCellDevice>(_targetModule.ToString());
                if (platingCellDevice == null)
                {
                    NotifyError(eEvent.ERR_EFEM_ROBOT, $"{_targetModule} entity is null, can not pick", -1);
                    return false;
                }
                if (platingCellDevice != null&&platingCellDevice.PlatingCellDeviceData.ClamShellClosed)
                {
                    NotifyError(eEvent.ERR_EFEM_ROBOT, $"{_targetModule} clamshell closed, can not pick", -1);
                    return false;
                }
                string vertical = ModuleMatcherManager.Instance.GetPlatingVerticalByCell(_targetModule.ToString());
                PlatingCellVerticalEntity verticalEntity = Singleton<RouteManager>.Instance.GetModule<PlatingCellVerticalEntity>(vertical);
                if (verticalEntity == null)
                {
                    NotifyError(eEvent.ERR_EFEM_ROBOT, $"{_targetModule} vertical entity is null, can not pick", -1);
                    return false;
                }
                if (verticalEntity != null&&!verticalEntity.IsInLoad)
                {
                    NotifyError(eEvent.ERR_EFEM_ROBOT, $"{_targetModule} vertical is not in load, can not pick", -1);
                    return false;
                }
            }

            if (_hand == Hand.Blade1 && _flip == Flip.Down)
            {
                NotifyError(eEvent.ERR_EFEM_ROBOT, $"lower arm cannot flip", -1);
                return false;
            }
            if (ModuleHelper.IsSRD(_targetModule) && ModuleHelper.IsInstalled(_targetModule))
            {
                _srdModule = Singleton<RouteManager>.Instance.GetModule<SRDEntity>(_targetModule.ToString());
                if (!_srdModule.IsHomed)
                {
                    NotifyError(eEvent.ERR_EFEM_ROBOT,  $"{_targetModule} is not homed, please home it first",-1);
                    return false;
                }
                //判断arm是否在home位置上
                //JetAxisBase jetAxisBase = DEVICE.GetDevice<JetAxisBase>($"{_targetModule}.Arm");
                //double position = jetAxisBase.MotionData.MotorPosition;
                //bool result = jetAxisBase.CheckPositionIsInStation(position, "Home");
                //if (!result)
                //{
                //    NotifyError(eEvent.ERR_EFEM_ROBOT,  $"{_targetModule} armaxis {position} is not in homed place",-1);
                //    return false;
                //}
                if (_srdModule.IsSrdDoorClosed)
                {
                    NotifyError(eEvent.ERR_EFEM_ROBOT,  $"{_targetModule} door closed, can not pick",-1);
                    return false;
                }
                if (_srdModule.IsSrdChuckVacuum)
                {
                    NotifyError(eEvent.ERR_EFEM_ROBOT,  $"{_targetModule} Vacuum on, can not pick",-1);
                    return false;
                }
                if (_hand == Hand.Blade2)
                {
                    NotifyError(eEvent.ERR_EFEM_ROBOT, $"cannot use upper hand to pick from loadport", -1);
                    return false;
                }
            }
            if (WaferManager.Instance.CheckHasWafer(ModuleName.EfemRobot, (int)_hand))
            {
                NotifyError(eEvent.ERR_EFEM_ROBOT,  $"Efem robot arm{_hand} already has a wafer, cannot do the pick action",-1);
                return false;
            }

            if (WaferManager.Instance.CheckNoWafer(_targetModule, _targetSlot))
            {
                NotifyError(eEvent.ERR_EFEM_ROBOT,  $"The target slot: {_targetModule}.{_targetSlot + 1} has no wafer, cannot do the pick action",-1);
                
                return false;
            }
            else
            {
                WaferInfo waferInfo = WaferManager.Instance.GetWafer(_targetModule, _targetSlot);
                if(waferInfo.Status!=WaferStatus.Normal)
                {
                    NotifyError(eEvent.ERR_EFEM_ROBOT, $"The target slot: {_targetModule}.{_targetSlot + 1} wafer status is {waferInfo.Status}, cannot do the pick action", -1);
                    return false;
                }
            }



            if (_moveQueue.Count >= 2)
            {
                if (!ModuleHelper.IsLoadPort(_targetModule))
                {
                    NotifyError(eEvent.ERR_EFEM_ROBOT,  $"Wrong double pick command, target is not loadport",-1);
                    return false;
                }

                _hand2 = _hand != Hand.Blade1 ? Hand.Blade1 : Hand.Blade2;
                if (WaferManager.Instance.CheckHasWafer(ModuleName.EfemRobot, (int)_hand2))
                {
                    NotifyError(eEvent.ERR_EFEM_ROBOT,  $"Efem robot arm{_hand2} already has a wafer, cannot do the double pick action",-1);
                    return false;
                }

                _targetSlot2 = _moveQueue.ToArray()[1].SourceSlot;
                if (WaferManager.Instance.CheckNoWafer(_targetModule, _targetSlot2))
                {
                    NotifyError(eEvent.ERR_EFEM_ROBOT,  $"The target slot: {_targetModule}.{_targetSlot2 + 1} has no wafer, cannot do the double pick action",-1);
                    return false;
                }
            }
            return true;
        }

        public RState Monitor()
        {
            if(_bDoublePick)
            {
                Runner.Wait(PickStep.WaitModuleReady,  WaitModuleReady)
                        .Run(PickStep.Picking1,        Pick1,          Pick1Done,      _moveTimeout)
                        .Run(PickStep.Picking2,        Pick2,          Pick2Done,      _moveTimeout)
                        .End(PickStep.End,             ActionDone);
            }
            else
            {
                Runner.Wait(PickStep.WaitModuleReady, WaitModuleReady)
                        .Run(PickStep.Picking1,        Pick1,          Pick1Done,      _moveTimeout)
                        .End(PickStep.End,             ActionDone);
            }

            return Runner.Status;
        }
        public void Abort()
        {
            _efem.Halt();
        }

        private bool WaitModuleReady()
        {
            if (_efem.IsInPauseStatus)
            {
                return false;
            }
            return _efem.Status == RState.End;
        }

        private bool Pick1()
        {
            return _efem.Pick(_targetModule, _targetSlot, _hand,_flip);
        }

        private bool Pick1Done()
        {
            if (_efem.Status == RState.End)
            {
                WaferManager.Instance.WaferMoved(_targetModule, _targetSlot, ModuleName.EfemRobot, (int)_hand);
                return true;
            }
            else if (_efem.Status == RState.Failed)
            {
                NotifyError(eEvent.ERR_EFEM_ROBOT,  $"Efem robot picking failed: {_efem.Status}",-1);
                return true;
            }

            return false;
        }

        private bool Pick2()
        {
            return _efem.Pick(_targetModule, _targetSlot2, _hand2, _flip);
        }

        private bool Pick2Done()
        {
            if (_efem.Status == RState.End)
            {
                WaferManager.Instance.WaferMoved(_targetModule, _targetSlot2, ModuleName.EfemRobot, (int)_hand2);
                return true;
            }
            else if (_efem.Status == RState.Failed)
            {
                NotifyError(eEvent.ERR_EFEM_ROBOT,  $"Efem robot picking failed: {_efem.Status}",-1);
                return true;
            }

            return false;
        }

        private bool ActionDone()
        { 
            return true; 
        }
        /// <summary>
        /// 重试
        /// </summary>
        /// <param name="step"></param>
        public RState Retry(int step)
        {
            if(!CheckPreCondition())
            {
                return RState.Failed;
            }
            _efem.Reset();
            List<Enum> preStepIds = new List<Enum>();
            AddPreSteps(PickStep.PufVacuumOff, preStepIds);
            return Runner.Retry(PickStep.PufVacuumOff, preStepIds, Module, "Pick Retry");
        }

        /// <summary>
        /// 忽略前
        /// </summary>
        /// <param name="step"></param>
        /// <param name="preStepIds"></param>
        private void AddPreSteps(PickStep step, List<Enum> preStepIds)
        {
            for (int i = 0; i < (int)step; i++)
            {
                preStepIds.Add((PickStep)i);
            }
        }
        /// <summary>
        /// 检验前面完成状态
        /// </summary>
        /// <returns></returns>
        public bool CheckCompleteCondition(int index)
        {
            if (WaferManager.Instance.CheckNoWafer(ModuleName.EfemRobot, (int)_hand))
            {
                NotifyError(eEvent.ERR_EFEM_ROBOT, $"Efem robot arm{_hand} has no wafer", -1);
                return false;
            }
            if (WaferManager.Instance.CheckHasWafer(_targetModule, _targetSlot))
            {
                NotifyError(eEvent.ERR_EFEM_ROBOT, $"{_targetModule} Slot{_targetSlot} has no wafer", -1);
                return false;
            }
            return true;
        }
    }
}