Mars200/Mars/JetEfem/Efems/EfemModule.cs

using System;
using Aitex.Core.RT.DataCenter;
using Aitex.Core.RT.Device;
using Aitex.Core.RT.Event;
using Aitex.Core.RT.Fsm;
using Aitex.Core.RT.OperationCenter;
using Aitex.Core.RT.Routine;
using Aitex.Core.RT.SCCore;
using Aitex.Core.Utilities;
using Aitex.Sorter.Common;
using JetEfemLib.Aligners;
using JetEfemLib.LPs;
using JetEfemLib.SignalTowers;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.Event;
using MECF.Framework.Common.SubstrateTrackings;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Efems.Rorzes;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots;
using MECF.Framework.RT.ModuleLibrary.EfemModules;

namespace JetEfemLib.Efems
{
    public class EfemModule : EfemModuleBase
    {
        public enum STATE
        {
            NotInstall,

            Init,

            Idle,

            Homing,

            HomingAll,

            Picking,

            Placing,

            RobotGoto,

            Swapping,

            Mapping,

            Load,
            Unload,

            Error,

            NotConnect,
            Extending,
            Retracting,

            Griping,
            UnGriping,
        }

        public enum MSG
        {
            Home,

            HomeAll,

            Connected,
            Disconnected,

            Map,

            Pick,

            PickAndPlace,
            Place,
            Goto,

            Reset,
            Abort,
            Error,

            ToInit,
            Extend,
            Retract,

            Grip,
            UnGrip,
        }

        public enum EfemType
        {
            FutureEfem2P = 1,
            JetEfem = 2,
            FutureEfem3P = 3,
        }

        public override bool IsReady
        {
            get { return FsmState == (int)STATE.Idle && CheckAllMessageProcessed(); }
        }

        public override bool IsError
        {
            get { return FsmState == (int)STATE.Error; }
        }

        public override bool IsInit
        {
            get { return FsmState == (int)STATE.Init; }
        }

        public bool IsBusy
        {
            get { return !IsInit && !IsError && !IsReady; }
        }

        public event Action<string> OnEnterError;

        public bool IsEfemRobotHomed { get; set; }

        public JetEfemRobot RobotDevice { get; set; }
        public RorzeEfem EfemDevice { get; set; }
        public JetEfemSignalTower SignalTowerDevice { get; set; }

        public JetEfemLoadPort LP1Device { get; set; }
        public JetEfemLoadPort LP2Device { get; set; }
        public JetEfemLoadPort LP3Device { get; set; }
        public FutureBuffer BufADevice { get; set; }
        public FutureBuffer BufBDevice { get; set; }
        public FutureAligner Aligner1Device { get; set; }
        public FutureAligner Aligner2Device { get; set; }

        private EfemHomeRoutine _homeRoutine;
        private EfemHomeAllRoutine _homeAllRoutine;
        private EfemPickRoutine _pickRoutine;
        private EfemPlaceRoutine _placeRoutine;
        private EfemGotoRoutine _gotoRoutine;
        private EfemMapRoutine _mapRoutine;
        private EfemPickAndPlaceRoutine _pickAndPlaceRoutine;

        private EfemExtendRoutine _extendRoutine;
        private EfemRetractRoutine _retractRoutine;

       

        //private EfemPickPMRoutine _pickPMRoutine;
        //private EfemPlacePMRoutine _placePMRoutine;
        //private EfemPickAndPlacePMRoutine _pickAndPlacePMRoutine;

        private bool _isInit;

        public EfemModule(ModuleName module) : base(2)
        {
            Module = module.ToString();
            Name = module.ToString();
            IsOnline = true;
        }

        public override bool Initialize()
        {

            InitDevice();

            InitRoutine();

            InitFsm();

            InitOp();

            InitData();

            return base.Initialize();
        }

        private void InitRoutine()
        {
            _homeRoutine = new EfemHomeRoutine(this);
            _homeAllRoutine = new EfemHomeAllRoutine(this);
            _pickRoutine = new EfemPickRoutine(this);
            _placeRoutine = new EfemPlaceRoutine(this);
            _gotoRoutine = new EfemGotoRoutine(this);
            _mapRoutine = new EfemMapRoutine(this);
            _pickAndPlaceRoutine = new EfemPickAndPlaceRoutine(this);
            _extendRoutine = new EfemExtendRoutine(this);
            _retractRoutine = new EfemRetractRoutine(this);

            //_pickPMRoutine = new EfemPickPMRoutine(this);
            //_placePMRoutine = new EfemPlacePMRoutine(this);
            //_pickAndPlacePMRoutine = new EfemPickAndPlacePMRoutine(this);
        }

