JET-EFEM/athos/athosRT/Modules/EFEMs/Routine/PickRoutine.cs

using Aitex.Core.Common;
using Aitex.Core.RT.Device;
using Aitex.Core.RT.Device.Unit;
using Aitex.Core.RT.Event;
using Aitex.Core.RT.SCCore;
using Aitex.Core.Util;
using Aitex.Sorter.Common;
using athosRT.Devices;
using athosRT.Devices.PA;
using athosRT.FSM;
using athosRT.tool;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.SubstrateTrackings;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.LoadPorts;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.LoadPorts.LoadPortBase;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robot;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots.RobotBase;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Numerics;
using System.Text;
using System.Threading.Tasks;
using static athosRT.Modules.FLP.FlipperEntity;
using EV = athosRT.tool.EV;

namespace athosRT.Modules.EFEMs.Routine
{
    public class PickRoutine : ModuleRoutineBase, FSM.IRoutine
    {
        protected bool MultiWaferSize = Singleton<DeviceDefineManager>.Instance.GetValue<bool>(nameof(MultiWaferSize)).GetValueOrDefault();
        private SCConfigItem _scPickTimeout = null;
        private int _timeout = 0;
        private int _axisTimeout = 0;
        private int _axisSpeed = 0;
        public double WaitForPickTime = 0.0;
        private LoadPort _lpDevice;
        private bool _skipWait;
        protected readonly bool HaveMotionAxis = Singleton<DeviceDefineManager>.Instance.GetValue<bool>("MotionAxisInstalled").GetValueOrDefault();
        public ModuleName Source { get; set; }

        public int Slot { get; set; }

        public Hand Blade { get; set; }
        protected int OffsetX;
        protected int OffsetY;
        protected int OffsetZ;

        public RobotBaseDevice robot;
        protected Aligner aligner = null;
        protected IoCoolingBuffer buffer1   = null;
        protected IoCoolingBuffer buffer2   = null;
        protected PreAligner aligner1  = null;
        protected PreAligner aligner2  = null;
        protected LoadLockDevice ll1;
        protected LoadLockDevice ll2;
        protected IoCoolingBuffer _ioCoolBuffer;
        private int _existInterval = SC.GetValue<int>("Robot.Robot.ExistInterval");
        private bool _isFlipper = false;

        public PickRoutine(ModuleName module) : base(module)
        {
            robot = DEVICE.GetDevice<RobotBaseDevice>("Robot");
            _scPickTimeout = SC.GetConfigItem("Robot.TimeLimitForPickWafer");
            aligner = DEVICE.GetDevice<Aligner>("Aligner");
            buffer1 = DEVICE.GetDevice<IoCoolingBuffer>("CoolingBuffer1");
            buffer2 = DEVICE.GetDevice<IoCoolingBuffer>("CoolingBuffer2");
            aligner1 = DEVICE.GetDevice<PreAligner>("Aligner1");
            aligner2 = DEVICE.GetDevice<PreAligner>("Aligner2");
            if (!HaveMotionAxis)
                return;
            _axisTimeout = SC.GetValue<int>("MotionAxis.MoveTimeout");
            _axisSpeed = SC.GetValue<int>("MotionAxis.AutoSpeed");
        }
        public RState Start(params object[] objs)
        {
            Reset();
            _isFlipper = false;
            _timeout = robot.RobotCommandTimeout * 1000;
            ModuleName moduleName = ModuleName.System;
            string module = moduleName.ToString();
            object[] objArray = new object[1];
            moduleName = Source;
            objArray[0] = (object)string.Format("{0}{1:D2} by {2}", (object)moduleName.ToString(), (object)(this.Slot + 1), (object)this.Blade.ToString());
            //EV.PostMessage<EventEnum>(module, EventEnum.PickingWaferFromChamberBegins, objArray);
            if (ModuleHelper.IsTurnOverStation(Source))
                _isFlipper = true;

            if (Singleton<WaferManager>.Instance.CheckNoWafer(Source, Slot))
            {
                LogObject.Error("robot", $"cannot pick from Source:{Source} Slot:{Slot} to {Blade} as no wafer");
                return RState.Failed;
            }
            return Runner.Start(Module, "Pick Routine");
        }

