ZZ-EFEM/Jet/Jet_001_2P_Jet/EfemRT/Routines/CommonRoutine.cs

using System;
using System.Collections.Generic;
using System.Linq;
using Aitex.Core.Common;
using Aitex.Core.RT.Device;
using Aitex.Core.RT.Device.Unit;
using Aitex.Core.RT.Event;
using Aitex.Core.RT.Routine;
using Aitex.Core.RT.SCCore;
using Aitex.Core.Util;
using EFEM.RT.Devices;


using Aitex.Sorter.Common;
using EFEMSC;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.SubstrateTrackings;
using MECF.Framework.Common.Utilities;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.LoadPorts;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.OcrReaders;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robot;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robot.NX100;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots.RobotBase;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.LoadPorts.LoadPortBase;
using Aitex.Core.RT.Log;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.BufferStations;

namespace EFEM.RT.Routines
{
    public class CommonRoutine : SeqenecRoutine
    {
        protected bool bUINotify;

        public string Module { get; set; }
        public string Name { get; set; }
        public string Display { get; set; }


        //protected LoadPort loadportA = null;
        //protected LoadPort loadportB = null;
        //protected RIDReader ridreaderA = null;
        //protected RIDReader ridreaderB = null;

        protected RobotBaseDevice robot = null;
        protected Aligner aligner = null;
        protected OcrReader widreader = null;

        protected IoCoolingBuffer buffer1 = null;
        protected IoCoolingBuffer buffer2 = null;

        protected IoCoolingBuffer aligner1 = null;
        protected IoCoolingBuffer aligner2 = null;
        protected LoadLockDevice ll1 = null;
        protected LoadLockDevice ll2 = null;
        protected IoCoolingBuffer _ioCoolBuffer;
        protected BufferStation bf1 = null;
        protected BufferStation bf2 = null;
        private List<LoadPortBaseDevice> LPs;

        protected readonly bool HaveAligner = DeviceDefineManager.Instance.GetValue<bool>("AlignerInstalled") ?? false;
        protected readonly bool AlignerNeedMoveUpCommand = DeviceDefineManager.Instance.GetValue<bool>("AlignerNeedMoveUpCommand") ?? false;
        protected readonly int LoadPortQuantity = DeviceDefineManager.Instance.GetValue<int>("LoadPortQuantity") ?? 0;
        protected readonly string RobotTypeDefine = DeviceDefineManager.Instance.GetValue("RobotType");
        protected readonly bool HaveMotionAxis = DeviceDefineManager.Instance.GetValue<bool>("MotionAxisInstalled") ?? false;
        protected static readonly bool LoadLockDoorControlByStation = DeviceDefineManager.Instance.GetValue<bool>("LLDoorControlByStation") ?? false;
        protected readonly string LoadPortTypeDefine = DeviceDefineManager.Instance.GetValue("LoadPortType");

        protected int OffsetX;
        protected int OffsetY;
        protected int OffsetZ;

        private DeviceTimer _timerQuery = new DeviceTimer();
        private int _queryPeriod = 500;    //ms

        protected bool NeedSetParameter;

        protected bool IsStopped;
        private int _existInterval = SC.GetValue<int>("Robot.Robot.ExistInterval");
        protected bool NeedRobotGripAndUngrip=SC.GetValue<bool>("System.EnableRobotGripAndUngrip");

        public bool IsEnableIdentifyThickness
        {
            get
            {
                if (SC.ContainsItem("System.IsEnableIdentifyThickness"))
                    return SC.GetValue<bool>("System.IsEnableIdentifyThickness");
                return false;
            }
        }

        public bool IsEnableMultiWaferSize
        {
            get
            {
                if (SC.ContainsItem("System.IsEnableMultiWaferSize"))
                    return SC.GetValue<bool>("System.IsEnableMultiWaferSize");
                return false;
            }
        }
        public string EFemNum
        {
            get
            {
                if (SC.ContainsItem("System.EFEMNUM"))
                    return SC.GetStringValue("System.EFEMNUM");
                return "001";
            }
        }
        public bool IsVacuumMode
        {
            get
            {
                if (SC.ContainsItem("Robot.Robot.IsVacuumMode"))
                    return SC.GetValue<bool>("Robot.Robot.IsVacuumMode");
                return true;
            }
        }
        private DeviceTimer _timerPerf = new DeviceTimer();
        public CommonRoutine()
        {            
            robot = DEVICE.GetDevice<RobotBaseDevice>(DeviceName.Robot);
            aligner = DEVICE.GetDevice<Aligner>(DeviceName.Aligner);
            widreader = DEVICE.GetDevice<OcrReader>(DeviceName.WIDReader);

            buffer1 = DEVICE.GetDevice<IoCoolingBuffer>(DeviceName.CoolingBuffer1);
            buffer2 = DEVICE.GetDevice<IoCoolingBuffer>(DeviceName.CoolingBuffer2);

            aligner1 = DEVICE.GetDevice<IoCoolingBuffer>(DeviceName.Aligner1);
            aligner2 = DEVICE.GetDevice<IoCoolingBuffer>(DeviceName.Aligner2);
            ll1 = DEVICE.GetDevice<LoadLockDevice>(DeviceName.LL1);
            ll2 = DEVICE.GetDevice<LoadLockDevice>(DeviceName.LL2);
            bf1 = DEVICE.GetDevice<BufferStation>(DeviceName.Buffer1);
            bf2 = DEVICE.GetDevice<BufferStation>(DeviceName.Buffer2);
            var lpNames = new List<ModuleName>(Singleton<DeviceManager>.Instance.LpNames);
            LPs = new List<LoadPortBaseDevice>();
            lpNames.ForEach(lp =>
            {    
                if(!SC.GetValue<bool>($"LoadPort.{lp}.Disable")) 
                    LPs.Add(DEVICE.GetDevice<LoadPortBaseDevice>(lp.ToString())); 
            });
        }