        public void InitDevice()
        {
            RobotDevice = DEVICE.GetDevice<JetEfemRobot>($"{ModuleName.EfemRobot}.{ModuleName.EfemRobot}");
            SignalTowerDevice = DEVICE.GetDevice<JetEfemSignalTower>($"{ModuleName.System}.{ModuleName.SignalTower}");

            //for (int i = 1; i <= 3; i++)
            //{
            //    FutureEfemLoadPort lp = DEVICE.GetDevice<FutureEfemLoadPort>($"LP{i}.LP{i}");

            //    //lp.SetRobot(RobotDevice);
            //}

            BufADevice = DEVICE.GetDevice<FutureBuffer>($"{ModuleName.LLA}");
            BufBDevice = DEVICE.GetDevice<FutureBuffer>($"{ModuleName.LLB}");
            //LP1Device = DEVICE.GetDevice<JetEfemLoadPort>($"{ModuleName.LP1}");
            //LP2Device = DEVICE.GetDevice<JetEfemLoadPort>($"{ModuleName.LP2}");
            //LP3Device = DEVICE.GetDevice<FutureEfemLoadPort>($"{ModuleName.LP3}");
            Aligner1Device = DEVICE.GetDevice<FutureAligner>($"{ModuleName.Aligner1}.{ModuleName.Aligner1}");
            Aligner2Device = DEVICE.GetDevice<FutureAligner>($"{ModuleName.Aligner2}.{ModuleName.Aligner2}");
            if (SC.GetValue<int>("EFEM.EfemType") == (int)EfemType.FutureEfem2P)
            {
                EfemDevice = DEVICE.GetDevice<FutureEfem>($"{ModuleName.System}.{ModuleName.EFEM}");
            }
            else if (SC.GetValue<int>("EFEM.EfemType") == (int)EfemType.JetEfem)
            {
                EfemDevice = DEVICE.GetDevice<JetEfem>($"{ModuleName.System}.{ModuleName.EFEM}");
            }
            else if (SC.GetValue<int>("EFEM.EfemType") == (int)EfemType.FutureEfem3P)
            {
                EfemDevice = DEVICE.GetDevice<FutureEfem3P>($"{ModuleName.System}.{ModuleName.EFEM}");
            }
            EfemDevice.Connection.OnConnected += Connection_OnConnected;
            EfemDevice.Connection.OnDisconnected += Connection_OnDisconnected;
            EfemDevice.Connection.OnError += Connection_OnError;

            EfemDevice.OnDeviceAlarmStateChanged += EfemDevice_OnDeviceAlarmStateChanged;
        }

        private void InitData()
        {
            DATA.Subscribe($"{Module}.Status", () => StringFsmStatus);
            DATA.Subscribe($"{Module}.IsOnline", () => IsOnline);
            DATA.Subscribe($"{Module}.IsHomed", () => IsEfemRobotHomed);
            DATA.Subscribe($"{Module}.IsError", () => IsError);

            DATA.Subscribe($"{Module}.IsConnected", () => EfemDevice?.Connection.IsConnected);

            DATA.Subscribe($"{Module}.WaferSize", () => WaferManager.Instance.GetWaferSize(ModuleHelper.Converter(Module), 0).ToString());
            DATA.Subscribe($"{Module}.RobotWaferSize", () =>
            WaferManager.Instance.GetWaferSize(ModuleHelper.Converter("EfemRobot"), 0).ToString());
            DATA.Subscribe($"{Module}.RobotWaferSize1", () =>
            WaferManager.Instance.GetWaferSize(ModuleHelper.Converter("EfemRobot"), 1).ToString());

        }

