Mars200/FrameworkLocal/RTEquipmentLibrary/HardwareUnits/Robots/SUNWAY/SPaceTRanRobot.cs

using Aitex.Core.Common;
using Aitex.Core.RT.DataCenter;
using Aitex.Core.RT.Device;
using Aitex.Core.RT.Device.Unit;
using Aitex.Core.RT.Event;
using Aitex.Core.RT.Log;
using Aitex.Core.RT.OperationCenter;
using Aitex.Core.RT.SCCore;
using Aitex.Core.Util;
using MECF.Framework.Common.CommonData;
using MECF.Framework.Common.Communications;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.SubstrateTrackings;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.LoadPorts.LoadPortBase;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robot;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots.RobotBase;
using System;
using System.Collections.Generic;
using System.Globalization;
using System.Text;
using System.Threading;
using MECF.Framework.Common.CommonData;
using EventType = Aitex.Core.RT.Event.EventType;
using Aitex.Sorter.Common;
using System.Text.RegularExpressions;
using System.Linq;

namespace MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots.SUNWAY
{
    public class SPaceTRanRobot : RobotBaseDevice, IConnection
    {
        private bool isSimulatorMode;
        private string _scRoot;
        public TokenGenerator SeqnoGenerator { get; private set; }
        public RROpModeEnum CurrentOpMode { get; set; }
        public bool IsOrgshCompleted { get; set; }
        public bool IsCmdProcessing { get; set; }
        public RROpStatusEnum CurrentOpStatus { get; set; }
        public int CurrentOperationSpeed { get; set; } //0:Normal, 1-K Set Speed
        public string CurrentIdCodeForErrorController { get; set; }

        public bool _diEms { get; set; }
        public bool _diTempStop { get; set; }
        public bool _diVacuumPressure { get; set; }
        public bool _diAirSourcePressure { get; set; }
        public bool _diZaxisFan { get; set; }
        public bool _diUpperArmFan { get; set; }
        public bool _diLowerArmFan { get; set; }
        public bool _diUpperFinger1ArmWaferExistence1 { get; set; }
        public bool _diUpperFinger1ArmWaferExistence2 { get; set; }
        public bool _diLowerArmWaferExistence1 { get; set; }
        public bool _diLowerArmWaferExistence2 { get; set; }
        public bool _diTPEms { get; set; }
        public bool _diDeadmanSwitch { get; set; }
        public bool _diModeKey { get; set; }


        public bool IsEnableSeqNo { get; private set; }
        public bool IsEnableCheckSum { get; private set; }
        public int CurrentSeqNo { get; set; }
        public RobotArmEnum chcekingArm { get; set; }

        private int _existInterval = SC.GetValue<int>("TM.TMRobot.ExistInterval");

        public string PortName;

        private string _address;
        private bool _enableLog;
        private SPaceTRanRobotConnection _connection;
        private R_TRIG _trigError = new R_TRIG();

        private R_TRIG _trigCommunicationError = new R_TRIG();
        private R_TRIG _trigRetryConnect = new R_TRIG();
        public string Address => _address;
        private PeriodicJob _thread;
        private object _locker = new object();
        private LinkedList<HandlerBase> _lstHandlers = new LinkedList<HandlerBase>();
        private IoSensor _diRobotReady = null;           //0=OK，1=Busy
        private IoSensor _diRobotBlade1WaferOn = null;   //0为下手臂无片，1为有片
        private IoSensor _diRobotBlade2WaferOn = null;   //0为上手臂无片，1为有片
        private IoSensor _diRobotError = null; //Normal ON
        private IoSensor _diTPinUse = null;
        Dictionary<RobotArmEnum, Tuple<IoSensor, string>> _armSensors = null;

        private IoTrigger _doRobotHold = null; // Normal ON
        public ModuleName CurrentInteractiveModule { get; private set; }
        public bool IsConnected => throw new NotImplementedException();

        public bool IsGrippedBlade1 { get; private set; }
        public bool IsGrippedBlade2 { get; private set; }
        public bool IsPermittedInterlock1 { get; private set; }
        public bool IsPermittedInterlock2 { get; private set; }
        public bool IsPermittedInterlock3 { get; private set; }
        public bool IsPermittedInterlock4 { get; private set; }
        public bool IsPermittedInterlock5 { get; private set; }
        public bool IsPermittedInterlock6 { get; private set; }
        public bool IsPermittedInterlock7 { get; private set; }
        public bool IsPermittedInterlock8 { get; private set; }

        public float CurrentThetaPosition { get; private set; }
        public float CurrentExtensionPosition { get; private set; }
        public float CurrentArm1Position { get; private set; }
        public float CurrentArm2Position { get; private set; }
        public float CurrentZPosition { get; private set; }

        public float CommandThetaPosition { get; private set; }
        public float CommandExtensionPosition { get; private set; }
        public float CommandArm1Position { get; private set; }
        public float CommandArm2Position { get; private set; }
        public float CommandZPosition { get; private set; }
        public int SpeedLevel { get; private set; }
        public int SpeedLevelSetting { get; private set; }

        public string ReadMemorySpec { get; private set; }
        public string ReadTransferStation { get; private set; }
        public int ReadSlotNumber { get; private set; }
        public string ReadArmPosture { get; private set; }
        public RobotArmEnum ReadBladeNo { get; private set; }
        public float ReadThetaPosition { get; private set; }
        public float ReadExtensionPosition { get; private set; }
        public float ReadArm1Position { get; private set; }
        public float ReadArm2Position { get; private set; }
        public float ReadZPosition { get; private set; }
        public Dictionary<string, string> ReadStationItemValues { get; private set; } = new Dictionary<string, string>();
        public Dictionary<string, string> ReadStationItemContents { get; private set; } = new Dictionary<string, string>();

        public string ReadParameterType { get; private set; }
        public string ReadParameterNo { get; private set; }
        public string ReadParameterValue { get; private set; }

        public bool IsManipulatorBatteryLow { get; private set; }
        public bool IsCommandExecutionReady { get; private set; }
        public bool IsServoON { get; private set; }
        public bool IsErrorOccurred { get; private set; }
        public bool IsControllerBatteryLow { get; private set; }

