Mars200/Mars/JetEfem/Efems/JetEfemRobot.cs

using System;
using System.Collections.Generic;
using System.Xml;
using Aitex.Core.RT.DataCenter;
using Aitex.Core.RT.Device;
using Aitex.Core.RT.Event;
using Aitex.Core.RT.Log;
using Aitex.Core.RT.SCCore;
using Aitex.Sorter.Common;
using MECF.Framework.Common.CommonData;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.SubstrateTrackings;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Efems.Rorzes;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots.RobotBase;

namespace JetEfemLib.Efems
{
    public class JetEfemRobot : RobotBaseDevice, IEfemRobotCallback
    {
        private ModuleName _module;
        private string _efemName;
        private IRorzeEfemController _efem;

        public string Address { get; }
        public bool IsConnected { get; }

        public string EFEMGripStateBlade1
        {
            get;
            set;
        }

        public string EFEMGripStateBlade2
        {
            get;
            set;
        }

        //EfemRobotHomeRoutine _homeRoutine;

        public JetEfemRobot(string module, XmlElement node, string ioModule = "") : base("EfemRobot", "EfemRobot")
        {
            var attrModule = node.GetAttribute("module");
            base.Module = string.IsNullOrEmpty(attrModule) ? module : attrModule;
            base.Name = node.GetAttribute("id");

            _efemName = node.GetAttribute("efem");
            _module = ModuleHelper.Converter(Module);
        }

        public JetEfemRobot(string module, string name, string efemName) : base("EfemRobot", "EfemRobot")
        {
            _efemName = efemName;
            _module = ModuleHelper.Converter(module);
        }

        public bool Connect()
        {
            return true;
        }

        public bool Disconnect()
        {
            return true;
        }

        protected override bool Init()
        {

            if (_efem == null && !string.IsNullOrEmpty(_efemName))
            {
                _efem = DEVICE.GetDevice<RorzeEfem>(_efemName);
                _efem.SetRobotCallback(ModuleHelper.Converter(Module), this);
            }

            RorzeEfemHandler.ParameterModuleMap = new Dictionary<string, ModuleName>()
            {
                {"SYSTEM", ModuleName.System},
                {"System", ModuleName.System},
                {"ROB", ModuleName.EfemRobot},
                {"P1", ModuleName.LP1},
                {"P2", ModuleName.LP2},
                {"P3", ModuleName.LP3},
                {"P4", ModuleName.LP4},
                {"LLA", ModuleName.LLA},
                {"LLB", ModuleName.LLB},
                //{"BF1", ModuleName.Buffer},
                //{"ALIGN", ModuleName.Aligner},
                {"ALIGN3", ModuleName.Aligner1},
                {"ALIGN4", ModuleName.Aligner2},
                {"ALIGN", ModuleName.Cooling1},
                {"ALIGN2", ModuleName.Cooling2},
            };

            DATA.Subscribe($"{Module}1.WaferSize", () => WaferManager.Instance.GetWaferSize(_module, 0).ToString());
            DATA.Subscribe($"{Module}2.WaferSize", () => WaferManager.Instance.GetWaferSize(_module, 1).ToString());

            DATA.Subscribe("EfemRobot.GripStateBlade1", () => EFEMGripStateBlade1);
            DATA.Subscribe("EfemRobot.GripStateBlade2", () => EFEMGripStateBlade2);

            IsOnline = true;

            return true;
        }

        protected override bool fClear(object[] param)
        {
            return true;
        }

        protected override bool fStartReadData(object[] param)
        {
            ModuleName tempmodule = (ModuleName)Enum.Parse(typeof(ModuleName), param[0].ToString());
            _efem.QueryMapResult(tempmodule.ToString(), out string reason);
            return true;
        }


        protected override bool fStartSetParameters(object[] param)
        {
            return true;
        }

        protected override bool fStartTransferWafer(object[] param)
        {
            return true;
        }

        protected override bool fStartUnGrip(object[] param)
        {
            RobotArmEnum arm = (RobotArmEnum)param[0];
            if (!_efem.RobotGrip((Hand)(int)arm, false, out string reason))
            {
                EV.PostAlarmLog(Module, $"EFEMRobot can not UnGrip " + reason);
                return false;
            }

            if (arm == RobotArmEnum.Lower)
            {
                EFEMGripStateBlade1 = "OFF";
            }

            if (arm == RobotArmEnum.Upper)
            {
                EFEMGripStateBlade2 = "OFF";
            }

            return true;
        }