        private void InitOp()
        {
            OP.Subscribe($"{Name}.Connect", (string cmd, object[] args) =>
            {
                if (EfemDevice.Connection != null && !EfemDevice.Connection.IsConnected)
                {
                    EfemDevice.Connect();
                }
                return true;
            });

            OP.Subscribe($"{Name}.Disconnect", (string cmd, object[] args) =>
            {
                if (EfemDevice.Connection != null && EfemDevice.Connection.IsConnected)
                {
                    EfemDevice.Disconnect();
                }
                return true;
            });

            OP.Subscribe($"{Name}.SetOnline", (string cmd, object[] args) =>
            {
                IsOnline = true;
                return true;
            });

            OP.Subscribe($"{Name}.SetOffline", (string cmd, object[] args) =>
            {
                IsOnline = false;
                return true;
            });

            OP.Subscribe($"{Name}.Home", (string cmd, object[] args) =>
            {
                return CheckToPostMessage((int)MSG.Home);
            });

            OP.Subscribe($"{Name}.HomeAll", (string cmd, object[] args) =>
            {
                return CheckToPostMessage((int)MSG.HomeAll);
            });

            OP.Subscribe($"{Name}.Abort", (string cmd, object[] args) =>
            {
                return CheckToPostMessage((int)MSG.Abort);
            });

            OP.Subscribe($"{Name}.Reset", (string cmd, object[] args) =>
            {
                return CheckToPostMessage((int)MSG.Reset);
            });

            OP.Subscribe($"{Name}.Grip", (string cmd, object[] args) =>
            {
                return CheckToPostMessage((int)MSG.Grip, args);
            });

            OP.Subscribe($"{Name}.UnGrip", (string cmd, object[] args) =>
            {
                return CheckToPostMessage((int)MSG.UnGrip, args);
            });

            OP.Subscribe($"{Name}.Pick", (string cmd, object[] args) =>
            {
                return CheckToPostMessage((int)MSG.Pick, args);
            });

            OP.Subscribe($"{Name}.Place", (string cmd, object[] args) =>
            {
                return CheckToPostMessage((int)MSG.Place, args);
            });

            OP.Subscribe($"{Name}.Extend", (string cmd, object[] args) =>
            {
                return CheckToPostMessage((int)MSG.Extend, args[0], args[1], args[2], args[3]);
            });
            OP.Subscribe($"{Name}.Retract", (string cmd, object[] args) =>
            {
                return CheckToPostMessage((int)MSG.Retract, args[0], args[1], args[2], args[3]);
            });

            OP.Subscribe($"{Name}.Goto", (string cmd, object[] args) =>
            {
                return CheckToPostMessage((int)MSG.Goto, args[0]);
            });

            OP.Subscribe($"{Name}.Map", (string cmd, object[] args) =>
            {
                return CheckToPostMessage((int)MSG.Map, args[0]);
            });

        }