        public bool IsCheckInterlockWaferPresenceOnBlade1 { get; private set; }
        public bool IsCheckInterlockWaferPresenceOnBlade2 { get; private set; }

        public bool IsCheckInterlockPAOp { get; private set; }
        public bool IsCheckInterlockPAWaferStatus { get; private set; }
        public bool IsCheckInterlockPAWaferStatusByCCD { get; private set; }

        public string RobotSystemVersion { get; private set; }
        public string RobotSoftwareVersion { get; private set; }

        public string ReadMappingTransferStation { get; private set; }
        public int ReadMappingSlotNumbers { get; private set; }
        public string ReadSlotMap { get; private set; }
        public int RobotBodyNumber { get; private set; }

        public string TMGripStateBlade1
        {
            get;
            set;
        }

        public string TMGripStateBlade2
        {
            get;
            set;
        }

        public Dictionary<string, float> ReadMappingCalibrationResult { get; private set; }

        protected override void SubscribeWaferLocation()
        {
            WaferManager.Instance.SubscribeLocation(Name, 4);
        }


        public SPaceTRanRobot(string module, string name, string scRoot, IoSensor[] dis, IoTrigger[] dos) : base(module, name)
        {
            Module = module;
            Name = name;

            isSimulatorMode = SC.ContainsItem("System.IsSimulatorMode") ? SC.GetValue<bool>("System.IsSimulatorMode") : false;
            _scRoot = scRoot;
            _address = SC.GetStringValue($"{_scRoot}.{Name}.Address");
            _enableLog = SC.GetValue<bool>($"{_scRoot}.{Name}.EnableLogMessage");
            RobotBodyNumber = SC.GetValue<int>($"{_scRoot}.{Name}.BodyNumber");

            SpeedLevelSetting = SC.GetValue<int>($"{_scRoot}.{Name}.SpeedLevel");
            _connection = new SPaceTRanRobotConnection(this, _address);
            _connection.EnableLog(_enableLog);

            if (dis != null && dis.Length >= 5)
            {
                _diRobotReady = dis[0];
                _diRobotBlade1WaferOn = dis[1];
                _diRobotBlade2WaferOn = dis[2];
                _diRobotError = dis[3];
                _diTPinUse = dis[4];
                _diRobotError.OnSignalChanged += _diRobotError_OnSignalChanged;
                _diTPinUse.OnSignalChanged += _diTPinUse_OnSignalChanged;
            }
            if (dos != null && dos.Length >= 1)
            {
                _doRobotHold = dos[0];
            }

            ConnectionManager.Instance.Subscribe($"{Name}", _connection);
            _thread = new PeriodicJob(1, OnTimer, $"{_scRoot}.{Name} MonitorHandler", true);

            RegisterSpecialData();
        }

        public void NotifyAlarmByErrorCode(string errorcode)
        {
            EV.Notify($"{Name}Error{errorcode}");
        }

        private void _diTPinUse_OnSignalChanged(IoSensor arg1, bool arg2)
        {
            SetMaintenanceMode(!arg1.Value);
        }

        private void _diRobotError_OnSignalChanged(IoSensor arg1, bool arg2)
        {
            if (arg1.Value == false)
            {
                OnError("RobotError");
            }
        }

        private void RegisterSpecialData()
        {
            DATA.Subscribe($"{Module}.{Name}.CurrentArm1Position", () => CurrentArm1Position);
            DATA.Subscribe($"{Module}.{Name}.CurrentArm2Position", () => CurrentArm2Position);
            DATA.Subscribe($"{Module}.{Name}.CurrentExtensionPosition", () => CurrentExtensionPosition);
            DATA.Subscribe($"{Module}.{Name}.CurrentThetaPosition", () => CurrentThetaPosition);
            DATA.Subscribe($"{Module}.{Name}.CurrentZPosition", () => CurrentZPosition);

            DATA.Subscribe($"{Module}.{Name}.IsManipulatorBatteryLow", () => IsManipulatorBatteryLow);
            DATA.Subscribe($"{Module}.{Name}.IsCommandExecutionReady", () => IsCommandExecutionReady);
            DATA.Subscribe($"{Module}.{Name}.IsServoON", () => IsServoON);
            DATA.Subscribe($"{Module}.{Name}.IsErrorOccurred", () => IsErrorOccurred);
            DATA.Subscribe($"{Module}.{Name}.IsControllerBatteryLow", () => IsControllerBatteryLow);
            DATA.Subscribe($"{Module}.{Name}.IsWaferPresenceOnBlade1", () => IsWaferPresenceOnBlade1);
            DATA.Subscribe($"{Module}.{Name}.IsWaferPresenceOnBlade2", () => IsWaferPresenceOnBlade2);

            DATA.Subscribe($"{Module}.{Name}.ErrorCode", () => ErrorCode);
            DATA.Subscribe($"{Module}.{Name}.IsGrippedBlade1", () => IsGrippedBlade1);
            DATA.Subscribe($"{Module}.{Name}.IsGrippedBlade2", () => IsGrippedBlade2);
            DATA.Subscribe($"{Module}.{Name}.IsPermittedInterlock1", () => IsPermittedInterlock1);
            DATA.Subscribe($"{Module}.{Name}.IsPermittedInterlock2", () => IsPermittedInterlock2);
            DATA.Subscribe($"{Module}.{Name}.IsPermittedInterlock3", () => IsPermittedInterlock3);
            DATA.Subscribe($"{Module}.{Name}.IsPermittedInterlock4", () => IsPermittedInterlock4);
            DATA.Subscribe($"{Module}.{Name}.IsPermittedInterlock5", () => IsPermittedInterlock5);
            DATA.Subscribe($"{Module}.{Name}.IsPermittedInterlock6", () => IsPermittedInterlock6);
            DATA.Subscribe($"{Module}.{Name}.IsPermittedInterlock7", () => IsPermittedInterlock7);
            DATA.Subscribe($"{Module}.{Name}.IsPermittedInterlock8", () => IsPermittedInterlock8);

            DATA.Subscribe($"{Module}.{Name}.RobotSpeed", () => SpeedLevelSetting.ToString());
            DATA.Subscribe($"{Name}.RobotSpeed", () =>
            {

                return SpeedLevelSetting.ToString();
            });

            DATA.Subscribe($"{Name}.GripStateBlade1", () => TMGripStateBlade1);
            DATA.Subscribe($"{Name}.GripStateBlade2", () => TMGripStateBlade2);

            OP.Subscribe("SetSpeed", InvokeSetSpeed);

        }