        protected override bool fStartGrip(object[] param)
        {
            RobotArmEnum arm = (RobotArmEnum)param[0];
            if (!_efem.RobotGrip((Hand)(int)arm , true , out string reason))
            {
                EV.PostAlarmLog(Module, $"EFEMRobot can not Grip " + reason);
                return false;
            }

            if(arm== RobotArmEnum.Lower)
            {
                EFEMGripStateBlade1 = "ON";
            }

            if (arm == RobotArmEnum.Upper)
            {
                EFEMGripStateBlade2 = "ON";
            }

            return true;
        }

        protected override bool fStartGoTo(object[] param)
        {
            RobotArmEnum arm = (RobotArmEnum)param[0];
            ModuleName module = (ModuleName)Enum.Parse(typeof(ModuleName), param[1].ToString());
            int slot = int.Parse(param[2].ToString());

            if (WaferManager.Instance.CheckHasWafer(ModuleName.EfemRobot, arm == RobotArmEnum.Lower ? 0 : 1))
            {
                if (!_efem.MoveToReadyPut(module, slot, arm == RobotArmEnum.Lower ? Hand.Blade1 : Hand.Blade2, out string reason))
                {
                    EV.PostAlarmLog(Module, $"Can not move to {module} slot {slot + 1}, " + reason);
                    return false;
                }
            }
            else
            {
                if (!_efem.MoveToReadyGet(module, slot, arm == RobotArmEnum.Lower ? Hand.Blade1 : Hand.Blade2, out string reason))
                {
                    EV.PostAlarmLog(Module, $"Can not move to {module} slot {slot + 1}, " + reason);
                    return false;
                }
            }

            CmdRobotArm = arm;
            CmdTarget = module;
            CmdTargetSlot = slot;

            MoveInfo = new RobotMoveInfo()
            {
                Action = RobotAction.Moving,
                ArmTarget = CmdRobotArm == RobotArmEnum.Lower ? RobotArm.ArmA : RobotArm.ArmB,
                BladeTarget = BuildBladeTarget(),
            };

            return true;
        }

        protected override bool fGoToComplete(object[] param)
        {
            MoveInfo = new RobotMoveInfo()
            {
                Action = RobotAction.Moving,
                ArmTarget = CmdRobotArm == RobotArmEnum.Lower ? RobotArm.ArmA : RobotArm.ArmB,
                BladeTarget = BuildBladeTarget(),
            };

            return base.fGoToComplete(param);
        }

        protected override bool fStartMapWafer(object[] param)
        {
            ModuleName tempmodule = (ModuleName)Enum.Parse(typeof(ModuleName), param[0].ToString());
            int thicknessIndex = int.Parse(param[1].ToString());

            CmdRobotArm = RobotArmEnum.Lower;
            CmdTarget = tempmodule;
            CmdTargetSlot = 0;

            MoveInfo = new RobotMoveInfo()
            {
                Action = RobotAction.Moving,
                ArmTarget = CmdRobotArm == RobotArmEnum.Lower ? RobotArm.ArmA : RobotArm.ArmB,
                BladeTarget = BuildBladeTarget(),
            };

            RorzeEfem efem = _efem as RorzeEfem;

            efem.MapCarrier(tempmodule.ToString(), out string _, out string reason);

            return true;
        }

        protected override bool fMapComplete(object[] param)
        {
            MoveInfo = new RobotMoveInfo()
            {
                Action = RobotAction.Moving,
                ArmTarget = CmdRobotArm == RobotArmEnum.Lower ? RobotArm.ArmA : RobotArm.ArmB,
                BladeTarget = BuildBladeTarget(),
            };

            return base.fMapComplete(param);
        }

