Kepler-Release/Venus/Venus_RT/Modules/TM/VenusEntity/SEMFPickRoutine.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.PreAligner;
using Venus_RT.Devices.TM;
using Venus_RT.Modules.VCE;

namespace Venus_RT.Modules.TM.VenusEntity
{
    public class SEMFPickRoutine : ModuleRoutineBase, IRoutine
    {
        private enum PickStep
        {
            seWaitModuleReady,
            sePrepareModule,
            seCheckStatus,
            seDoorOpen,
            sePAQuery,
            sePALoopStart,
            sePALoopAlign,
            sePALoopPick,
            sePALoopPlace,
            sePALoopEnd,
            sePicking,
            seNotifyDone,
        }

        private TMBase _tm;
        private IPreAlign _vpa;
        private ITransferRobot _robot;

        private VceEntity _vceModule;
        private ModuleName _targetModule;
        private int _targetSlot;
        private int _pickingTimeout;

        public int currentStepNo;

        Hand _hand;

        /// <summary>
        /// 完整pick动作仅可vce vpa取片
        /// </summary>
        /// <param name="tm"></param>
        /// <param name="robot"></param>
        /// <param name="vpa"></param>
        public SEMFPickRoutine(TMBase tm, ITransferRobot robot, IPreAlign vpa, ModuleName module) : base(module)
        {
            _tm = tm;
            _robot = robot;
            _vpa = vpa;
        }

        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;
            }
            //MoveItem pickItem = new MoveItem() { SourceModule = , };
            var pickItem = (Queue<MoveItem>)objs[0];
            _targetModule = pickItem.Peek().SourceModule;
            _targetSlot = pickItem.Peek().SourceSlot;
            _hand = pickItem.Peek().RobotHand;

            //如果目标是Vce 否则是vpa
            if (ModuleHelper.IsLoadPort(_targetModule) && ModuleHelper.IsInstalled(_targetModule))
            {
                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;
                }

                //如果vce有错误 报错
                if (_vceModule.IsError)
                {
                    LOG.Write(eEvent.ERR_TM, Module, $"Invalid target module : {_targetModule} is Error! Please solve Error first!");
                    return RState.Failed;
                }
                if (_targetSlot < 0)
                {
                    LOG.Write(eEvent.ERR_TM, Module, $"VCE target slot cannot be {_targetSlot}. Please check it first.");
                    return RState.Failed;
                }