        private bool OnTimer()
        {
            try
            {

                if (!_connection.IsConnected || _connection.IsCommunicationError)
                {
                    lock (_locker)
                    {
                        _lstHandlers.Clear();
                    }
                    _trigRetryConnect.CLK = !_connection.IsConnected;
                    if (_trigRetryConnect.Q && !_connection.Connect())
                    {
                        EV.PostAlarmLog(Module, $"Can not connect with {_connection.Address}, {Module}.{Name}");
                    }
                    return true;
                }

                HandlerBase handler = null;
                lock (_locker)
                {
                    if (!_connection.IsBusy)
                    {
                        if (_lstHandlers.Count > 0)
                        {
                            handler = _lstHandlers.First.Value;
                            ExecuteHandler(handler);
                            _lstHandlers.RemoveFirst();
                        }
                    }
                    else
                    {
                        _connection.MonitorTimeout();
                        _trigCommunicationError.CLK = _connection.IsCommunicationError;
                        if (_trigCommunicationError.Q)
                        {
                            _lstHandlers.Clear();
                            OnError($"{Module}.{Name} communication error, {_connection.LastCommunicationError}");
                        }
                    }
                }
            }
            catch (Exception ex)
            {
                LOG.Write(ex);
            }
            return true;
        }


        public bool Connect()
        {
            return _connection.Connect();
        }

        public bool Disconnect()
        {
            return _connection.Disconnect();
        }

        public override bool IsReady()
        {
            //if (_diRobotReady!=null && !_diRobotReady.Value) 
            //    return false;
            if (_diRobotError != null && !_diRobotError.Value)
                return false;
            if (_diTPinUse != null && !_diTPinUse.Value)
                return false;
            return RobotState == RobotStateEnum.Idle && !IsBusy;
        }

        private readonly Regex _rex_check_load = new Regex(@"LOAD\s+(A|B)\s+PAN\s+(L|R)\s+(OFF|ON)\s*");
        public bool ParseReadData(string command, string rdata)
        {
            try
            {
                if (_rex_check_load.IsMatch(rdata))
                {
                    return ParseWaferIsPresence(command, rdata);
                }
                return true;
            }
            catch (Exception ex)
            {
                LOG.Write(ex);
                return true;
            }
        }

        Dictionary<string, List<Tuple<string, int>>> _handToFork = new Dictionary<string, List<Tuple<string, int>>>()
        {
            { "A",new List<Tuple<string, int>>(){ { new Tuple<string, int>("R", (int)ArmBlade.ArmAFork0) },{ new Tuple<string, int>("L", (int)ArmBlade.ArmAFork2) } } },
            { "B",new List<Tuple<string, int>>(){ { new Tuple<string, int>("R", (int)ArmBlade.ArmBFork1) },{ new Tuple<string, int>("L", (int)ArmBlade.ArmBFork3) } } },
        };
        private bool ParseWaferIsPresence(string command, string rdata)
        {
            try
            {
                var results = _rex_check_load.Match(rdata);
                var arm = results.Groups[1].Value;
                var pan = command.Split(' ')[5];
                var waferStatus = results.Groups[3].Value;
                if (_handToFork.ContainsKey(arm))
                {
                    var fork = _handToFork[arm].FirstOrDefault(p=>p.Item1 == pan);
                    if(fork != null)
                    {
                        if (WaferManager.Instance.CheckHasWafer(ModuleName.TMRobot, fork.Item2))
                        {
                            WaferManager.Instance.DeleteWafer(ModuleName.TMRobot, fork.Item2);
                        }
                        if (waferStatus == "ON" || waferStatus =="?")
                        {
                            if (!WaferManager.Instance.CheckHasWafer(ModuleName.TMRobot, fork.Item2))
                            {
                                WaferManager.Instance.CreateWafer(ModuleName.TMRobot, fork.Item2, waferStatus == "ON" ? WaferStatus.Normal : WaferStatus.Unknown);
                            }
                        }
                    }
                }
                return true;
            }
            catch (Exception ex)
            {
                LOG.Write(ex);
                return true;
            }
        }

        protected override bool fClear(object[] param)
        {
            lock (_locker)
            {
                _lstHandlers.Clear();
                _connection.ForceClear();
                _lstHandlers.AddLast(new SPaceTRanRobotSetHandler(this, "RESET"));
            }
            return true;
        }

        protected override bool fStartReadData(object[] param)
        {
            if (param.Length < 1) return false;
            string readcommand = param[0].ToString();
            switch (readcommand)
            {
                case "CurrentStatus":
                    break;
                case "CheckWaferIsPresence":
                    {
                        lock (_locker)
                        {
                            IsBusy = true;
                            RobotArmEnum arm = chcekingArm = (RobotArmEnum)param[1];
                            int interval = Convert.ToInt16(param[2]);
                            switch (arm)
                            {
                                case RobotArmEnum.Lower:
                                    _lstHandlers.AddLast(new SPaceTRanRobotReadHandler(this, "RQ LOAD ARM A PAN L"));
                                    _lstHandlers.AddLast(new SPaceTRanRobotReadHandler(this, "RQ LOAD ARM A PAN R"));
                                    break;
                                case RobotArmEnum.Upper:
                                    _lstHandlers.AddLast(new SPaceTRanRobotReadHandler(this, "RQ LOAD ARM B PAN L"));
                                    _lstHandlers.AddLast(new SPaceTRanRobotReadHandler(this, "RQ LOAD ARM B PAN R"));
                                    break;
                            }
                        }
                    }
                    break;
                default:
                    break;
            }
            return true;
        }