        private void InitFsm()
        {
            EnumLoop<STATE>.ForEach((item) =>
            {
                MapState((int)item, item.ToString());
            });

            EnumLoop<MSG>.ForEach((item) =>
            {
                MapMessage((int)item, item.ToString());
            });
            EnableFsm(50, IsInstalled ? STATE.Init : STATE.NotInstall);

            //Error
            AnyStateTransition(MSG.Error, FsmOnError, STATE.Error);
            Transition(STATE.Error, MSG.Reset, FsmReset, STATE.Idle);
            EnterExitTransition<STATE, FSM_MSG>(STATE.Error, FsmEnterError, FSM_MSG.NONE, FsmExitError);
            AnyStateTransition(FSM_MSG.WARNING, fWarning, FSM_STATE.SAME);
            AnyStateTransition((int)FSM_MSG.ALARM, fAlarm, (int)STATE.Error);


            //connection
            AnyStateTransition(MSG.Disconnected, FsmOnDisconnected, STATE.NotConnect);
            Transition(STATE.NotConnect, MSG.Connected, FsmOnConnected, STATE.Init);
            Transition(STATE.NotConnect, MSG.Reset, FsmResetConnect, STATE.NotConnect);

            //Init
            Transition(STATE.Init, MSG.Home, FsmStartHome, STATE.Homing);
            Transition(STATE.Error, MSG.Home, FsmStartHome, STATE.Homing);
            Transition(STATE.Idle, MSG.Home, FsmStartHome, STATE.Homing);
            Transition(STATE.Homing, FSM_MSG.TIMER, FsmMonitorHomeTask, STATE.Idle);
            Transition(STATE.Homing, MSG.Error, null, STATE.Init);
            Transition(STATE.Homing, MSG.Abort, FsmAbortTask, STATE.Init);

            AnyStateTransition(MSG.ToInit, FsmToInit, STATE.Init);

            //idle
            EnterExitTransition<STATE, FSM_MSG>(STATE.Idle, FsmEnterIdle, FSM_MSG.NONE, FsmExitIdle);

            //robot home all
            Transition(STATE.Init, MSG.HomeAll, FsmStartHomeAll, STATE.HomingAll);
            Transition(STATE.Error, MSG.HomeAll, FsmStartHomeAll, STATE.HomingAll);
            Transition(STATE.Idle, MSG.HomeAll, FsmStartHomeAll, STATE.HomingAll);
            Transition(STATE.HomingAll, FSM_MSG.TIMER, FsmMonitorHomingAllTask, STATE.Idle);
            Transition(STATE.HomingAll, MSG.Error, null, STATE.Init);
            Transition(STATE.HomingAll, MSG.Abort, FsmAbortTask, STATE.Init);

            //robot map
            Transition(STATE.Idle, MSG.Map, FsmStartMap, STATE.Mapping);
            Transition(STATE.Mapping, FSM_MSG.TIMER, FsmMonitorTask, STATE.Idle);
            Transition(STATE.Mapping, MSG.Abort, FsmAbortTask, STATE.Idle);

            //robot grip
            Transition(STATE.Idle, MSG.Grip, FsmStartRobotGrip, STATE.Griping);
            Transition(STATE.Griping, FSM_MSG.TIMER, FsmMonitorTask, STATE.Idle);
            Transition(STATE.Griping, MSG.Abort, FsmAbortTask, STATE.Idle);

            //robot ungrip
            Transition(STATE.Idle, MSG.UnGrip, FsmStartRobotUnGrip, STATE.UnGriping);
            Transition(STATE.UnGriping, FSM_MSG.TIMER, FsmMonitorTask, STATE.Idle);
            Transition(STATE.UnGriping, MSG.Abort, FsmAbortTask, STATE.Idle);

            //robot pick
            Transition(STATE.Idle, MSG.Pick, FsmStartRobotPick, STATE.Picking);
            Transition(STATE.Picking, FSM_MSG.TIMER, FsmMonitorTask, STATE.Idle);
            Transition(STATE.Picking, MSG.Abort, FsmAbortTask, STATE.Idle);

            //robot pickAndplace
            Transition(STATE.Idle, MSG.PickAndPlace, FsmStartPickAndPlace, STATE.Swapping);
            Transition(STATE.Swapping, FSM_MSG.TIMER, FsmMonitorTask, STATE.Idle);
            Transition(STATE.Swapping, MSG.Abort, FsmAbortTask, STATE.Idle);

            //robot place
            Transition(STATE.Idle, MSG.Place, FsmStartRobotPlace, STATE.Placing);
            Transition(STATE.Placing, FSM_MSG.TIMER, FsmMonitorTask, STATE.Idle);
            Transition(STATE.Placing, MSG.Abort, FsmAbortTask, STATE.Idle);

            //robot Extend
            Transition(STATE.Idle, MSG.Extend, FsmStartRobotExtend, STATE.Extending);
            Transition(STATE.Extending, FSM_MSG.TIMER, FsmMonitorTask, STATE.Idle);
            Transition(STATE.Extending, MSG.Abort, FsmAbortTask, STATE.Idle);

            //robot Retract
            Transition(STATE.Idle, MSG.Retract, FsmStartRobotRetract, STATE.Retracting);
            Transition(STATE.Retracting, FSM_MSG.TIMER, FsmMonitorTask, STATE.Idle);
            Transition(STATE.Retracting, MSG.Abort, FsmAbortTask, STATE.Idle);

            //robot goto
            Transition(STATE.Idle, MSG.Goto, FsmStartRobotGoto, STATE.RobotGoto);
            Transition(STATE.RobotGoto, FSM_MSG.TIMER, FsmMonitorTask, STATE.Idle);
            Transition(STATE.RobotGoto, MSG.Abort, FsmAbortTask, STATE.Idle);
        }

        private bool FsmToInit(object[] param)
        {
            return true;
        }

        private bool FsmResetConnect(object[] param)
        {
            //if (!EfemDevice.Connection.IsConnected)
            //{
            //    EfemDevice.Connect();
            //    return false;
            //}

            //if (!EfemDevice.EmoAlarm.IsAcknowledged)
            //{
            //    EfemDevice.ClearError();
            //    return false;
            //}

            return true;
        }