        protected override bool fStartSwapWafer(object[] param)
        {
            return true;
        }
        private static Dictionary<ModuleName, ModuleName> _mapLL = new Dictionary<ModuleName, ModuleName>()
        {
            {ModuleName.LLA, ModuleName.LLB},
            {ModuleName.LLB, ModuleName.LLA }
        };
        private static Dictionary<int, int> _slotToFloor = new Dictionary<int, int>() {
            {0,0 },
            {2,1 },
            {4,2 },
            {6,3 }
        };
        protected override bool fStartPlaceWafer(object[] param)
        {
            RobotArmEnum arm = (RobotArmEnum)param[0];
            ModuleName module = (ModuleName)Enum.Parse(typeof(ModuleName), param[1].ToString());
            int slot = ((int[])param[2])[0];//int.Parse(param[2].ToString());

            if (!CheckEnablePlace(module, slot, arm, out string reason))
            {
                EV.PostAlarmLog(Module, $"Can not place wafer to {module} slot {slot + 1}, " + reason);
                return false;
            }
            CmdRobotArm = arm;
            CmdTarget = module;
            CmdTargetSlot = slot;

            if (SC.IsDoubleFork)
            {
                if (ModuleHelper.IsLoadLock(module))
                {
                    module = ModuleName.LLA;
                    if ((slot % 2) > 0)
                    {
                        module = ModuleName.LLB;
                        slot = slot - 1;
                    }
                    slot = _slotToFloor[slot];
                    if ((ModuleName)Enum.Parse(typeof(ModuleName), param[1].ToString()) == ModuleName.LLB) slot = slot + 4;
                }
            }
            if (!_efem.PutWafer(module, slot, (Hand)(int)arm, out reason))
            {
                EV.PostAlarmLog(Module, $"Can not place wafer to {module} slot {slot + 1}, " + reason);
                return false;
            }

            MoveInfo = new RobotMoveInfo()
            {
                Action = RobotAction.Placing,
                ArmTarget = CmdRobotArm == RobotArmEnum.Lower ? RobotArm.ArmA : RobotArm.ArmB,
                BladeTarget = BuildBladeTarget(),
            };

            return true;
        }

        private bool CheckEnablePlace(ModuleName module, int slot, RobotArmEnum arm, out string reason)
        {
            if (arm == RobotArmEnum.Both)
            {
                if (!WaferManager.Instance.CheckNoWafer(module, slot) && !WaferManager.Instance.CheckNoWafer(module, slot + 1))
                {
                    reason = "target has wafer";
                    return false;
                }
                if (!WaferManager.Instance.CheckHasWafer(ModuleName.EfemRobot, 0) && !WaferManager.Instance.CheckHasWafer(ModuleName.EfemRobot, 1))
                {
                    reason = "robot arm has no wafer";
                    return false;
                }
            }
            else
            {
                if (!WaferManager.Instance.CheckNoWafer(module, slot))
                {
                    reason = "target has wafer";
                    return false;
                }
                if (!WaferManager.Instance.CheckHasWafer(ModuleName.EfemRobot, (int)arm))
                {
                    reason = "robot arm has no wafer";
                    return false;
                }
            }


            reason = string.Empty;
            return true;
        }


        protected override bool fPlaceComplete(object[] param)
        {
            if (CmdRobotArm == RobotArmEnum.Both)
            {
                WaferManager.Instance.WaferMoved(ModuleHelper.Converter(Module), (int)Hand.Blade1, CmdTarget, CmdTargetSlot);
                WaferManager.Instance.WaferMoved(ModuleHelper.Converter(Module), (int)Hand.Blade2, CmdTarget, CmdTargetSlot + 1);
            }
            else
            {
                WaferManager.Instance.WaferMoved(ModuleHelper.Converter(Module), (int)CmdRobotArm, CmdTarget, CmdTargetSlot);
            }

            MoveInfo = new RobotMoveInfo()
            {
                Action = RobotAction.Moving,
                ArmTarget = CmdRobotArm == RobotArmEnum.Lower ? RobotArm.ArmA : RobotArm.ArmB,
                BladeTarget = BuildBladeTarget(),
            };

            return base.fPlaceComplete(param);
        }

        private string BuildBladeTarget()
        {
            return (CmdRobotArm == RobotArmEnum.Upper ? "ArmB" : "ArmA") + "." + CmdTarget;
        }


        protected override bool fStartPickWafer(object[] param)
        {
            RobotArmEnum arm = (RobotArmEnum)param[0];
            ModuleName module = (ModuleName)Enum.Parse(typeof(ModuleName), param[1].ToString());
            int slot = ((int[])param[2])[0];//int.Parse(param[2].ToString());

            if (!CheckEnablePick(module, slot, arm, out string reason))
            {
                EV.PostAlarmLog(Module, $"Can not pick wafer from {module} slot {slot + 1}, " + reason);
                return false;
            }
            CmdRobotArm = arm;
            CmdTarget = module;
            CmdTargetSlot = slot;
            if (SC.IsDoubleFork)
            {
                if (ModuleHelper.IsLoadLock(module))
                {
                    module = ModuleName.LLA;
                    if ((slot % 2) > 0)
                    {
                        module = ModuleName.LLB;
                        slot = slot - 1;
                    }
                    slot = _slotToFloor[slot];
                    if ((ModuleName)Enum.Parse(typeof(ModuleName), param[1].ToString()) == ModuleName.LLB) slot = slot + 4;
                }
            }
            if (!_efem.GetWafer(module, slot, (Hand)(int)arm, out reason))
            {
                EV.PostAlarmLog(Module, $"Can not pick wafer from {module} slot {slot + 1}, " + reason);
                return false;
            }

            MoveInfo = new RobotMoveInfo()
            {
                Action = RobotAction.Picking,
                ArmTarget = CmdRobotArm == RobotArmEnum.Lower ? RobotArm.ArmA : RobotArm.ArmB,
                BladeTarget = BuildBladeTarget(),
            };

            return true;
        }