        private void ExecuteHandler(HandlerBase handler)
        {
            _connection.Execute(handler);

        }

        protected override bool fStartSetParameters(object[] param)
        {
            try
            {
                string strParameter;
                string setcommand = param[0].ToString();
                _setParameter = setcommand;
                switch (setcommand)
                {
                    case "MotionSpeed":   // SSPD Set the motion speed

                        break;
                    case "TransferSpeedLevel":    //SSLV Select the transfer speed level
                        string sslvlevel = param[1].ToString();

                        int nSpeed = 100;
                        nSpeed = Convert.ToInt32(sslvlevel);

                        if (nSpeed > 100 || 0 > nSpeed)
                        {
                            EV.PostAlarmLog(Name, $"Set {setcommand} with invalid parameter:" + nSpeed);
                            return false;
                        }
                        lock (_locker)
                        {
                            _lstHandlers.AddLast(new SPaceTRanRobotSetHandler(this, "SET ACTION SPEEDS ", $"{nSpeed}"));
                        }
                        if (SC.ContainsItem($"{_scRoot}.{Name}.SpeedLevel"))
                        {
                            SC.SetItemValue($"{_scRoot}.{Name}.SpeedLevel", Convert.ToInt32(sslvlevel));
                            SpeedLevelSetting = SC.GetValue<int>($"{_scRoot}.{Name}.SpeedLevel");
                        }
                        break;

                    case "WaferSize":
                        _setSizeArm = (RobotArmEnum)param[1];
                        _setSize = (WaferSize)param[2];
                        break;
                }
            }
            catch (Exception)
            {
                string reason = "";
                if (param != null)
                {
                    foreach (var para in param)
                    {
                        reason += para.ToString() + ",";
                    }
                }
                EV.PostAlarmLog(Name, "Set command parameter invalid:" + reason);
                return false;
            }
            return true;
        }
        private string _setParameter;
        private RobotArmEnum _setSizeArm;
        private WaferSize _setSize;

        protected override bool fMonitorSetParamter(object[] param)
        {
            IsBusy = false;
            switch (_setParameter)
            {
                case "WaferSize":
                    if (_setSizeArm == RobotArmEnum.Lower || _setSizeArm == RobotArmEnum.Blade1)
                        WaferManager.Instance.UpdateWaferSize(RobotModuleName, 0, _setSize);
                    if (_setSizeArm == RobotArmEnum.Upper || _setSizeArm == RobotArmEnum.Upper)
                        WaferManager.Instance.UpdateWaferSize(RobotModuleName, 1, _setSize);
                    if (_setSizeArm == RobotArmEnum.Both)
                    {
                        WaferManager.Instance.UpdateWaferSize(RobotModuleName, 0, _setSize);
                        WaferManager.Instance.UpdateWaferSize(RobotModuleName, 1, _setSize);
                    }
                    Size = _setSize;
                    break;
            }



            return true;
        }
        protected override bool fStartTransferWafer(object[] param)
        {
            return false;
        }
        protected override bool fStartUnGrip(object[] param)
        {
            lock (_locker)
            {
                RobotArmEnum arm = (RobotArmEnum)param[0];
                var fork = (string)param[1];
                switch (arm)
                {
                    case RobotArmEnum.Lower:
                        _lstHandlers.AddLast(new SPaceTRanRobotMotionHandler(this, "VAC", $" OFF ARM A{ForkMapToRightOrLeft(fork)}"));
                        break;
                    case RobotArmEnum.Upper:
                        _lstHandlers.AddLast(new SPaceTRanRobotMotionHandler(this, "VAC", $" OFF ARM B{ForkMapToRightOrLeft(fork)}"));
                        break;
                    case RobotArmEnum.Both:
                        _lstHandlers.AddLast(new SPaceTRanRobotMotionHandler(this, "VAC", $" OFF ARM A{ForkMapToRightOrLeft(fork)}"));
                        _lstHandlers.AddLast(new SPaceTRanRobotMotionHandler(this, "VAC", $" OFF ARM B{ForkMapToRightOrLeft(fork)}"));
                        break;

                }
            }

            RobotArmEnum armenum = (RobotArmEnum)param[0];
            switch (armenum)
            {
                case RobotArmEnum.Lower:
                    TMGripStateBlade1 = "OFF";
                    break;
                case RobotArmEnum.Upper:
                    TMGripStateBlade2 = "OFF";
                    break;
                case RobotArmEnum.Both:
                    TMGripStateBlade1 = "OFF";
                    TMGripStateBlade2 = "OFF";
                    break;

            }

            return true;
        }

        private string ForkMapToRightOrLeft(string fork)
        {
            //{ Hand.Blade1,new Tuple<string, string, string>("A", "l", "r") },
            //{ Hand.Blade2,new Tuple<string, string, string>("B", "L", "R") },
            if (fork == "A" || fork == "B") return "";
            return $" PAN {fork.ToUpper()}";
        }

        protected override bool fStartGrip(object[] param)
        {
            lock (_locker)
            {
                RobotArmEnum arm = (RobotArmEnum)param[0];
                var fork = (string)param[1];
                switch (arm)
                {
                    case RobotArmEnum.Lower:
                        _lstHandlers.AddLast(new SPaceTRanRobotMotionHandler(this, "VAC", $" ON ARM A{ForkMapToRightOrLeft(fork)}"));
                        break;
                    case RobotArmEnum.Upper:
                        _lstHandlers.AddLast(new SPaceTRanRobotMotionHandler(this, "VAC", $" ON ARM B{ForkMapToRightOrLeft(fork)}"));
                        break;
                    case RobotArmEnum.Both:
                        _lstHandlers.AddLast(new SPaceTRanRobotMotionHandler(this, "VAC", $" ON ARM A{ForkMapToRightOrLeft(fork)}"));
                        _lstHandlers.AddLast(new SPaceTRanRobotMotionHandler(this, "VAC", $" ON ARM B{ForkMapToRightOrLeft(fork)}"));
                        break;

                }
            }

            RobotArmEnum armenum = (RobotArmEnum)param[0];
            switch (armenum)
            {
                case RobotArmEnum.Lower:
                    TMGripStateBlade1 = "ON";
                    break;
                case RobotArmEnum.Upper:
                    TMGripStateBlade2 = "ON";
                    break;
                case RobotArmEnum.Both:
                    TMGripStateBlade1 = "ON";
                    TMGripStateBlade2 = "ON";
                    break;

            }
            return true;
        }