        private bool FsmOnConnected(object[] param)
        {
            //SignalTowerDevice.ResetData();

            //LP1Device.ResetData();
            //LP2Device.ResetData();
            //LP3Device.ResetData();
            //LP4Device.ResetData();

            //EfemDevice.ResetData();

            return true;
        }

        private bool FsmOnDisconnected(object[] param)
        {

            return true;
        }

        private bool FsmExitIdle(object[] param)
        {
            return true;
        }

        private bool FsmEnterIdle(object[] param)
        {
            return true;
        }

        private bool FsmExitError(object[] param)
        {
            return true;
        }

        private bool fWarning(object[] objs)
        {
            //IsWarning = false;
            return true;
        }

        private bool fAlarm(object[] objs)
        {
            ExecuteAbort();

            return true;
        }

        private bool FsmEnterError(object[] param)
        {
            if (OnEnterError != null)
                OnEnterError(Module);
            return true;
        }

        private bool FsmStartHome(object[] param)
        {
            RState ret = StartRoutine(_homeRoutine);
            if (ret == RState.Failed || ret == RState.End)
                return false;

            _isInit = false;
            IsEfemRobotHomed = false;

            return ret == RState.Running;
        }

        private bool FsmStartHomeAll(object[] param)
        {
            RState ret = StartRoutine(_homeAllRoutine);
            if (ret == RState.Failed || ret == RState.End)
                return false;

            _isInit = false;
            IsEfemRobotHomed = false;

            return ret == RState.Running;
        }

        private bool FsmStartRobotGrip(object[] param)
        {
            RobotArmEnum GripBlade = (RobotArmEnum)param[0];
            RobotDevice.Grip(GripBlade);

            return true;
        }

        private bool FsmStartRobotUnGrip(object[] param)
        {
            RobotArmEnum GripBlade = (RobotArmEnum)param[0];
            RobotDevice.Release(GripBlade);

            return true;
        }

        private bool FsmStartRobotPick(object[] param)
        {
            //if (!param[0].ToString().Contains("PM"))
            //{
            if (param.Length == 3)
            {
                _pickRoutine.Init(ModuleHelper.Converter((string)param[0]), (int)param[1], (Hand)param[2]);
            }
            else
            {
                _pickRoutine.Init(ModuleHelper.Converter((string)param[0]), (int)param[1]);
            }

            RState ret = StartRoutine(_pickRoutine);
            if (ret == RState.Failed || ret == RState.End)
                return false;
            return ret == RState.Running;
            //}
            //else;
            //{
            //    if (param.Length == 3)
            //    {
            //        _pickPMRoutine.Init(ModuleHelper.Converter((string)param[0]), (int)param[1], (Hand)param[2]);
            //    }
            //    else
            //    {
            //        _pickPMRoutine.Init(ModuleHelper.Converter((string)param[0]), (int)param[1]);
            //    }

            //    Result ret = StartRoutine(_pickPMRoutine);
            //    if (ret == Result.FAIL || ret == Result.DONE)
            //        return false;
            //    return ret == Result.RUN;

            //}
        }

        private bool FsmStartPickAndPlace(object[] param)
        {
            //if (!param[0].ToString().Contains("PM"))
            //{
            _pickAndPlaceRoutine.Init(ModuleHelper.Converter((string)param[0]), (Hand)param[1], (int)param[2], (Hand)param[3], (int)param[4]);

            RState ret = StartRoutine(_pickAndPlaceRoutine);
            if (ret == RState.Failed || ret == RState.End)
                return false;
            return ret == RState.Running;
            //}
            //else
            //{
            //    _pickAndPlacePMRoutine.Init(ModuleHelper.Converter((string)param[0]), (Hand)param[1], (int)param[2], (Hand)param[3], (int)param[4]);

            //    Result ret = StartRoutine(_pickAndPlacePMRoutine);
            //    if (ret == Result.FAIL || ret == Result.DONE)
            //        return false;
            //    return ret == Result.RUN;
            //}
        }