        public RState Monitor()
        {

            if (_isFlipper)
                Runner
                    .Wait(PickStep.WaitForPickModuleIdle, CheckFlipperIsIdle, (int)WaitForPickTime * 1000)
                    .Run(PickStep.NotifyFlipperPrepare, NotifyFlipperOpen, CheckFlipperOpen, 80 * 1000)
                    .Run(PickStep.CheckBladeWaferIsExist, CheckBladeWaferIsExist, WaitRobotMotion, _timeout)
                    .Run(PickStep.CheckBeforePick, NullFun, CheckBeforePick, _timeout)
                    .Run(PickStep.PickWafer, PickWafer, WaitRobotMotion, 100000)
                    .Run(PickStep.NotifyFlipperOver, NotifyFlipperClose, CheckFlipperIsIdle,_timeout)
                    .End(PickStep.CheckAfterPick, NullFun, CheckAfterPick, _timeout);

            else
                Runner
                    .Delay(PickStep.WaitForPickModuleIdle, (int)WaitForPickTime * 1000)
                    .Run(PickStep.CoolBufferMoveUP, CoolBufferMoveUP, WaitCoolBufferMoveUp, _timeout)
                    .Run(PickStep.CheckBladeWaferIsExist, CheckBladeWaferIsExist, WaitRobotMotion, _timeout)
                    .Run(PickStep.CheckBeforePick, NullFun, CheckBeforePick, _timeout)
                    .Run(PickStep.PickWafer, PickWafer, WaitRobotMotion, 100000)
                    .End(PickStep.CheckAfterPick, NullFun,CheckAfterPick, _timeout);


            return Runner.Status;
        }