        private bool CheckEnablePick(ModuleName module, int slot, RobotArmEnum arm, out string reason)
        {
            if (arm == RobotArmEnum.Both)
            {
                if (!WaferManager.Instance.CheckHasWafer(module, slot) && !WaferManager.Instance.CheckHasWafer(module, slot + 1))
                {
                    reason = "source has no wafer";
                    return false;
                }
                if (!WaferManager.Instance.CheckNoWafer(ModuleName.EfemRobot, 0) && !WaferManager.Instance.CheckNoWafer(ModuleName.EfemRobot, 1))
                {
                    reason = "robot arm has wafer";
                    return false;
                }
            }
            else
            {
                if (!WaferManager.Instance.CheckHasWafer(module, slot))
                {
                    reason = "source has no wafer";
                    return false;
                }
                if (!WaferManager.Instance.CheckNoWafer(ModuleName.EfemRobot, (int)arm))
                {
                    reason = "robot arm has wafer";
                    return false;
                }
            }

            reason = string.Empty;
            return true;
        }

        protected override bool fPickComplete(object[] param)
        {
            if (CmdRobotArm == RobotArmEnum.Both)
            {
                WaferManager.Instance.WaferMoved(CmdTarget, CmdTargetSlot, ModuleHelper.Converter(Module), (int)Hand.Blade1);
                WaferManager.Instance.WaferMoved(CmdTarget, CmdTargetSlot + 1, ModuleHelper.Converter(Module), (int)Hand.Blade2);
            }
            else
            {
                WaferManager.Instance.WaferMoved(CmdTarget, CmdTargetSlot, ModuleHelper.Converter(Module), (int)CmdRobotArm);
            }

            MoveInfo = new RobotMoveInfo()
            {
                Action = RobotAction.Moving,
                ArmTarget = CmdRobotArm == RobotArmEnum.Lower ? RobotArm.ArmA : RobotArm.ArmB,
                BladeTarget = BuildBladeTarget(),
            };

            return base.fPickComplete(param);
        }

        protected override bool fResetToReady(object[] param)
        {
            return true;
        }

        protected override bool fReset(object[] param)
        {
            return true;
        }

        #region Init/Home
        public bool Home(out string reason)
        {
            if (!CheckToPostMessage((int)RobotMsg.StartInit))
            {
                reason = $"Can not home at {(RobotStateEnum)fsm.State} state";
                return false;
            }
            reason = string.Empty;
            return true;
        }

        protected override bool fStartInit(object[] param)
        {
            CmdRobotArm = RobotArmEnum.Lower;
            CmdTarget = ModuleName.System;
            CmdTargetSlot = 0;

            MoveInfo = new RobotMoveInfo()
            {
                Action = RobotAction.Retracting,
                ArmTarget = CmdRobotArm == RobotArmEnum.Lower ? RobotArm.ArmA : RobotArm.ArmB,
                BladeTarget = BuildBladeTarget(),
            };

            return true;
        }

        protected override bool fInitComplete(object[] param)
        {


            return base.fInitComplete(param);
        }

        protected override bool fMonitorInit(object[] param)
        {
            IsBusy = false;
            return true;
        }

        protected override bool fMonitorReadData(object[] param)
        {
            return base.fMonitorReadData(param);
        }

        #endregion



        public override bool HomeAllAxes(out string reason)
        {
            if (!_efem.HomeAllAxes(out reason))
                return false;
            IsBusy = true;
            return true;
        }