        private bool FsmStartRobotPlace(object[] param)
        {
            //if (!param[0].ToString().Contains("PM"))
            //{
            if (param.Length == 3)
            {
                _placeRoutine.Init(ModuleHelper.Converter((string)param[0]), (int)param[1], (Hand)param[2]);
            }
            else
            {
                _placeRoutine.Init(ModuleHelper.Converter((string)param[0]), (int)param[1]);
            }

            RState ret = StartRoutine(_placeRoutine);

            if (ret == RState.Failed || ret == RState.End)
                return false;
            return ret == RState.Running;
            //}
            //else
            //{
            //    if (param.Length == 3)
            //    {
            //        _placePMRoutine.Init(ModuleHelper.Converter((string)param[0]), (int)param[1], (Hand)param[2]);
            //    }
            //    else
            //    {
            //        _placePMRoutine.Init(ModuleHelper.Converter((string)param[0]), (int)param[1]);
            //    }

            //    Result ret = StartRoutine(_placePMRoutine);

            //    if (ret == Result.FAIL || ret == Result.DONE)
            //        return false;
            //    return ret == Result.RUN;
            //}

        }
        private bool FsmStartRobotExtend(object[] param)
        {
            _extendRoutine.Init(ModuleHelper.Converter((string)param[0]), (int)param[1], param[2].ToString(), param[3].ToString()); ;

            RState ret = StartRoutine(_extendRoutine);
            if (ret == RState.Failed || ret == RState.End)
                return false;
            return ret == RState.Running;
        }
        private bool FsmStartRobotRetract(object[] param)
        {
            _retractRoutine.Init(ModuleHelper.Converter((string)param[0]), (int)param[1], param[2].ToString(), param[3].ToString());

            RState ret = StartRoutine(_retractRoutine);
            if (ret == RState.Failed || ret == RState.End)
                return false;
            return ret == RState.Running;
        }

        private bool FsmStartRobotGoto(object[] param)
        {
            _gotoRoutine.Init(ModuleHelper.Converter((string)param[0]), (int)param[1], (Hand)param[2]);

            RState ret = StartRoutine(_gotoRoutine);
            if (ret == RState.Failed || ret == RState.End)
                return false;
            return ret == RState.Running;
        }

        private bool FsmStartMap(object[] param)
        {
            _mapRoutine.Init(ModuleHelper.Converter((string)param[0]));

            RState ret = StartRoutine(_mapRoutine);
            if (ret == RState.Failed || ret == RState.End)
                return false;
            return ret == RState.Running;
        }

        private bool FsmAbortTask(object[] param)
        {
            AbortRoutine();
            return true;
        }

        private bool FsmMonitorHomeTask(object[] param)
        {
            RState ret = MonitorRoutine();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                PostMsg(MSG.Error);
                return false;
            }

            if (ret == RState.End)
            {
                _isInit = true;
                IsEfemRobotHomed = true;
                OP.DoOperation($"{Module}.ResetTask");
                return true;
            }

            return false;
        }

        private bool FsmMonitorHomingAllTask(object[] param)
        {
            RState ret = MonitorRoutine();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                PostMsg(MSG.Error);
                return false;
            }

            if (ret == RState.End)
            {
                _isInit = true;
                IsEfemRobotHomed = true;
                OP.DoOperation($"{Module}.ResetTask");
                return true;
            }