        protected override bool fStartInit(object[] param)
        {
            SpeedLevelSetting = SC.GetValue<int>($"{_scRoot}.{Name}.SpeedLevel");

            int nSpeed = SC.GetValue<int>($"{_scRoot}.{Name}.SpeedLevel"); ;

            if (nSpeed > 100 || 0 > nSpeed)
            {
                EV.PostAlarmLog(Name, $"Set Speed with invalid parameter:" + nSpeed);
                return false;
            }

            lock (_locker)
            {
                if (_doRobotHold != null)
                {
                    _doRobotHold.SetTrigger(true, out _);
                }
                _lstHandlers.AddLast(new SPaceTRanRobotMotionHandler(this, "HOME ALL"));
                _lstHandlers.AddLast(new SPaceTRanRobotSetHandler(this, $"SET ACTION SPEEDS {nSpeed}"));


            }
            return true;
        }

        protected override bool fStartHome(object[] param)
        {
            SpeedLevelSetting = SC.GetValue<int>($"{_scRoot}.{Name}.SpeedLevel");

            int nSpeed = SC.GetValue<int>($"{_scRoot}.{Name}.SpeedLevel"); ;

            if (nSpeed > 100 || 0 > nSpeed)
            {
                EV.PostAlarmLog(Name, $"Set Speed with invalid parameter:" + nSpeed);
                return false;
            }
            lock (_locker)
            {
                if (_doRobotHold != null)
                {
                    _doRobotHold.SetTrigger(true, out _);
                    Thread.Sleep(100);
                }
                _lstHandlers.AddLast(new SPaceTRanRobotMotionHandler(this, "HOME ALL"));
                _lstHandlers.AddLast(new SPaceTRanRobotSetHandler(this, $"SET ACTION SPEEDS {nSpeed}"));
            }
            return true;
        }

        protected override bool fStartGoTo(object[] param)
        {
            try
            {
                RobotArmEnum arm = (RobotArmEnum)param[0];
                ModuleName module = (ModuleName)Enum.Parse(typeof(ModuleName), param[1].ToString());
                if (!CheckArm(arm)) return false;
                var slots = (int[])param[2];
                int slot = slots[0];
                int trsSlot = GetTargetSlotByModule(module, slots);
                string trsStation = GetWaferSizeIndex(module, slot, module, slot);
                if (string.IsNullOrWhiteSpace(trsStation))
                {
                    EV.PostAlarmLog("Robot", "Invalid Parameter.");
                    return false;
                }
                var nArm = (arm == RobotArmEnum.Lower) ? "A" : (arm == RobotArmEnum.Upper) ? "B" : "";
                if (string.IsNullOrWhiteSpace(nArm))
                {
                    EV.PostAlarmLog(Module, $"Start go to error：[{nArm}] Invalid Parameter.");
                    return false;
                }
                string strpara = $" N {trsStation} R RE Z DN SLOT {trsSlot} ARM {nArm}";
                lock (_locker)
                {
                    if (_lstHandlers.Count == 0 && !_connection.IsBusy)
                    {
                        ExecuteHandler(new SPaceTRanRobotMotionHandler(this, "GOTO", strpara));
                    }
                    else
                    {
                        _lstHandlers.AddLast(new SPaceTRanRobotMotionHandler(this, "GOTO", strpara));
                    }
                }
                return true;
            }
            catch (Exception ex)
            {
                LOG.Write(ex);
                return false;
            }
        }

        protected override bool fGoToComplete(object[] param)
        {
            try
            {
                RobotArmEnum arm = (RobotArmEnum)CurrentParamter[0];
                ModuleName module = (ModuleName)Enum.Parse(typeof(ModuleName), CurrentParamter[1].ToString());
                if (!CheckArm(arm)) return false;
                var slots = (int[])CurrentParamter[2];
                SetRobotMoveInfo(arm, module, slots[0], RobotAction.Moving);
            }
            catch (Exception ex)
            {
                LOG.Write(ex);
                return false;
            }

            return base.fGoToComplete(param);
        }

        protected override bool fStop(object[] param)
        {
            lock (_locker)
            {
                if (_doRobotHold != null)
                    _doRobotHold.SetTrigger(false, out _);
                _lstHandlers.Clear();
                _connection.ForceClear();
                ExecuteHandler(new SPaceTRanRobotMotionHandler(this, "HALT"));
            }
            return true; ;
        }

        protected override bool fStartMove(object[] param)
        {
            return false;
        }
        
        protected override bool fStartMapWafer(object[] param)
        {
            return false;
        }

        protected override bool fStartSwapWafer(object[] param)
        {
            return false;
        }

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

        protected override bool fStartPlaceWafer(object[] param)
        {
            try
            {
                RobotArmEnum arm = (RobotArmEnum)param[0];
                if (!CheckArm(arm)) return false;
                ModuleName module = (ModuleName)Enum.Parse(typeof(ModuleName), param[1].ToString());
                var slots = (int[])param[2];
                int slot = slots[0];
                var pan = (Pan)CurrentParamter[3];

                int trsSlot = GetTargetSlotByModule(module, slots);
                string trsStation = GetWaferSizeIndex(module, slot, module, slot);
                if (string.IsNullOrWhiteSpace(trsStation))
                {
                    EV.PostAlarmLog("Robot", "Invalid Parameter.");
                    return false;
                }
                if (pan == Pan.None)
                {
                    EV.PostAlarmLog("Robot", "Place error:Pan Invalid Parameter");
                    return false;
                }
                ExectueCommand("PLACE", GetParam(arm, pan, trsSlot, trsStation));
                SetRobotMoveInfo(arm, module, slot, RobotAction.Placing);
                return true;
            }
            catch (Exception ex)
            {
                LOG.Write(ex);
                return false;
            }
        }