        private bool CheckAfterPick()
        {
            switch (Blade)
            {
                case Hand.Blade1:
                    if (!CheckSeneorHasWafer(ModuleName.Robot, (int)Blade))
                    {
                        LogObject.Error("Robot", "Wafer Is Not Detected on blade1 After Pick");
                       
                        EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferNotDetectedAfterPick);
                        return false;
                    }
                    break;
                case Hand.Blade2:
                    for (int index = 0; index < robot.Blade2Slots; ++index)
                    {
                        if (!CheckSeneorHasWafer(ModuleName.Robot, 1 + index))
                        {
                            LogObject.Error("Robot", "Wafer Is Not Detected on blade2 After Pick");
                            EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferNotDetectedAfterPick);
                            return false;
                        }
                    }
                    break;
                default:
                    for (int slot1 = 0; slot1 < robot.Blade2Slots + 1; ++slot1)
                    {
                        if (!CheckSeneorHasWafer(ModuleName.Robot, slot1))
                        {
                            EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferNotDetectedAfterPick);
                            return false;
                        }
                    }
                    break;
            }
            return true;
         }

        private bool PickWafer()
        {
            bool flag = OffsetX == 0 && OffsetY == 0 && OffsetZ == 0 ? robot.Pick((RobotArmEnum)Blade, Source.ToString(), Slot) : robot.PickEx((RobotArmEnum)Blade, Source.ToString(), Slot, (float)OffsetX, (float)OffsetY, (float)OffsetZ);
            if (flag)
            {
                LogObject.Info("robot",$"pick from Source:{Source} Slot:{Slot} to {Blade}");
                return true;
            }
            
            {
                LogObject.Error("robot", $"cannot pick from Source:{Source} Slot:{Slot} to {Blade}");
                return false;
            }
        }

        private bool CheckBeforePick()
        {
            //"Check wafer information", this.Source, this.Slot, this.Blade,
            string reason ="";
            if (!CheckRobotMotionInterlock(Source, Slot, out reason))
            {
                LogObject.Error("System", reason);
                return false;
            }
            switch (Blade)
            {
                case Hand.Blade1:
                    if (ModuleHelper.IsLoadLock(Source) && Singleton<EfemEntity>.Instance.IsOnlineMode)
                    {
                        //Singleton<WaferManager>.Instance.CreateWafer(Source, Slot, WaferStatus.Normal);
                    }
                    else if (!Singleton<WaferManager>.Instance.CheckWafer(Source, Slot, WaferStatus.Normal))
                    {
                        EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferAbsentWithoutRecord, (object)string.Format("target {0}{1:D2}", (object)Source.ToString(), (object)(Slot + 1)));
                        return false;
                    }
                    if (!Singleton<WaferManager>.Instance.CheckWafer(ModuleName.Robot, 0, WaferStatus.Empty))
                    {
                        EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferDetectedBeforePick);
                        return false;
                    }
                    if (!CheckSensorNoWafer(ModuleName.Robot, 0))
                    {
                        EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferDetectedBeforePick);
                        return false;
                    }
                    if (ModuleHelper.IsBuffer(Source) && !CheckSeneorHasWafer(Source, Slot))
                    {
                        EV.PostWarningLog(Source.ToString(), "Can not pick wafer from buffer, Buffer slot has no wafer.");
                        return false;
                    }
                    break;
                case Hand.Blade2:
                    bool flag1 = true;
                    for (int index = 0; index < robot.Blade2Slots; ++index)
                    {
                        //if (ModuleHelper.IsLoadLock(Source) && Singleton<EfemEntity>.Instance.IsOnlineMode)
                        //    Singleton<WaferManager>.Instance.CreateWafer(Source, Slot, WaferStatus.Normal);
                        //else
                        //    flag1 = Singleton<WaferManager>.Instance.CheckWafer(Source, Slot + index, WaferStatus.Normal);
                        if (!Singleton<WaferManager>.Instance.CheckWafer(ModuleName.Robot, (int)(Blade + index), WaferStatus.Empty))
                        {
                            EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferDetectedBeforePick);
                            return false;
                        }
                        if (!CheckSensorNoWafer(ModuleName.Robot, (int)(Blade + index)))
                        {
                            EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferDetectedBeforePick);
                            return false;
                        }
                        if (ModuleHelper.IsBuffer(Source))
                        {
                            flag1 = CheckSeneorHasWafer(Source, Slot + index);
                            if (!flag1)
                            {
                                EV.PostWarningLog(Source.ToString(), "Can not pick wafer from buffer, Buffer slot has no wafer.");
                                return false;
                            }
                        }
                    }
                    if (!flag1)
                    {
                        EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferAbsentWithoutRecord, string.Format("target {0}{1:D2}", (object)Source.ToString(), (object)(Slot + 1)));
                        return false;
                    }
                    break;
                default:
                    bool flag2 = true;
                    for (int slot1 = 0; slot1 < robot.Blade2Slots + 1; ++slot1)
                    {
                        //if (ModuleHelper.IsLoadLock(Source) && Singleton<EfemEntity>.Instance.IsOnlineMode)
                        //    Singleton<WaferManager>.Instance.CreateWafer(Source, Slot, WaferStatus.Normal);
                        //else
                        //    flag2 = Singleton<WaferManager>.Instance.CheckWafer(Source, Slot, WaferStatus.Normal);
                        if (!Singleton<WaferManager>.Instance.CheckWafer(ModuleName.Robot, slot1, WaferStatus.Empty))
                        {
                            EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferDetectedBeforePick);
                            return false;
                        }
                        if (!CheckSensorNoWafer(ModuleName.Robot, slot1))
                        {
                            EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferDetectedBeforePick);
                            return false;
                        }
                        if (ModuleHelper.IsBuffer(Source) && Blade == Hand.Both)
                        {
                            flag2 &= this.CheckSeneorHasWafer(Source, Slot + slot1);
                            if (!flag2)
                            {
                                EV.PostWarningLog(Source.ToString(), "Can not pick wafer from buffer, Buffer slot has no wafer.");
                                return false;
                            }
                        }
                    }
                    if (!flag2)
                    {
                        EV.PostMessage<EventEnum>(ModuleName.System.ToString(), EventEnum.WaferAbsentWithoutRecord, string.Format("target {0}{1:D2}", (object)Source.ToString(), (object)Slot));
                        return false;
                    }
                    break;
            }
            if (!ModuleHelper.IsCoolingBuffer(Source) || DEVICE.GetDevice<IoCoolingBuffer>(Source.ToString()).CheckPinUp())
                return true;
            EV.PostWarningLog(Source.ToString(), "Can not pick wafer from buffer, buffer isn't pick position.");
            return false;
        }
        
        private bool WaitRobotMotion()
        {
            if (!robot.IsBusy)
            {
                LogObject.Info("Robot", "Robot准备完毕");
                if (robot.IsError)
                {
                    if (ModuleHelper.IsLoadPort(Source))
                        Singleton<RouteManager1>.Instance.GetLP(Source).PostMsg(268435443);
                    LogObject.Info("Robot", "Robot进入Error");
                    return false;
                }
                return true;
            }
            else
            {
                //LogObject.Error("Robot", "等待Robot准备完毕");
                return false;
            }
        }
        private bool CheckBladeWaferIsExist()
        {
            //this.robot, this.Blade, this._timeout
            if (!robot.ReadParameter(new object[3] { "CheckWaferIsPresence", (Blade == Hand.Blade1 ? 0 : 1), _existInterval }))
            {
                //手臂上有wafer 无法pick
                LogObject.Error("Robot", "手臂上有片 无法pick");
                return false;
            }
            else
            {
                LogObject.Info("Robot", "手臂上无片 可以正常pick");
                return true;
            }
        }

        private bool WaitCoolBufferMoveUp()
        {
            if (ModuleHelper.IsCoolingBuffer(Source))
            {
                IoCoolingBuffer device = GetCoolBuffer(Source);
                if (!device.Error)
                    return device.CheckMovedUp() && !device.Busy;
                return false;
            }
            return true;
        }

        private bool CoolBufferMoveUP()
        {
            //(4, this.GetCoolBuffer(this.Source), Singleton<WaferManager>.Instance.GetWaferSize(this.Source, this.Slot), this._timeout);
            
            if (ModuleHelper.IsCoolingBuffer(Source))
            {
                WaferSize size = Singleton<WaferManager>.Instance.GetWaferSize(Source, Slot);
                IoCoolingBuffer device = GetCoolBuffer(Source);
                try
                {
                    return (device.CheckPinUp() || device.Move(size, true, out _)) ? true : false;

                }
                catch (Exception ex)
                {
                    Trace.WriteLine($"{Name} Error:{ex}");
                    LogObject.Error(Name, $"Error:{ex}");
                }
            }
            return true;
        }

        public void Abort()
        {

        }

        public IoCoolingBuffer GetCoolBuffer(ModuleName coolbufferName)
        {
            switch (coolbufferName)
            {
                //case ModuleName.Aligner1:
                //    _ioCoolBuffer = aligner1;
                //    break;
                //case ModuleName.Aligner2:
                //    _ioCoolBuffer = aligner2;
                //    break;
                case ModuleName.CoolingBuffer1:
                    _ioCoolBuffer = buffer1;
                    break;
                case ModuleName.CoolingBuffer2:
                    _ioCoolBuffer = buffer2;
                    break;
            }
            return _ioCoolBuffer;
        }

        protected bool CheckRobotMotionInterlock(ModuleName chamber, int slot, out string reason)
        {
            LogObject.Info("Robot",$"chamber:{chamber},slot:{slot}");
            reason = string.Empty;
            if (robot.RobotState == RobotStateEnum.Error)
            {
                reason = "robot is error.";
                if(ModuleHelper.IsLoadPort(Source))
                    Singleton<RouteManager1>.Instance.GetLP(Source).PostMsg(268435443);
                return false;
            }
            if (!robot.IsReady())
            {
                reason = string.Format("robot isn't Ready.");
                return false;
            }
            if (chamber == ModuleName.Aligner || ModuleHelper.IsTurnOverStation(chamber))
            {
                //if (aligner.Moving)
                //{
                //    reason = string.Format("aligner is moving.");
                //    return false;
                //}
                if (robot.IsReady())
                    return true;
                reason = string.Format("aligner isn't Ready.");
                return false;
            }
            if (ModuleHelper.IsLoadPort(chamber))
            {
                LoadPortBaseDevice device = DEVICE.GetDevice<LoadPortBaseDevice>(chamber.ToString());
                string lpreason = string.Empty;
                if (!device.IsEnableTransferWafer(out reason))
                {
                    reason = string.Format($"{chamber} isn't Ready.The reason is {lpreason}");
                    return false;
                }
                if (device.MapError)
                {
                    reason = string.Format("{0} Map has crossed error.", (object)chamber.ToString());
                    return false;
                }
                if (ModuleHelper.IsLoadPort(chamber) && !Singleton<RouteManager1>.Instance.GetLP(chamber).IsLoaded)
                {
                    if (Singleton<RouteManager1>.Instance.GetLP(chamber).IsDoorOpen)
                    {
                        reason = string.Format("{0} SMIF1PODOPEN signal is Off.", (object)chamber.ToString());
                        return false;
                    }
                    if (Singleton<RouteManager1>.Instance.GetLP(chamber).IsPresent)
                    {
                        reason = string.Format("{0} SensorSMIF1PODPRESENT signal is Off,Foup is presence.", (object)chamber.ToString());
                        return false;
                    }
                    if (Singleton<RouteManager1>.Instance.GetLP(chamber).IsReady)
                    {
                        reason = string.Format("{0} SensorSMIF1READY signal is Off.", (object)chamber.ToString());
                        return false;
                    }
                }
                return true;
            }
            if (ModuleHelper.IsLoadLock(chamber))
            {

                if (WaferManager.Instance.CheckNoWafer(chamber, 0))
                    reason = "记录中该位置没有wafer";

                switch (chamber)
                {
                    case ModuleName.LL1:
                    case ModuleName.LLA:
                    case ModuleName.PMA:
                        if (!DeviceModel.LL1DoorIsOpen.Value)
                            reason = $"{chamber}门信号未开启 无法传送";
                        return DeviceModel.LL1DoorIsOpen.Value && WaferManager.Instance.CheckHasWafer(chamber, 0);
                    case ModuleName.LL2:
                    case ModuleName.LLB:
                    case ModuleName.PMB:
                        if (!DeviceModel.LL2DoorIsOpen.Value)
                            reason = $"{chamber}门信号未开启 无法传送";
                        return DeviceModel.LL2DoorIsOpen.Value && WaferManager.Instance.CheckHasWafer(chamber, 0);
                    default:
                        return true;
                }
            }
                //return DEVICE.GetDevice<LoadLockDevice>(chamber.ToString()).IsEnableTransferWafer(out reason);
            if (ModuleHelper.IsCoolingBuffer(chamber))
            {
                IoCoolingBuffer device = DEVICE.GetDevice<IoCoolingBuffer>(chamber.ToString());
                if (device.Busy)
                {
                    reason = "buffer is not idle";
                    return false;
                }
                if (device.CheckPinUp())
                    return true;
                reason = "buffer pin not up position";
                return false;
            }

            if (ModuleHelper.IsAligner(chamber))
            {
                //Singleton<WaferManager>.Instance.CheckHasWafer(chamber, 1);
                //PreAligner device = DEVICE.GetDevice<PreAligner>(chamber.ToString());
                //if (device.Busy)
                //{
                //    reason = "Aligner is not Idle";
                //    return false;
                //}
                return true;
            }

            if (ModuleHelper.IsBuffer(chamber))
            {

                return true;
            }
            reason = "error target";
            return false;
        }

        protected bool CheckSensorNoWafer(ModuleName chamber, int slot)
        {
            if (SC.GetValue<bool>("System.IsSimulatorMode") || chamber != ModuleName.Robot)
                return Singleton<WaferManager>.Instance.CheckNoWafer(chamber, slot);
            return slot == 0 ? !robot.IsWaferPresenceOnBlade1 : !robot.IsWaferPresenceOnBlade2;
        }
        protected bool CheckSeneorHasWafer(ModuleName chamber, int slot)
        {
            if (SC.GetValue<bool>("System.IsSimulatorMode") || chamber != ModuleName.Robot)
                return Singleton<WaferManager>.Instance.CheckHasWafer(chamber, slot);
            return slot == 0 ? robot.IsWaferPresenceOnBlade1 : robot.IsWaferPresenceOnBlade2;
        }

        private bool CheckFlipperIsIdle()
        {
            return Singleton<RouteManager1>.Instance.GetFlipper().IsIdle;
        }

        private bool NotifyFlipperOpen()
        {
            Singleton<RouteManager1>.Instance.GetFlipper().PostMsg(FlipperMSG.PrepareTransfer);
            return true;
        }

        private bool NotifyFlipperClose()
        {
            Singleton<RouteManager1>.Instance.GetFlipper().PostMsg(FlipperMSG.EndTransfer);
            return true;
        }

        private bool CheckFlipperOpen()
        {
            if (!Singleton<RouteManager1>.Instance.GetFlipper().IsTransfer)
            {
                Singleton<RouteManager1>.Instance.GetFlipper().PostMsg(FlipperMSG.PrepareTransfer);
                return false;
            }
            else
                return true;
        }


        public enum PickStep
        {
            WaitForPickModuleIdle,
            CoolBufferMoveUP,
            CheckBladeWaferIsExist,
            CheckBeforePick,
            PickWafer,
            CheckAfterPick,
            NotifyFlipperPrepare,
            NotifyFlipperOver,
        }
    }
}