        protected override bool fError(object[] param)
        {
            string errorInfo = param[0].ToString();
            if (param.Length > 0)
                EV.PostAlarmLog(Module, errorInfo);

            if (fsm.State == (int)RobotStateEnum.Initializing)
            {

            }
            else if (fsm.State == (int)RobotStateEnum.Picking)
            {
                if(ModuleHelper.IsEfemRobot(Module) || ModuleHelper.IsEfem(ModuleHelper.Converter(Module)))
                    EV.PostAlarmLog(ModuleName.EFEM.ToString(), errorInfo);
                bool waferGot = errorInfo.Contains("|WAFER");
                if (waferGot)
                {
                    if (CmdRobotArm == RobotArmEnum.Both)
                    {
                        WaferManager.Instance.WaferMoved(CmdTarget, CmdTargetSlot, ModuleHelper.Converter(Module), (int)Hand.Blade1);
                        WaferManager.Instance.WaferMoved(CmdTarget, CmdTargetSlot + 1, ModuleHelper.Converter(Module), (int)Hand.Blade2);
                    }
                    else
                    {
                        WaferManager.Instance.WaferMoved(CmdTarget, CmdTargetSlot, ModuleHelper.Converter(Module), (int)CmdRobotArm);
                    }

                }
            }
            else if (fsm.State == (int)RobotStateEnum.Placing)
            {
                if (ModuleHelper.IsEfemRobot(Module) || ModuleHelper.IsEfem(ModuleHelper.Converter(Module)))
                    EV.PostAlarmLog(ModuleName.EFEM.ToString(), errorInfo);
                bool waferPut = errorInfo.Contains("|NONWAF");
                if (waferPut)
                {
                    if (CmdRobotArm == RobotArmEnum.Both)
                    {
                        WaferManager.Instance.WaferMoved(ModuleHelper.Converter(Module), (int)Hand.Blade1, CmdTarget, CmdTargetSlot);
                        WaferManager.Instance.WaferMoved(ModuleHelper.Converter(Module), (int)Hand.Blade2, CmdTarget, CmdTargetSlot + 1);
                    }
                    else
                    {
                        WaferManager.Instance.WaferMoved(ModuleHelper.Converter(Module), (int)CmdRobotArm, CmdTarget, CmdTargetSlot);
                    }

                }
            }


            MoveInfo = new RobotMoveInfo()
            {
                Action = RobotAction.Moving,
                ArmTarget = CmdRobotArm == RobotArmEnum.Lower ? RobotArm.ArmA : RobotArm.ArmB,
                BladeTarget = BuildBladeTarget(),
            };

            return true;
        }

        protected override bool fStartExtendForPick(object[] param)
        {
            return true;
        }

        protected override bool fStartExtendForPlace(object[] param)
        {
            return true;
        }

        protected override bool fStartRetractFromPick(object[] param)
        {
            return true;
        }

        protected override bool fStartRetractFromPlace(object[] param)
        {
            return true;
        }

        public override RobotArmWaferStateEnum GetWaferState(RobotArmEnum arm)
        {
            return RobotArmWaferStateEnum.Unknown;
        }


        public bool Pick(ModuleName target, Hand blade, int targetSlot, out string reason)
        {
            reason = string.Empty;
            return base.Pick((RobotArmEnum)(int)blade, target.ToString(), new int[] { targetSlot });
        }

        public bool Place(ModuleName target, Hand blade, int targetSlot, out string reason)
        {
            reason = string.Empty;
            return base.Place((RobotArmEnum)(int)blade, target.ToString(), new int[] { targetSlot });
        }

        public bool PickAndPlace(ModuleName pickTarget, Hand pickHand, int pickSlot, ModuleName placeTarget, Hand placeHand,
            int placeSlot, out string reason)
        {
            reason = string.Empty;
            return false;
        }

        public bool Goto(ModuleName target, Hand blade, int targetSlot, out string reason)
        {
            reason = string.Empty;
            return base.GoTo((RobotArmEnum)(int)blade, target.ToString(), targetSlot, RobotPostionEnum.PickReady);
        }


        public bool Map(ModuleName target, int thicknessIndex, out string reason)
        {
            reason = string.Empty;
            return base.WaferMapping(target, thicknessIndex);
        }

        public void NoteFailed(string error)
        {

            IsBusy = false;
            PostMsg(RobotMsg.ERROR, error);
        }

        public void NoteComplete()
        {
            if (fsm.State == (int)RobotStateEnum.Initializing)
            {
                IsBusy = false;
                return;
            }

            if (fsm.State == (int)RobotStateEnum.Mapping)
            {
                IsBusy = false;
                PostMsg(RobotMsg.ReadData, CmdTarget);
            }
            else
            {
                IsBusy = false;
                PostMsg(RobotMsg.ActionDone);
            }
        }

        public void NoteCancel(string error)
        {
            if (fsm.State == (int)RobotStateEnum.Initializing)
            {
                PostMsg(RobotMsg.ERROR, error);
                IsBusy = false;
                return;
            }
            IsBusy = false;
            PostMsg(RobotMsg.ERROR, error);
        }

    }

}