        protected override bool fPlaceComplete(object[] param)
        {
            RobotArmEnum arm = (RobotArmEnum)CurrentParamter[0];
            if (arm == RobotArmEnum.Both)
            {
                EV.PostAlarmLog("Robot", "Invalid Parameter.");
                return false;
            }
            ModuleName sourcemodule;
            if (!Enum.TryParse(CurrentParamter[1].ToString(), out sourcemodule)) return false;

            var sourceSlots = (int[])CurrentParamter[2];

            var pan = (Pan)CurrentParamter[3];
            if (sourceSlots == null || pan == Pan.None)
            {
                EV.PostAlarmLog("Robot", "Place error:Invalid Parameter");
                return false;
            }
            Array.Sort(sourceSlots);
            var handSlots = Converter.MapBladeAndPanToSlot((int)CurrentParamter[0], pan);
            for (int i = 0; i < handSlots.Length; i++)
            {
                WaferManager.Instance.WaferMoved(RobotModuleName, handSlots[i], sourcemodule, sourceSlots[i]);
            }
            SetRobotMoveInfo(arm, ModuleName.System, 0, RobotAction.Moving);
            return true;
        }

        protected override bool fStartPickWafer(object[] param)
        {
            try
            {
                RobotArmEnum arm = (RobotArmEnum)param[0];
                if (!CheckArm(arm)) return false;
                ModuleName module = (ModuleName)Enum.Parse(typeof(ModuleName), param[1].ToString());
                var slots = (int[])param[2];
                int slot = slots[0];
                var pan = (Pan)CurrentParamter[3];

                int trsSlot = GetTargetSlotByModule(module, slots);
                string trsStation = GetWaferSizeIndex(module, slot, module, slot);
                if (string.IsNullOrWhiteSpace(trsStation))
                {
                    EV.PostAlarmLog("Robot", "Invalid Parameter.");
                    return false;
                }
                if (pan == Pan.None)
                {
                    EV.PostAlarmLog("Robot", "Pick error:Pan Invalid Parameter");
                    return false;
                }
                ExectueCommand("PICK", GetParam(arm, pan, trsSlot, trsStation));
                SetRobotMoveInfo(arm, module, slot, RobotAction.Picking);
                return true;
            }
            catch (Exception ex)
            {
                LOG.Write(ex);
                return false;
            }
        }

        protected override bool fPickComplete(object[] param)
        {
            RobotArmEnum arm = (RobotArmEnum)CurrentParamter[0];
            if (arm == RobotArmEnum.Both)
            {
                EV.PostAlarmLog("Robot", "Invalid Parameter.");
                return false;
            }
            ModuleName module = (ModuleName)Enum.Parse(typeof(ModuleName), CurrentParamter[1].ToString());
            ModuleName sourcemodule;
            if (!Enum.TryParse(CurrentParamter[1].ToString(), out sourcemodule)) return false;

            if (arm == RobotArmEnum.Both)
            {
                EV.PostAlarmLog("Robot", "Invalid Parameter.");
                return false;
            }
            var sourceSlots = (int[])CurrentParamter[2];
            var pan = (Pan)CurrentParamter[3];
            if (sourceSlots == null || pan == Pan.None)
            {
                EV.PostAlarmLog("Robot", "Pick error:Invalid Parameter");
                return false;
            }
            Array.Sort(sourceSlots);
            var handSlots = Converter.MapBladeAndPanToSlot((int)CurrentParamter[0], pan);
            for (int i = 0; i < handSlots.Length; i++)
            {
                WaferManager.Instance.WaferMoved(sourcemodule, sourceSlots[i], RobotModuleName, handSlots[i]);
            }
            SetRobotMoveInfo(arm, ModuleName.System, 0, RobotAction.Moving);
            return true;
        }

        protected override bool fResetToReady(object[] param)
        {
            if (_doRobotHold != null)
                _doRobotHold.SetTrigger(true, out _);
            return true;
        }

        public override void Reset()
        {
            CheckToPostMessage(RobotMsg.Reset);
        }

        protected override bool fReset(object[] param)
        {
            if (!_connection.IsConnected)
            {
                _address = SC.GetStringValue($"{_scRoot}.{Name}.Address");
                _enableLog = SC.GetValue<bool>($"{_scRoot}.{Name}.EnableLogMessage");
                _connection = new SPaceTRanRobotConnection(this, _address);
                _connection.EnableLog(_enableLog);
                _connection.Connect();
            }
            lock (_locker)
            {
                if (_doRobotHold != null)
                    _doRobotHold.SetTrigger(true, out _);
                _lstHandlers.Clear();
                _connection.ForceClear();
                _lstHandlers.AddLast(new SPaceTRanRobotSetHandler(this, "RESET"));
                _lstHandlers.AddLast(new SPaceTRanRobotSetHandler(this, "SET SERVOS ON"));
            }
            return true;
        }

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

        protected override bool fStartExtendForPick(object[] param)
        {
            try
            {
                RobotArmEnum arm = (RobotArmEnum)param[0];
                if (!CheckArm(arm)) return false;
                ModuleName module = (ModuleName)Enum.Parse(typeof(ModuleName), param[1].ToString());
                var slots = (int[])param[2];
                int slot = slots[0];
                var pan = (Pan)CurrentParamter[3];
                int trsSlot = GetTargetSlotByModule(module, slots);
                string trsStation = GetWaferSizeIndex(module, slot, module, slot);
                if (string.IsNullOrWhiteSpace(trsStation))
                {
                    EV.PostAlarmLog("Robot", "Invalid Parameter.");
                    return false;
                }
                if (pan == Pan.None)
                {
                    EV.PostAlarmLog("Robot", "ExtendForPick error:Pan Invalid Parameter");
                    return false;
                }
                ExecuteComand(arm, pan, trsSlot, trsStation, "PICK", "ENRT");
                SetRobotMoveInfo(arm, module, slot, RobotAction.Extending);
                return true;
            }
            catch (Exception ex)
            {
                LOG.Write(ex);
                return false;
            }
        }