            return false;
        }


        private bool FsmMonitorTask(object[] param)
        {
            RState ret = MonitorRoutine();
            if (ret == RState.Failed)
            {
                PostMsg(MSG.Error);
                return false;
            }

            return ret == RState.End;
        }


        private bool FsmOnError(object[] param)
        {
            ExecuteAbort();
            return true;
        }

        private void ExecuteAbort()
        {
            if (FsmState == (int)STATE.Picking)
            {
                _pickRoutine.Abort();
                OP.DoOperation("System.Abort", null);
            }
            else if (FsmState == (int)STATE.Placing)
            {
                _placeRoutine.Abort();
                OP.DoOperation("System.Abort", null);
            }
            else if (FsmState == (int)STATE.RobotGoto)
            {
                _gotoRoutine.Abort();
                OP.DoOperation("System.Abort", null);
            }
            else
                AbortRoutine();
        }

        private bool FsmReset(object[] param)
        {
            //if (!EfemDevice.Connection.IsConnected)
            //{
            //    EfemDevice.Connect();
            //    return false;
            //}

            //if (!EfemDevice.EmoAlarm.IsAcknowledged)
            //{
            //    EfemDevice.ClearError();
            //    return false;
            //}

            if (!_isInit)
            {
                PostMsg(MSG.ToInit);
                return false;
            }

            if (RobotDevice.IsError)
            {
                EV.PostWarningLog(Module, $"Robot in error, home to recover");

                PostMsg(MSG.ToInit);
                return false;
            }

            return true;
        }

        #region Service functions

        public override bool Home(out string reason)
        {
            if (!CheckToPostMessage((int)MSG.Home))
            {
                reason = $"Can not home in {StringFsmStatus} status";
                return false;
            }
            reason = string.Empty;
            return true;
        }


        public override void Reset()
        {
            SignalTowerDevice.Reset();
            if (IsError)
            {
                if (RobotDevice.IsError)
                {
                    RobotDevice.Reset();
                }
                else if (RobotDevice.RobotState != RobotStateEnum.Idle && RobotDevice.RobotState != RobotStateEnum.Init)
                {
                    EV.PostWarningLog(Module, $"Robot in {RobotDevice.RobotState}, need home to reset error");
                    return;
                }

                CheckToPostMessage((int)MSG.Reset);
            }

        }

        private bool CheckDeviceHasError()
        {
            if (RobotDevice.IsError)
                return true;

            return false;
        }

        public override bool Pick(ModuleName target, Hand blade, int targetSlot, out string reason)
        {
            CheckToPostMessage((int)MSG.Pick, target.ToString(), targetSlot, blade);
            reason = string.Empty;
            return true;
        }

        public override bool Place(ModuleName target, Hand blade, int targetSlot, out string reason)
        {
            CheckToPostMessage((int)MSG.Place, target.ToString(), targetSlot, blade);
            reason = string.Empty;
            return true;
        }

        public override bool PickAndPlace(ModuleName pickTarget, Hand pickHand, int pickSlot, ModuleName placeTarget, Hand placeHand,
            int placeSlot, out string reason)
        {
            CheckToPostMessage((int)MSG.PickAndPlace, pickTarget.ToString(), pickHand, pickSlot, placeHand, placeSlot);

            reason = string.Empty;
            return true;
        }

        public override bool Goto(ModuleName target, Hand blade, int targetSlot, out string reason)
        {
            CheckToPostMessage((int)MSG.Goto, target.ToString(), targetSlot, blade);
            reason = string.Empty;
            return true;
        }
        //public override bool GotoRE(ModuleName target, Hand blade, int targetSlot, RobotPostionEnum postiontype, out string reason)
        //{   
        //    if(postiontype.ToString().Contains("Extend"))
        //        CheckToPostMessage((int)MSG.Extend, target.ToString(), targetSlot, blade, postiontype);
        //    else if(postiontype.ToString().Contains("Retract"))
        //        CheckToPostMessage((int)MSG.Retract, target.ToString(), targetSlot, blade, postiontype);
        //    reason = string.Empty;
        //    return true;
        //}
        public override bool Map(ModuleName target, out string reason)
        {
            if (!CheckToPostMessage((int)MSG.Map, target.ToString()))
            {
                reason = $"Can not support map in {StringFsmStatus}";
                return false;
            }
            reason = string.Empty;
            return true;
        }

        private void EfemDevice_OnDeviceAlarmStateChanged(string module, AlarmEventItem alarmItem)
        {
            if (IsInit)
                return;

            if (!alarmItem.IsAcknowledged)
            {
                if (alarmItem.Level == EventLevel.Warning)
                {
                    EV.PostWarningLog(Module, alarmItem.Description);
                }
                else
                {
                    EV.PostAlarmLog(Module, alarmItem.Description);
                }

                //if (alarmItem.EventEnum == EfemDevice.IsMaintain.EventEnum
                //|| alarmItem.EventEnum == EfemDevice.EmoAlarm.EventEnum
                //|| alarmItem.EventEnum == EfemDevice.DoorOpen.EventEnum )
                //{
                //    _isInit = false;
                //    PostMsg(MSG.Error);
                //}
                //else
                if (alarmItem.Level == EventLevel.Alarm)
                {
                    PostMsg(MSG.Error);
                }
            }
            else
            {
                if (IsError)
                    CheckToPostMessage((int)MSG.Reset);
            }
        }

        private void Connection_OnError(string obj)
        {
            PostMsg(MSG.Disconnected);
        }

        private void Connection_OnDisconnected()
        {

            PostMsg(MSG.Disconnected);
        }

        private void Connection_OnConnected()
        {
            PostMsg(MSG.Connected);
        }
        #endregion

    }
}