                //if (_tm.VCESlitDoorClosed)
                //{
                //    LOG.Write(eEvent.ERR_TM, Module, $"cannot pick cause vce slitdoor not open.");
                //    return RState.Failed;
                //}
                //如果VCE门是关闭的 说明还未进行pump 无法取片等
                //if (_tm.VCESlitDoorClosed)
                //{
                //    LOG.Write(eEvent.ERR_TM, Module, $"Invalid target module : {_targetModule} slitdoor not open! Please pump down vce and open it first!");
                //    return RState.Failed;
                //}
            }
            //如果目标又不是VPA 报错
            else if (!ModuleHelper.IsAligner(_targetModule))
            {
                LOG.Write(eEvent.ERR_TM, Module, $"Invalid target module : {_targetModule} for pick action");
                return RState.Failed;
            }

            //检查目标手臂上没有wafer
            if (WaferManager.Instance.CheckHasWafer(ModuleName.TMRobot, (int)_hand))
            {
                LOG.Write(eEvent.ERR_TM, Module, $"Cannot pick as TM Robot Arm: {_hand} already has a wafer");
                return RState.Failed;
            }

            //检查目标槽位上有wafer
            if (WaferManager.Instance.CheckNoWafer(_targetModule, _targetSlot))
            {
                LOG.Write(eEvent.ERR_TM, Module, $"Cannot pick as {_targetModule} Slot {_targetSlot + 1} has no wafer");
                return RState.Failed;
            }


            //2023/10/13 朱永吉 atm mode的判断不允许自动pump vent 信号不满足手动pump vent
            //if (RouteManager.IsATMMode && _targetModule == ModuleName.VCE1)
            //{
            //    if (!_tm.IsVCEATM)
            //    {
            //        LOG.Write(eEvent.ERR_TM, Module, $"System in ATM Mode but VCE is not ATM!");
            //        return RState.Failed;
            //    }
            //
            //    if (!_tm.IsTMATM)
            //    {
            //        LOG.Write(eEvent.ERR_TM, Module, $"System in ATM Mode but TM is not ATM!");
            //        return RState.Failed;
            //    }
            //}
            //else
            //{
            //    if (_tm.IsVCEATM)
            //    {
            //        LOG.Write(eEvent.ERR_TM, Module, $"System not in ATM Mode but VCE is ATM!");
            //        return RState.Failed;
            //    }
            //
            //    if (_tm.IsTMATM)
            //    {
            //        LOG.Write(eEvent.ERR_TM, Module, $"System not in ATM Mode but TM is ATM!");
            //        return RState.Failed;
            //    }
            //}

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

            return Runner.Start(Module, $"Pick from {_targetModule}");
        }

        public RState Monitor()
        {
            //if (ModuleHelper.IsAligner(_targetModule) && Singleton<RouteManager>.Instance.IsAutoRunning)
            //{
            //    Runner.Wait(PickStep.seWaitModuleReady, CheckModuleReady, 10 * 60 * 1000)
            //          .Run(PickStep.sePrepareModule, PrepareModule, CheckModuleReady)
            //          .LoopStart(PickStep.sePALoopStart, "PA Loop Pick", 2, QueryOffset, Aligner1IsIdle, _pickingTimeout)
            //          .LoopRunIf(PickStep.sePALoopPick,  _vpa.IsOverRange, Picking, WaitPickDone, _pickingTimeout)
            //          .LoopRunIf(PickStep.sePALoopPlace, _vpa.IsOverRange, Placing, WaitPlaceDone, _pickingTimeout)
            //          .LoopRunIf(PickStep.sePALoopAlign, _vpa.IsOverRange, AlignWafer, Aligner1IsIdle, _pickingTimeout)
            //          .LoopEnd(PickStep.sePALoopEnd, QueryOffset, Aligner1IsIdle, _pickingTimeout)
            //          .Run(PickStep.sePicking, LastPAPick, WaitPickDone, _pickingTimeout)
            //          .End(PickStep.seNotifyDone, NullFun, 500);
            //}
            //else
            {
                Runner.Wait(PickStep.seWaitModuleReady, CheckModuleReady, 10 * 60 * 1000)
                      .Run(PickStep.sePrepareModule, PrepareModule, CheckModuleReady)
                      .RunIf(PickStep.seDoorOpen, ModuleHelper.IsLoadPort(_targetModule), VCEDoorOpen, CheckVCEDoorOpen)
                      .RunIf(PickStep.sePAQuery, ModuleHelper.IsAligner(_targetModule), QueryOffset, Aligner1IsIdle, _pickingTimeout)
                      .RunIf(PickStep.seCheckStatus, ModuleHelper.IsLoadPort(_targetModule), VCECheckStatus, CheckStatusIsOk)
                      .Run(PickStep.sePicking, Picking, WaitPickDone, _pickingTimeout)
                      .End(PickStep.seNotifyDone, NullFun, 500);
            }



            return Runner.Status;
        }



        private bool VCECheckStatus()
        {
            return _vceModule.CheckToPostMessage((int)VceMSG.CheckStatus);
        }

        private bool CheckStatusIsOk()
        {
            return _vceModule.CurrentSlot == (_targetSlot + 1);
        }

        private bool PrepareModule()
        {
            switch (_targetModule)
            {
                case ModuleName.LP1:
                case ModuleName.LP2:
                    return _vceModule.CheckToPostMessage((int)VceMSG.Goto, _targetSlot);//移动到目标槽位
                case ModuleName.Aligner1:
                case ModuleName.Aligner2:
                    return true;//10/17暂时为true 后可能要求旋转到0
                default:
                    return false;
            }
        }

        private bool WaitPlaceDone()
        {
            if (_robot.Status == RState.Running)
            {
                return false;
            }
            else if (_robot.Status == RState.End)
            {
                WaferManager.Instance.WaferMoved(ModuleName.TMRobot, (int)_hand, _targetModule, _targetSlot);
                return true;
            }
            else
            {
                Runner.Stop($"TM Robot Placing failed, {_robot.Status}");
                return true;
            }
        }

        //private bool P

        private bool WaitPickDone()
        {
            if (_robot.Status == RState.Running)
            {
                return false;
            }
            else if (_robot.Status == RState.End)
            {
                WaferManager.Instance.WaferMoved(_targetModule, _targetSlot, ModuleName.TMRobot, (int)_hand);
                return true;
            }
            else
            {
                Runner.Stop($"TM Robot Picking failed, {_robot.Status}");
                return true;
            }
        }

        private bool VCEDoorOpen()
        {
            return _tm.TurnSlitDoor(VCE2LP.QueryLP2VCE(_targetModule), true);
        }

        private bool CheckVCEDoorOpen()
        {
            return _tm.CheckSlitValveOpen(VCE2LP.QueryLP2VCE(_targetModule));
        }

        private bool AlignWafer()
        {
            return _vpa.Align();
        }


        private bool QueryOffset()
        {
            return _vpa.QueryOffset();
        }

        private bool Aligner1IsIdleAndInRange()
        {
            if (_vpa.Status == RState.Running)
            {
                return false;
            }
            else if (_vpa.Status == RState.End)
            {
                return true;
            }
            else
            {
                Runner.Stop($"VPA Query Offset failed, {_vpa.Status}");
                return true;
            }
        }

        private bool Aligner1IsIdle()
        {
            if (_vpa.Status == RState.Running)
            {
                return false;
            }
            else if (_vpa.Status == RState.End)
            {
                return true;
            }
            else
            {
                Runner.Stop($"VPA Query Offset failed, {_vpa.Status}");
                return true;
            }
        }

        private bool WaitRobotIsIdle()
        {
            if (_robot.Status == RState.Running)
            {
                return false;
            }
            else if (_robot.Status == RState.End)
            {
                return true;
            }
            else
            {
                Runner.Stop($"TM Robot QueryOffset failed, {_robot.Status}");
                return true;
            }
        }

        private bool LastPAPick()
        {
            if (_vpa.IsOverRange)
            {
                Runner.Stop($"VPA is Over Range in 3 times,Pick Failed");
                return false;
            }
            else
            {
                if ((_vpa.ROffset != 0 || _vpa.TOffset != 0))
                {
                    int[] RT = calculateRT(_vpa.ROffset, _vpa.TOffset);
                    LOG.Write(eEvent.INFO_TM_ROBOT, ModuleName.TMRobot, $"will pick from PA with R:{RT[1]} D:{RT[0]}");
                    return _robot.PickWithOffset(_targetModule, 0, _hand, RT[1], RT[0]);
                }
                else
                    return _robot.Pick(_targetModule, 0, _hand);
            }

        }

        private bool Picking()
        {
            //到达目标点位 pick固定槽位点的wafer
            if (ModuleHelper.IsAligner(_targetModule))
            {
                if ((_vpa.ROffset != 0 || _vpa.TOffset != 0))
                {
                    int[] RT = calculateRT(_vpa.ROffset, _vpa.TOffset);
                    LOG.Write(eEvent.INFO_TM_ROBOT, ModuleName.TMRobot, $"will pick from PA with R:{RT[1]} D:{RT[0]}");
                    return _robot.PickWithOffset(_targetModule, 0, _hand, RT[1], RT[0]);
                }
                else
                    return _robot.Pick(_targetModule, 0, _hand);
            }
            else
                return _robot.Pick(VCE2LP.QueryLP2VCE(_targetModule), 0, _hand);
        }

        private bool Placing()
        {
            return _robot.Place(_targetModule, 0, _hand);
        }

        private int[] calculateRT(int OriginR, int OriginS)
        {
            int[] RT = new int[2];
            try
            {
                bool desflag = false;
                double angle = 0;
                if (OriginR / 10 < 90)
                    angle = OriginR / 10;
                if (OriginR / 10 < 270 && OriginR / 10 > 90)
                {
                    desflag = true;
                    angle = OriginR / 10 - 90;
                }
                if (OriginR / 10 > 270)
                    angle = 450 - OriginR / 10;

                double angleInRadians = angle * Math.PI / 180;
                //OriginS * 25.4 inch 转化为 mm
                double Robot2PA = OriginS * 25.4 / 10000;
                double Robot2Wafer = Math.Sqrt(Robot2PA * Robot2PA + 402.5 * 402.5 - 2 * Robot2PA * 402.5 * Math.Cos(angleInRadians));
                //距离（mm）
                double distance = Robot2Wafer - 402.5;

                //角度（度）
                double angleA = Math.Asin(Robot2PA * Math.Sin(angleInRadians) / Robot2Wafer) * 180 / Math.PI;
                if (desflag)
                    angleA = -angleA;
                RT[0] = (int)(angleA * 1000);
                RT[1] = (int)(distance * 1000);
            }

            catch
            {
                LOG.Write(eEvent.ERR_TM_ROBOT, ModuleName.TMRobot, "错误纠偏计算");
                RT[0] = 0;
                RT[1] = 0;
            }
            return RT;
        }

        private bool CheckModuleReady()
        {
            switch (_targetModule)
            {
                case ModuleName.LP1:
                case ModuleName.LP2:
                    return _vceModule.IsIdle;
                case ModuleName.Aligner1:
                case ModuleName.Aligner2:
                    return _vpa.Status == RState.End;
                default:
                    return false;
            }

        }

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