        protected override bool fStartExtendForPlace(object[] param)
        {
            try
            {
                RobotArmEnum arm = (RobotArmEnum)param[0];
                if (!CheckArm(arm)) return false;
                ModuleName module = (ModuleName)Enum.Parse(typeof(ModuleName), param[1].ToString());
                var slots = (int[])param[2];
                int slot = slots[0];
                var pan = (Pan)CurrentParamter[3];
                int trsSlot = GetTargetSlotByModule(module, slots);
                string trsStation = GetWaferSizeIndex(RobotModuleName, (int)arm, module, slot);
                if (string.IsNullOrWhiteSpace(trsStation))
                {
                    EV.PostAlarmLog("Robot", "Invalid Parameter.");
                    return false;
                }
                ExecuteComand(arm, pan, trsSlot, trsStation, "PLACE", "ENRT");
                SetRobotMoveInfo(arm, module, slot, RobotAction.Extending);
                return true;
            }
            catch (Exception ex)
            {
                LOG.Write(ex);
                return false;
            }
        }

        protected override bool fStartRetractFromPick(object[] param)
        {
            try
            {
                RobotArmEnum arm = (RobotArmEnum)param[0];
                if (arm == RobotArmEnum.Both)
                {
                    EV.PostAlarmLog("Robot", "Invalid Parameter.");
                    return false;
                }
                ModuleName module = (ModuleName)Enum.Parse(typeof(ModuleName), param[1].ToString());
                var slots = (int[])param[2];
                int slot = slots[0];            

                ModuleName sourcemodule;
                if (!Enum.TryParse(CurrentParamter[1].ToString(), out sourcemodule)) return false;

                var pan = (Pan)CurrentParamter[3];
                if (slots == null || pan == Pan.None)
                {
                    EV.PostAlarmLog("Robot", "Retract Pick error:Invalid Parameter");
                    return false;
                }
                Array.Sort(slots);
                var handSlots = Converter.MapBladeAndPanToSlot((int)CurrentParamter[0], pan);
                for (int i = 0; i < handSlots.Length; i++)
                {
                    WaferManager.Instance.WaferMoved(sourcemodule, slots[i], RobotModuleName, handSlots[i]);
                }
                int trsSlot = GetTargetSlotByModule(module, slots);
                string trsStation = GetWaferSizeIndex(module, slot, module, slot);
                if (string.IsNullOrWhiteSpace(trsStation))
                {
                    EV.PostAlarmLog("Robot", "Invalid Parameter.");
                    return false;
                }
                ExecuteComand(arm, pan, trsSlot, trsStation, "PICK", "STRT");
                SetRobotMoveInfo(arm, module, slot, RobotAction.Retracting);
                return true;
            }
            catch (Exception ex)
            {
                LOG.Write(ex);
                return false;
            }
        }

        protected override bool fStartRetractFromPlace(object[] param)
        {
            try
            {
                RobotArmEnum arm = (RobotArmEnum)param[0];
                if (arm == RobotArmEnum.Both)
                {
                    EV.PostAlarmLog("Robot", "Invalid Parameter.");
                    return false;
                }
                ModuleName module = (ModuleName)Enum.Parse(typeof(ModuleName), param[1].ToString());
                var slots = (int[])param[2];
                int slot = slots[0];
                ModuleName sourcemodule;
                if (!Enum.TryParse(CurrentParamter[1].ToString(), out sourcemodule)) return false;

                var pan = (Pan)CurrentParamter[3];
                if (slots == null || pan == Pan.None)
                {
                    EV.PostAlarmLog("Robot", "Retract Place error:Invalid Parameter");
                    return false;
                }
                Array.Sort(slots);
                var handSlots = Converter.MapBladeAndPanToSlot((int)CurrentParamter[0], pan);
                for (int i = 0; i < handSlots.Length; i++)
                {
                    WaferManager.Instance.WaferMoved(RobotModuleName, handSlots[i], sourcemodule, slots[i]);
                }
                int trsSlot = GetTargetSlotByModule(module, slots);
                string trsStation = GetWaferSizeIndex(module, slot, module, slot);
                if (string.IsNullOrWhiteSpace(trsStation))
                {
                    EV.PostAlarmLog("Robot", "Invalid Parameter.");
                    return false;
                }
                ExecuteComand(arm, pan, trsSlot, trsStation, "PLACE", "STRT");
                SetRobotMoveInfo(arm, module, slot, RobotAction.Retracting);
                return true;
            }
            catch (Exception ex)
            {
                LOG.Write(ex);
                return false;
            }
        }

        protected override bool fRetractFromPickComplete(object[] param)
        {
            Blade1Target = ModuleName.System;
            Blade2Target = ModuleName.System;
            CmdTarget = ModuleName.System;
            MoveInfo = new RobotMoveInfo()
            {
                Action = RobotAction.Moving,
                ArmTarget = CmdRobotArm == RobotArmEnum.Lower ? RobotArm.ArmA : RobotArm.ArmB,
                BladeTarget = BuildBladeTarget(),
            };
            return true;
        }

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

            return true;
        }