        public bool InitCommon()
        {

            return true;
        }

        protected void UpdateSCValue()
        {

        }


        //延时
        public void Delay(int id, string name, double time, Action<string> notify, Action<string> error)
        {
            Tuple<bool, Result> ret = Delay(id, () =>
            {
                notify(name);
                return true;
            }, time * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.RUN)
                {
                    throw (new RoutineBreakException());
                }
            }
        }

        #region waferID


        #endregion

        #region robot
        public void RobotReset(int id, RobotBaseDevice device, string name, int time, Action<string> notify, Action<string> error)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                notify(String.Format("{0} clear error", device.Name));
                string reason = string.Empty;
                device.RobotReset();
                return true;
            }, () =>
            {

                if (!device.IsBusy&& device.RobotState != RobotStateEnum.Error && device.RobotState == RobotStateEnum.Idle)
                { 
                    return true;
                }
                return false;

            }, time * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    error(String.Format("{0} timeout, than {1} seconds", name, time));
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }
        public void RobotServoOnOff(int id, Robot device, bool trueForOnFalseForOff, Action<string> notify, Action<string> error)
        {
            Tuple<bool, Result> ret = Execute(id, () =>
            {
                notify(String.Format("{0} Home", device.Name));
                string reason = string.Empty;
                return device.SetServoOnOff(trueForOnFalseForOff,out reason);
            });

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
            }
        }
        public void RobotInit(int id, Robot device, string name, Action<string> notify, Action<string> error)
        {
            Tuple<bool, Result> ret = Execute(id, () =>
            {
                notify(String.Format("{0} Home", device.Name));
                string reason = string.Empty;
                return device.Init(out reason);
            });

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
            }
        }
        public void RobotHome(int id, RobotBaseDevice device, string name, Action<string> notify, Action<string> error)
        {
            Tuple<bool, Result> ret = Execute(id, () =>
            {
                notify(String.Format("{0} Home", device.Name));
                string reason = string.Empty;
                return device.Home(null);
            });

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
            }
        }

        public void RobotArmHome(int id, RobotBaseDevice device, string name, Action<string> notify, Action<string> error)
        {
            Tuple<bool, Result> ret = Execute(id, () =>
            {
                notify(String.Format("{0} Arm Home", device.Name));
                string reason = string.Empty;
                return device.Home(null);
            });

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
            }
        }


        ///等待 Robot Motion
        public void WaitRobotMotion(int id, RobotBaseDevice device, string name, int time, Action<string> notify, Action<string> error)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                notify("Wait robot finish motion");
                _timerQuery.Start(_queryPeriod);  //开始查询

                return true;
            }, () =>
            {
                if (device.IsReady())
                {
                    _timerQuery.Stop();
                    return true;
                }

                if (query())
                {
                    //    string reason = string.Empty;
                    //     device.QueryState(out reason);      //查询位置
                }
                return false;
            }, time * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    error(String.Format("{0} timeout, than {1} seconds", name, time));
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }

        ///等待
        ///
        public void ReserveRobotToBusy(int id, RobotBaseDevice robot, string name,Action<string> notify, Action<string> error)
        {
            Tuple<bool, Result> ret = Execute(id, () =>
            {
                notify(String.Format("{0} set robot to busy", robot.Name));
                string reason = string.Empty;
                robot.IsBusy = true;
                return true;
            });

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
            }
        }
        public void Wait(int id, string name, double time, Action<string> notify, Action<string> error)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                notify($"Wait {time} seconds");
                _timerQuery.Start(_queryPeriod);  //开始查询

                return true;
            }, () =>
            {
                if (_timerQuery.GetElapseTime() >= time * 1000)
                {
                    _timerQuery.Stop();
                    return true;
                }
                return false;
            });

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }


        public void CoolBufferMoveUP(int id, IoCoolingBuffer device, WaferSize size, int time, Action<string> notify, Action<string> error)
        {
            Tuple<bool, Result> ret = Execute(id, () =>
            {
                notify($"Start {device.Name} Move Up");
                if (device.CheckPinUp()) return true;
                if (!device.Move(size, true, out string reason))
                {
                    Stop(reason);
                    return false;
                }

                return true;
            });

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
            }
        }
        
        public void RobotBusytoSimif(int id, ModuleName moduleName,bool on,int time, Action<string> notify, Action<string> error)
        {
            Tuple<bool, Result> ret = Execute(id, () =>
            {
                notify($"set TrigRBbusytoSIMF of {moduleName} signal {on} ");
                if (on)
                {
                    var value = moduleName == ModuleName.LP1 ? DeviceModel.TrigRBbusytoSIMF1.SetTrigger(true, out _) : DeviceModel.TrigRBbusytoSIMF2.SetTrigger(true, out _);
                    if (!value)
                    {

                        Stop(null);
                        return false;
                    }
                }
                else
                {
                    var value = moduleName == ModuleName.LP1 ? DeviceModel.TrigRBbusytoSIMF1.SetTrigger(false, out _) : DeviceModel.TrigRBbusytoSIMF2.SetTrigger(false, out _);
                    if (!value)
                    {
                        Stop(null);
                        return false;
                    }
                }

                return true;
            });

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
            }
        }
        public void UnGripRobotBlade(int id, Hand blade, RobotBaseDevice device, int time)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Release robot {blade}");
                //IoSensor sensorBlade = blade == Hand.Blade1
                //    ? DeviceModel.SensorRobotFork1WaferOn
                //    : DeviceModel.SensorRobotFork2WaferOn;
                //var iswaferpersence = blade == Hand.Blade1 ? robot.IsWaferPresenceOnBlade1 : robot.IsWaferPresenceOnBlade2;
                //int slot = blade == Hand.Blade1 ? 0 : 1;
                //if (WaferManager.Instance.CheckNoWafer(ModuleName.Robot, slot)) //!sensorBlade.Value &&
                {
                    if (!device.Release((RobotArmEnum)blade))
                    {
                        Stop(null);
                        return false;
                    }
                }
                return true;
            }, () =>
            {
                if (!device.IsBusy)
                {
                    return true;
                }

                //此处如果发生错误，忽略

                return false;
            }, time * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    Stop($"Release robot {blade} timeout, than {time} seconds");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }
        public void GripRobotBlade(int id, Hand blade, RobotBaseDevice device, int time)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Grip robot {blade}");
                //IoSensor sensorBlade = blade == Hand.Blade1
                //    ? DeviceModel.SensorRobotFork1WaferOn
                //    : DeviceModel.SensorRobotFork2WaferOn;
                //var iswaferpersence = blade == Hand.Blade1 ? robot.IsWaferPresenceOnBlade1 : robot.IsWaferPresenceOnBlade2;
                //int slot = blade == Hand.Blade1 ? 0 : 1;
                //if (!iswaferpersence && WaferManager.Instance.CheckNoWafer(ModuleName.Robot, slot)) //!sensorBlade.Value &&
                //{
                    if (!device.Grip((RobotArmEnum)blade))
                    {
                        Stop(null);
                        return false;
                    }
               // }
                return true;
            }, () =>
            {
                if (device.IsReady())
                {
                    return true;
                }

                //此处如果发生错误，忽略

                return false;
            }, time * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    Stop($"Release robot {blade} timeout, than {time} seconds");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }
        public void HomeReleaseRobotBlade(int id, Hand blade, RobotBaseDevice device, int time)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Release robot {blade}");
                //IoSensor sensorBlade = blade == Hand.Blade1
                //    ? DeviceModel.SensorRobotFork1WaferOn
                //    : DeviceModel.SensorRobotFork2WaferOn;
                //var iswaferpersence = blade == Hand.Blade1 ? robot.IsWaferPresenceOnBlade1 : robot.IsWaferPresenceOnBlade2;
                //int slot = blade == Hand.Blade1 ? 0 : 1;
                //if (!iswaferpersence&&WaferManager.Instance.CheckNoWafer(ModuleName.Robot, slot)) //!sensorBlade.Value &&
                //{
                    if (!device.Release((RobotArmEnum)blade))
                    {
                        Stop(null);
                        return false;
                    }
               // }
                return true;
            }, () =>
            {
                if (device.IsReady())
                {
                    return true;
                }

                //此处如果发生错误，忽略

                return false;
            }, time * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    Stop($"Release robot {blade} timeout, than {time} seconds");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }
        public void HomeGripAndUngripRobotBlade(int id, Hand blade, RobotBaseDevice device, int time)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Grip or Ungrip robot {blade}");
              
                int slot = blade == Hand.Blade1 ? 0 : 1;
                if (WaferManager.Instance.CheckHasWafer(ModuleName.Robot, slot))
                {
                    if (!device.Grip((RobotArmEnum)blade))
                    {
                        Stop(null);
                        return false;
                    }
                }
                else
                {
                    if (!device.Release((RobotArmEnum)blade))
                    {
                        Stop(null);
                        return false;
                    }
                }
               
                //Thread.Sleep(1000);
                return true;
            }, () =>
            {
                if (device.IsReady())
                {
                    return true;
                }

                //此处如果发生错误，忽略

                return false;
            }, time * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    Stop($"Grip robot {blade} timeout, than {time} seconds");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }

        
        public void CheckBladeWaferIsExist(int id, RobotBaseDevice device,Hand blade, int time)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Check Blade {blade} Wafer Is Exist");

                try
                {
                    if (!device.IsReady())
                    {
                        LOG.Write($"CheckBladeWaferIsExist:Robot IsReady={device.IsReady()}");
                        throw (new RoutineBreakException());
                    }
                    if (!device.ReadParameter(new object[] { "CheckWaferIsPresence", blade == Hand.Blade1 ? 0 : 1, _existInterval }))
                    {
                        Stop(null);
                        return false;
                    }
                }
                catch (Exception ex)
                {
                    LOG.Write(ex);
                }
                LOG.Write($"Check Blade Wafer Is Exist");
                //Thread.Sleep(1000);
                return true;
            }, () =>
            {
               // LOG.Write("CheckBladeWaferIsExist1: IsReady=false");
                try
               {
                   if (device.IsReady())
                    {
                        LOG.Write("CheckBladeWaferIsExist: IsReady=true");
                        return true;
                    }
                }
                catch (Exception ex)
                {
                    LOG.Write(ex);
                }

                //LOG.Write("CheckBladeWaferIsExist2: IsReady=false");
                //此处如果发生错误，忽略

                return false;
            }, time * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    Stop($"Check robot WaferPresence timeout, than {time} seconds");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }

        public void RobotSignalStatus(int id, RobotBaseDevice device , int time)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Query robot Signal Status");

                if (!device.ReadParameter(new object[] { "SignalStatus"}))
                {
                    Stop(null);
                    return false;
                }
                //Thread.Sleep(1000);
                return true;
            }, () =>
            {
                if (device.IsReady())
                {
                    return true;
                }

                //此处如果发生错误，忽略

                return false;
            }, time * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }
        public void ConfirmRobotBladeWafer(int id, Hand blade)
        {
            Execute(id, () =>
            {
                Notify($"Confirm robot {blade} wafer");

                //IoSensor sensorBlade = blade == Hand.Blade1
                //    ? DeviceModel.SensorRobotFork1WaferOn
                //    : DeviceModel.SensorRobotFork2WaferOn;

                var iswaferpersence = blade == Hand.Blade1 ? robot.IsWaferPresenceOnBlade1 : robot.IsWaferPresenceOnBlade2;
                int slot = blade == Hand.Blade1 ? 0 : 1;

                if (iswaferpersence)
                {
                    if (WaferManager.Instance.CheckNoWafer(ModuleName.Robot, slot))
                    {
                        EV.PostWarningLog(Module, $"Found wafer at robot {blade}");
                        WaferManager.Instance.CreateWafer(ModuleName.Robot, slot, WaferStatus.Normal);
                    }
                }
                else
                {
                    if (WaferManager.Instance.CheckHasWafer(ModuleName.Robot, slot))
                    {
                        EV.PostWarningLog(Module, $"Not found wafer at robot {blade}, please manually confirm again.");
                        WaferManager.Instance.DeleteWafer(ModuleName.Robot, slot);
                    }
                }

                return true;
            });
        }
        public void CoolBufferMoveDown(int id, IoCoolingBuffer device, WaferSize size, int time, Action<string> notify, Action<string> error)
        {
            Tuple<bool, Result> ret = Execute(id, () =>
            {
                notify($"Start {device.Name} Move Down");
                if (device.CheckPinDown()) return true;
                if (!device.Move(size, false, out string reason))
                {
                    Stop(reason);
                    return false;
                }

                return true;
            });

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
            }
        }

        public void WaitCoolBufferMoveUp(int id, IoCoolingBuffer device, string name, int time, Action<string> notify,
           Action<string> error)
        {
            var ret = ExecuteAndWait(id, () =>
            {
                notify(name);

                return true;
            }, () =>
            {
                if (device.Error)
                {
                    Stop("Can not move up, device error");
                    return null;
                }
                return device.CheckMovedUp() && !device.Busy ;
            }, time * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                    throw new RoutineFaildException();

                if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    error($"{name} timeout, than {time} seconds");
                    throw new RoutineFaildException();
                }

                bool isMovedUp = device.CheckMovedUp() && !device.Busy && !device.Error;

                if (!isMovedUp)
                {
                    throw new RoutineBreakException();
                }
            }
        }

        public void WaitCoolBufferMoveDown(int id, IoCoolingBuffer device, string name, int time, Action<string> notify,
           Action<string> error)
        {
            var ret = ExecuteAndWait(id, () =>
            {
                notify(name);
                _timerQuery.Start(_queryPeriod); //开始查询

                return true;
            }, () =>
            {
                if (device.Error)
                {
                    Stop("Can not move down, device error");
                    return null;
                }
                return device.CheckMovedDown() && !device.Busy;
            }, time * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL) throw new RoutineFaildException();

                if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    error($"{name} timeout, than {time} seconds");
                    throw new RoutineFaildException();
                }

                bool isMovedDown = device.CheckMovedDown() && !device.Busy && !device.Error;

                if (!isMovedDown)
                {
                    throw new RoutineBreakException();
                }
            }
        }

        public void SetWaferSize(int id, string name, int cmd, ModuleName station, int slot, int time, Action<string> notify, Action<string> error)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
                {
                    WaferSize size = WaferSize.WS0;

                    if (ModuleHelper.IsLoadPort(station))
                    {
 
                    }
                    else
                    {
                        size = WaferManager.Instance.GetWaferSize(station, slot);
                    }

                    notify(String.Format($"Set WaferSize parameter {cmd} {size.ToString()}"));
                    string reason = string.Empty;

                    //robot.SetWaferSize(cmd, size, out reason);
                    return true;
                }, () =>
                {
                    if (robot.RobotState == RobotStateEnum.Idle)
                    {
                        return true;
                    }
                    return false;

                }, time * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    error(String.Format("Set WaferSize timeout, than {0} seconds", time));
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }

        /// <summary>
        /// 目前只比对0114 parameter
        /// </summary>
        /// <param name="id"></param>
        /// <param name="name"></param>
        /// <param name="parameterNo"></param>
        /// <param name="parameterType"></param>
        /// <param name="waferSize"></param>
        /// <param name="time"></param>
        /// <param name="notify"></param>
        /// <param name="error"></param>
        /// <exception cref="RoutineFaildException"></exception>
        /// <exception cref="RoutineBreakException"></exception>
        protected void RobotGoto(int id, string name, ModuleName chamber, int slot, Hand blade, RobotPostionEnum postype,
            Action<string> notify, Action<string> error)
        {
            var reason = string.Empty;
            var ret = Execute(id, () =>
            {
                notify(string.Format("pick from {0}", chamber.ToString()));

                var obj = new object[] { (RobotArmEnum)(int)blade, chamber.ToString(), slot, postype };
                return robot.GoTo(obj);// (RobotArmEnum)(int)blade, chamber.ToString(), slot);
            });

            if (ret.Item1)
                if (ret.Item2 == Result.FAIL)
                {
                    error(reason);
                    throw new RoutineFaildException();
                }
        }
        protected void CheckTriggerSignalOn(int id,string name,ModuleName module,int time)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify(String.Format("Check Trigger Signal"));
                string reason = string.Empty; 
                return true;
            }, () =>
            { 
                if (SC.GetValue<bool>("System.IsSimulatorMode")) return true;
                if (module.Equals(ModuleName.LL1))
                {
                    if (DeviceModel.SensorPMASystemInterlock.Value && DeviceModel.TrigSafetytoPMA.Value)
                    {
                        return true;
                    }
                }
                if (module.Equals(ModuleName.LL2))
                {
                    if (DeviceModel.SensorPMBSystemInterlock.Value && DeviceModel.TrigSafetytoPMB.Value)
                    {
                        return true;
                    }
                }
                return false;

            }, time * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    Stop(String.Format("Trigger Signal Change timeout, than {0} seconds", time));
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }
        protected void PickWafer(int id, string name, ModuleName chamber, int slot, Hand blade, int x, int y, int z,
            Action<string> notify, Action<string> error)
        {
            var reason = string.Empty;
            var ret = Execute(id, () =>
            {
                notify(string.Format("pick from {0}", chamber.ToString()));

                if (x == 0 && y == 0 && z == 0)
                    return robot.Pick((RobotArmEnum)(int)blade,chamber.ToString(), slot);

                return robot.PickEx((RobotArmEnum)(int)blade, chamber.ToString(), slot, x, y, z);
            });

            if (ret.Item1)
                if (ret.Item2 == Result.FAIL)
                {
                    error(reason);
                    throw new RoutineFaildException();
                }
        }

        protected void PlaceWafer(int id, string name, ModuleName chamber, int slot, Hand blade, int x, int y, int z,
            Action<string> notify, Action<string> error)
        {
            var ret = Execute(id, () =>
            {
                notify(string.Format("Place wafer to {0}", chamber.ToString()));
                var reason = string.Empty;

                if (x == 0 && y == 0 && z == 0)
                    return robot.Place((RobotArmEnum)(int)blade, chamber.ToString(), slot);
                return robot.PlaceEx((RobotArmEnum)(int)blade, chamber.ToString(), slot, x, y, z);
            });

            if (ret.Item1)
                if (ret.Item2 == Result.FAIL)
                    throw new RoutineFaildException();
        }
        #endregion


        protected bool CheckRobotMotionInterlock(ModuleName chamber, int slot, out string reason)   //Pick/Place/Swap
        {
            reason = string.Empty;


            if (robot.RobotState == RobotStateEnum.Error)

            {
                reason = "robot is error.";
                return false;
            }

            //if (!robot.IsIdle)
            //{
            //    reason = string.Format("robot is moving.");
            //    return false;
            //}

            if (!CheckRobotReady())
            {
                reason = string.Format("robot isn't Ready.");
                return false;
            }

            if (chamber == ModuleName.Aligner)
            {
                if (aligner.Moving)
                {
                    reason = string.Format("aligner is moving.");
                    return false;
                }

                if (!CheckAlignerReady())
                {
                    reason = string.Format("aligner isn't Ready.");
                    return false;
                }

                /*
                if (aligner.Busy)
                {
                    reason = string.Format("{0} is busy.", chamber.ToString());
                    return false;
                }
                */
                return true;
            }
            else if (ModuleHelper.IsLoadPort(chamber))
            {
                LoadPortBaseDevice loadport = DEVICE.GetDevice<LoadPortBaseDevice>(chamber.ToString());

                if (!loadport.IsEnableTransferWafer(out _))
                {
                    reason = string.Format("{0} isn't Ready.", chamber.ToString());
                    return false;
                }
                if (loadport.MapError)
                {
                    reason = string.Format("{0} Map has crossed error.", chamber.ToString());
                    return false;
                }
                if (chamber.Equals(ModuleName.LP1))
                {
                    if (SC.GetValue<int>($"LoadPort.LP1.CstType") == 0)
                    {
                        if (!DeviceModel.SensorSMIF1PODOPEN.Value)
                        {
                            reason = string.Format("{0} SMIF1PODOPEN signal is Off.", chamber.ToString());
                            return false;
                        }
                        if (!DeviceModel.SensorSMIF1PODPRESENT.Value)
                        {
                            reason = string.Format("{0} SensorSMIF1PODPRESENT signal is Off,Foup is presence.", chamber.ToString());
                            return false;
                        }
                        if (!DeviceModel.SensorSMIF1READY.Value)
                        {
                            reason = string.Format("{0} SensorSMIF1READY signal is Off.", chamber.ToString());
                            return false;
                        }
                    }
                    else if (SC.GetValue<int>($"LoadPort.LP1.CstType") == 1)
                    {
                        if(loadport.CassetteState!=LoadportCassetteState.Normal)// (DeviceModel.Sensor4InchCstPresence.Value&& DeviceModel.Sensor6InchCstPresence.Value&& DeviceModel.Sensor8InchCstPresence.Value)
                        {
                            reason = string.Format("{0} Cst Presence signal is Off,Foup is  not presence.", chamber.ToString());
                            return false;
                        }

                    }
                    if (slot + 1 > loadport.ValidSlotsNumber)
                    {
                        reason = string.Format("{0} ValidSlotsNumber less than {1}.", chamber.ToString(), slot + 1);
                        return false;
                    }
                }
                else if (chamber.Equals(ModuleName.LP2))
                {
                    if (SC.GetValue<int>($"LoadPort.LP2.CstType") == 0)
                    {
                        if (!DeviceModel.SensorSMIF2PODOPEN.Value)
                        {
                            reason = string.Format("{0} SMIF2PODOPEN signal is Off.", chamber.ToString());
                            return false;
                        }
                        if (!DeviceModel.SensorSMIF2PODPRESENT.Value)
                        {
                            reason = string.Format("{0} SensorSMIF2PODPRESENT signal is Off,Foup is not presence .", chamber.ToString());
                            return false;
                        }
                        if (!DeviceModel.SensorSMIF2READY.Value)
                        {
                            reason = string.Format("{0} SensorSMIF2READY signal is Off.", chamber.ToString());
                            return false;
                        }
                    }
                    else if (SC.GetValue<int>($"LoadPort.LP2.CstType") == 1)
                    {
                        if (loadport.CassetteState != LoadportCassetteState.Normal)// (DeviceModel.Sensor4InchCstPresence.Value&& DeviceModel.Sensor6InchCstPresence.Value&& DeviceModel.Sensor8InchCstPresence.Value)
                        {
                            reason = string.Format("{0} Cst Presence signal is Off,Foup is  not presence.", chamber.ToString());
                            return false;
                        }

                    }
                    if (slot + 1 > loadport.ValidSlotsNumber)
                    {
                        reason = string.Format("{0} ValidSlotsNumber less than {1}.", chamber.ToString(), slot + 1);
                        return false;
                    }
                }
                return true;
            }

            if (ModuleHelper.IsLoadLock(chamber))
            {
                var ll = DEVICE.GetDevice<LoadLockDevice>(chamber.ToString());
                if (!ll.IsEnableTransferWafer(out reason))
                    return false;
                return true;
            }
            if (ModuleHelper.IsBuffer(chamber))
            {
                var bufferStation = DEVICE.GetDevice<BufferStation>(chamber.ToString());
                if (!bufferStation.IsEnableTransferWafer(out reason))
                    return false;
                return true;
            }
            if (ModuleHelper.IsCoolingBuffer(chamber)|| ModuleHelper.IsAligner(chamber))
            {
                IoCoolingBuffer buffer = DEVICE.GetDevice<IoCoolingBuffer>(chamber.ToString());

                if (buffer.Busy)
                {
                    reason = "buffer is not idle";

                    return false;
                }

                if (!buffer.CheckPinUp() )
                {
                    reason = "buffer pin not up position";

                    return false;
                }
                return true;

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

        protected bool CheckRobotReady()
        {
            return robot.IsReady();
        }

        protected bool CheckRobotMotionInterlock(out string reason)
        {
            reason = string.Empty;


            if (robot.RobotState != RobotStateEnum.Idle )
            {
                reason = string.Format("robot is not idle.");
                return false;
            }
          
            if (!CheckRobotReadySensor())
            {
                reason = string.Format("robot isn't ready.");
                return false;
            }

            return true;
        }
        public void QueryLoadportState(int id, LoadPort device)
        {
            Tuple<bool, Result> ret = Execute(id, () =>
            {
                Notify(String.Format("Query {0} state", device.Name));

                return device.IsIdle;
            });

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    Stop("not idle");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }

        public void QueryAlignerState(int id, Aligner device)
        {
            Tuple<bool, Result> ret = Execute(id, () =>
            {
                Notify(String.Format("Query {0} state", device.Name));

                return device.State == DeviceState.Idle;
            });

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    Stop("not idle");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }

        public void QueryCoolBufferState(int id, IoCoolingBuffer device)
        {
            var ret = Execute(id, () =>
            {
                Notify(string.Format("Query {0} state", device.Name));
                return true;
            });

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    Stop("Station wafer status error");
                    throw new RoutineFaildException();
                }

                throw new RoutineBreakException();
            }
        }

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

        public void QueryLoadportState(int id, string deviceName, int time)
        {

            LoadPortBaseDevice device = DEVICE.GetDevice<LoadPortBaseDevice>(deviceName);
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify(String.Format("{0} query state", deviceName));
                string reason = string.Empty;

                device.QueryState(out reason);
                return true;
            }, () =>
            {
                if (device.IsReady())
                {
                    return true;
                }
                return false;

            }, time * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    Stop(String.Format("{0} query timeout, than {1} seconds", device.Name, time));
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }

        public void IsLoadLockReadyToTransfer(string deviceName)
        {
            var device = DEVICE.GetDevice<LoadLockDevice>(deviceName);
            if (!device.IsEnableTransferWafer(out var reason))
            {
                EV.PostMessage(deviceName, EventEnum.DefaultWarning, reason);
                throw new RoutineBreakException();
            }
        }

        public void LoadLockOpenAtmDoor(int id, LoadLockDevice device, string name, int time, Action<string> notify, Action<string> error)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                notify(name);
                _timerQuery.Start(_queryPeriod);  //开始查询
                return LoadLockDevice.LoadLockAtmDoorState == LoadLockDoorState.Opened || LoadLockDevice.OpenAtmDoor(out string reason);
            }, () =>
            {
                if (LoadLockDevice.LoadLockAtmDoorState == LoadLockDoorState.Opened)
                    return true;
                return false;
            }, time * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    error(String.Format("{0} timeout, than {1} seconds", name, time));
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }

        public void LoadLockCloseAtmDoor(int id, LoadLockDevice device, string name, int time, Action<string> notify, Action<string> error)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                notify(name);
                return LoadLockDevice.LoadLockAtmDoorState == LoadLockDoorState.Closed || LoadLockDevice.CloseAtmDoor(out string reason);
            }, () =>
            {
                if (LoadLockDevice.LoadLockAtmDoorState == LoadLockDoorState.Closed)
                    return true;
                return false;
            }, time * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    error(String.Format("{0} timeout, than {1} seconds", name, time));
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }

        public void LoadLockOpenVtmDoor(int id, LoadLockDevice device, string name, int time, Action<string> notify, Action<string> error)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                notify(name);
                _timerQuery.Start(_queryPeriod);  //开始查询
                return LoadLockDevice.LoadLockVtmDoorState == LoadLockDoorState.Opened || LoadLockDevice.OpenVtmDoor(out string reason);
            }, () =>
            {
                if (LoadLockDevice.LoadLockVtmDoorState == LoadLockDoorState.Opened)
                    return true;
                return false;
            }, time * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    error($"{name} timeout, than {time} seconds");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }

        public void LoadLockCloseVtmDoor(int id, LoadLockDevice device, string name, int time, Action<string> notify, Action<string> error)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                notify(name);
                return LoadLockDevice.LoadLockVtmDoorState == LoadLockDoorState.Closed || LoadLockDevice.CloseVtmDoor(out string reason);
            }, () =>
            {
                if (LoadLockDevice.LoadLockVtmDoorState == LoadLockDoorState.Closed)
                    return true;
                return false;
            }, time * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    error(String.Format("{0} timeout, than {1} seconds", name, time));
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }

        protected void Default()
        {
            string reason = String.Empty;
        }

        protected virtual void Warning(string message)
        {
            EV.PostMessage(Module, EventEnum.DefaultWarning, message);
        }

        ///等待 Aligner Motion


        public void WaitAlignerMotion(int id, Aligner device, string name, int time, Action<string> notify, Action<string> error)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
                {
                    notify(name);
                    _timerQuery.Start(_queryPeriod);  //开始查询

                    return true;
                }, () =>
                {
                    // if (device.Moving == false && CheckAlignerReady())
                    if (device.Busy == false && CheckAlignerReady())
                    {
                        _timerQuery.Stop();
                        return true;
                    }

                    if (query())
                    {
                        //      string reason = string.Empty;
                        //       device.QueryState(out reason);      //查询位置
                    }
                    return false;
                }, time * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    error(String.Format("{0} timeout, than {1} seconds", name, time));
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }
        protected virtual void Notify(string message)
        {
            EV.PostInfoLog(Module, $"{Name}, {message}");
        }


        protected virtual void Stop(string failReason)
        {
            EV.PostAlarmLog(Module, $"{Name}, {failReason}");

        }

        public void Abort()
        {
        }

        private bool query()
        {
            if (_timerQuery.IsTimeout())
            {
                _timerQuery.Start(_queryPeriod);
                return true;
            }

            return false;
        }

        protected bool CheckSeneorHasWafer(ModuleName chamber, int slot)
        {
            if (!SC.GetValue<bool>(SorterCommon.ScPathName.System_IsSimulatorMode))
            {
                if (chamber == ModuleName.Robot)
                {
                    if (slot == 0)
                        return robot.IsWaferPresenceOnBlade1;//!DeviceModel.SensorRBlowerArmhavewafer.Value;

                    return robot.IsWaferPresenceOnBlade2;
                }
                //if (chamber == ModuleName.Aligner)
                //{
                //    return !DeviceModel.SensorPreAlignerWaferOn.Value;
                //}
            }

            return WaferManager.Instance.CheckHasWafer(chamber, slot);
        }

        protected bool CheckSensorNoWafer(ModuleName chamber, int slot)
        {
            if (!SC.GetValue<bool>(SorterCommon.ScPathName.System_IsSimulatorMode))
            {
                if (chamber == ModuleName.Robot)
                {
                    if (slot == 0)
                        return !robot.IsWaferPresenceOnBlade1; //DeviceModel.SensorRBlowerArmhavewafer.Value;

                    return !robot.IsWaferPresenceOnBlade2; //DeviceModel.SensorRBupperArmhavewafer.Value;
                }
                //if (chamber == ModuleName.Aligner)
                //{
                //    return DeviceModel.SensorPreAlignerWaferOn.Value;
                //}
            }

            return WaferManager.Instance.CheckNoWafer(chamber, slot);
        }


        protected bool CheckRobotReadySensor()
        {
            return (robot.RobotState == RobotStateEnum.Idle);
        }


        protected bool CheckAlignerReady()
        {
            return true;
        }
        ///等待 Loadport
        public void LoadportReset(int id, LoadPortBaseDevice device, string name, int time, Action<string> notify, Action<string> error)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
                {
                    notify(String.Format("{0} clear error", device.Name));
                    string reason = string.Empty;
                    return device.LoadportReset(out reason);
                }, () =>
                {
                    if (device.IsReady())
                    {
                        return true;
                    }
                    return false;

                }, time * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    error(String.Format("{0} timeout, than {1} seconds", name, time));
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }

        public void LoadportInit(int id, LoadPortBaseDevice device, string name, Action<string> notify, Action<string> error)
        {
            Tuple<bool, Result> ret = Execute(id, () =>
            {
                notify(String.Format("{0} Home", device.Name));
                string reason = string.Empty;
                return device.Home(out reason);
            });

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
            }
        }

        protected bool CheckLoadportMotionInterlock(ModuleName chamber, out string reason)
        {
            reason = string.Empty;


            if (robot.RobotState != RobotStateEnum.Idle)
            {
                reason = string.Format("robot is not idle.");
                return false;
            }          

            if (!CheckRobotReady())
            {
                reason = string.Format("robot isn't ready.");
                return false;
            }

            return true;
        }


        ///等待 Loadport
        public void WaitLoadportMotion(int id, LoadPortBaseDevice device, string name, int time, Action<string> notify, Action<string> error)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
                {
                    notify(name);
                    string msg = string.Empty;
                    //device.QueryState(out msg);      //查询状

                    _timerQuery.Start(_queryPeriod);  //开始查询
                    return true;
                }, () =>
                {
                   
                    if (device.CurrentState == LoadPortStateEnum.Error)
                    {
                        return null;
                    }
                    if (device.IsReady())
                    {
                        _timerQuery.Stop();
                        return true;
                    }
                    return false;

                }, time * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    error(String.Format("{0} timeout, than {1} seconds", name, time));
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }

        protected void LoadportAllReset(int id, int time)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify(String.Format("clear error for all loadports"));
                string reason = string.Empty;
                LPs.ForEach(lp => lp.ClearError(out reason));

                return true;
            }, () =>
            {
                if (LPs.Any(lp => lp.IsBusy))
                {
                    return false;
                }

                return true;

            }, time * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    Stop(String.Format("LoadportAllReset timeout, than {0} seconds", time));
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }

        protected void LoadportAllInit(int id, int time)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify(String.Format("All loadports Homing"));
                string reason = string.Empty;
                LPs.ForEach(lp =>lp.Home(out reason));
                _timerQuery.Start(_queryPeriod);  //开始查询
                return true;
            }, () =>
            {
                

                if (LPs.Any(lp => lp.CurrentState == LoadPortStateEnum.Error))
                {
                    return null;
                }

                if (LPs.Any(lp => !lp.IsHomed)) //!lp.IsReady() &&
                {
                    return false;
                }
                LPs.ForEach(m=>LOG.Write($"{m.Name}:{m.IsHomed }"));
                return true;

            }, time * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    Stop(String.Format("LoadportAllInit timeout, than {0} seconds", time));
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }

        protected void PostMessage(int id, string v)
        {
            Tuple<bool, Result> ret = Execute(id, () =>
            {
                EV.PostMessage(ModuleName.System.ToString(), EventEnum.GeneralInfo, v);
                
                return true;
            });

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
            }


            

        }
        public void TimeDelay(int id, int timeMs)
        {
            Tuple<bool, Result> ret = Delay(id, () =>
            {
                Notify($"Delay {timeMs} milliseconds");
                return true;
            }, timeMs);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.RUN)
                {
                    throw (new RoutineBreakException());
                }
            }
        }
    }
}