        public override RobotArmWaferStateEnum GetWaferState(RobotArmEnum arm)
        {
            if (arm == RobotArmEnum.Lower)
            {
                if (_diRobotBlade1WaferOn != null)
                {
                    if (_diRobotBlade1WaferOn.Value) return RobotArmWaferStateEnum.Absent;
                    else return RobotArmWaferStateEnum.Present;
                }

                return IsWaferPresenceOnBlade1 ? RobotArmWaferStateEnum.Present : RobotArmWaferStateEnum.Absent;
            }
            if (arm == RobotArmEnum.Upper)
            {
                if (_diRobotBlade2WaferOn != null)
                {
                    if (_diRobotBlade2WaferOn.Value) return RobotArmWaferStateEnum.Absent;
                    else return RobotArmWaferStateEnum.Present;
                }
                return IsWaferPresenceOnBlade2 ? RobotArmWaferStateEnum.Present : RobotArmWaferStateEnum.Absent;
            }
            if (arm == RobotArmEnum.Both)
            {
                if (_diRobotBlade1WaferOn != null && _diRobotBlade2WaferOn != null)
                {
                    if (_diRobotBlade2WaferOn.Value && _diRobotBlade1WaferOn.Value)
                        return RobotArmWaferStateEnum.Absent;
                    else if (!_diRobotBlade2WaferOn.Value && !_diRobotBlade1WaferOn.Value)
                        return RobotArmWaferStateEnum.Present;
                    else return RobotArmWaferStateEnum.Unknown;
                }

                if (IsWaferPresenceOnBlade1 && IsWaferPresenceOnBlade2)
                {
                    return RobotArmWaferStateEnum.Present;
                }
                if ((!IsWaferPresenceOnBlade1) && !IsWaferPresenceOnBlade2)
                {
                    return RobotArmWaferStateEnum.Absent;
                }

            }
            return RobotArmWaferStateEnum.Unknown;
        }

        public void NoteError(string errortext)
        {
            OnError(errortext);
        }

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

        public override bool OnActionDone(object[] param)
        {
            BladeTarget = ModuleName.System;
            Blade1Target = ModuleName.System;
            Blade2Target = ModuleName.System;
            IsBusy = false;
            if (_lstHandlers.Count == 0)
            {
                return base.OnActionDone(param);
            }

            return true;
        }

        public override void Terminate()
        {
            _thread.Stop();
            if (!SC.ContainsItem($"{_scRoot}.{Name}.CloseConnectionOnShutDown") || SC.GetValue<bool>($"{_scRoot}.{Name}.CloseConnectionOnShutDown"))
            {
                LOG.Write("Close connection for" + RobotModuleName.ToString());
                _connection.Disconnect();
            }
            base.Terminate();
        }

        private bool CheckArm(RobotArmEnum arm)
        {
            if (arm != RobotArmEnum.Lower && arm != RobotArmEnum.Upper)
            {
                EV.PostAlarmLog("Robot", "Invalid Parameter.");
                return false;
            }
            return true;
        }

        private Dictionary<string, int> _pairSlot = new Dictionary<string, int>()
        {
            {"01",1 },
            {"23",2 },
            {"45",3 },
            {"67",4 },
        };
        private int GetTargetSlotByModule(ModuleName module, int[] slots)
        {
            if (ModuleHelper.IsPm(module)) return 1;
            if (ModuleHelper.IsLoadLock(module))
            {
                Array.Sort(slots);
                string str = string.Join("", slots);
                foreach (var item in _pairSlot)
                {
                    if (item.Key == str || item.Key.Contains(str))
                    {
                        int slot = item.Value;
                        if (module == ModuleName.LLB) slot += 4;
                        return slot;
                    }
                }
            }
            EV.PostAlarmLog(Module, "TM robot get slot error ");
            return 0;
        }

        private void ExectueCommand(string command, string strpara)
        {
            lock (_locker)
            {
                if (_lstHandlers.Count == 0 && !_connection.IsBusy)
                {
                    ExecuteHandler(new SPaceTRanRobotMotionHandler(this, command, strpara));
                }
                else
                {
                    _lstHandlers.AddLast(new SPaceTRanRobotMotionHandler(this, command, strpara));
                }
            }
        }

        private void ExecuteComand(RobotArmEnum arm, Pan pan, int trsSlot, string trsStation, string action, string flag)
        {
            ExectueCommand(action, GetParam(arm, pan, trsSlot, trsStation, flag));
        }

        private string GetWaferSizeIndex(ModuleName sourceModule, int sourceSlot, ModuleName targetModule, int targetSlot)
        {
            var wz = WaferManager.Instance.GetWaferSize(sourceModule, sourceSlot);
            int wzindex = 0;
            if (wz == WaferSize.WS12) wzindex = 12;
            if (wz == WaferSize.WS8) wzindex = 8;
            if (wz == WaferSize.WS6) wzindex = 6;
            if (wz == WaferSize.WS4) wzindex = 4;
            if (wz == WaferSize.WS3) wzindex = 3;
            if (wz == WaferSize.WS0) wzindex = 8;
            return ModuleHelper.IsPm(targetModule) ? SC.GetStringValue($"CarrierInfo.{targetModule}{targetSlot + 1}Station{wzindex}") : SC.GetStringValue($"CarrierInfo.{targetModule}Station{wzindex}");
        }
        private void SetRobotMoveInfo(RobotArmEnum arm, ModuleName module, int slot, RobotAction action)
        {
            CmdRobotArm = arm;
            CmdTarget = module;
            Blade1Target = module;
            Blade2Target = module;
            MoveInfo = new RobotMoveInfo()
            {
                Action = action,
                ArmTarget = CmdRobotArm == RobotArmEnum.Lower ? RobotArm.ArmA : RobotArm.ArmB,
                BladeTarget = ModuleHelper.IsPm(module) ? (BuildBladeTarget() + slot) : BuildBladeTarget(),
            };
        }

        private string GetParam(RobotArmEnum arm, Pan pan, int slot, string station)
        {
            var nArm = (arm == RobotArmEnum.Lower) ? "A" : (arm == RobotArmEnum.Upper) ? "B" : "";
            string strpara = $" {station} SLOT {slot} ARM {nArm}";
            if (pan != Pan.Both)
            {
                strpara += $" PAN {Converter.MapBladeAndPanToFork((int)arm, pan).ToUpper()}";
            }
            return strpara;
        }
        private string GetParam(RobotArmEnum arm, Pan pan, int slot, string station, string flag)
        {
            return GetParam(arm, pan, slot, station) + $" {flag} NR";
        }
    }
    public enum RROpModeEnum
    {
        Initializing,
        Remote,
        Maitanance,
        Recovery,
    }

    public enum RROpStatusEnum
    {
        Stop,
        Moving,
        TempStop,
    }
}