ZZ-EFEM/Framework/RTEquipmentLibrary1/HardwareUnits/Robots/Hiwin/HiwinRobot.cs

using Aitex.Core.Common;
using Aitex.Core.RT.Device;
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.Common;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots.RobotBase;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots.HiWin
{
    //上银机械手
    public class HiwinRobot:RobotBaseDevice,IConnection
    {
        public string Address => Connection.Address;
        public bool IsConnected => Connection.IsConnected && !_connection.IsCommunicationError;
        public bool Connect()
        {
            return _connection.Connect();
        }

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

        public string PortStatus { get; set; } = "Closed";
        public Dictionary<string, string> ModuleAssociateStationDic
        {
            get => _moduleAssociateStationDic;
            set
            {
                _moduleAssociateStationDic = value;
            }
        }
        public Dictionary<string,string> ModuleStationDic { get; set; }

        private HiwinRobotConnection _connection;
        public HiwinRobotConnection Connection
        {
            get { return _connection; }
        }

        public bool SevoOnOff { get; private set; }
        public string MapResult { get; set; }

        private R_TRIG _trigError = new R_TRIG();
        private bool _isAlarm;

        private R_TRIG _trigCommunicationError = new R_TRIG();
        private R_TRIG _trigRetryConnect = new R_TRIG();

        private PeriodicJob _thread;

        private LinkedList<HandlerBase> _lstHandler = new LinkedList<HandlerBase>();

        public List<IOResponse> IOResponseList { get; set; } = new List<IOResponse>();
        private Dictionary<string, string> _errorCodeReferenceDic;
        private Dictionary<string, string> _moduleAssociateStationDic;

        private object _locker = new object();
        private SCConfigItem _scHomeTimeout;
        private SCConfigItem _scMotionTimeout;
        //private SCConfigItem _scBladeSlots;

        public int BladeSlots =>  1  ;
        private bool _enableLog;
        private string _scRoot;

        public HiwinRobot(string module, string name, string scRoot, string endof = "\r\n") : base(module, name,scRoot) 
        {
            string deviceIP = SC.GetStringValue($"{scRoot}.Address");
            _connection = new HiwinRobotConnection(deviceIP, endof);
            _scRoot = scRoot;
            _connection.EnableLog(SC.GetValue<bool>($"{scRoot}.EnableLogMessage"));

            if (_connection.Connect())
            {
                PortStatus = "Open";
                EV.PostInfoLog(Module, $"{Module} connected");
            }
            else
            {
                PortStatus = "Close";
                EV.PostInfoLog(Module, $"{Module} connect failed");
            }

            _thread = new PeriodicJob(50, OnTimer, $"{Module}.{Name} MonitorHandler", true);

            _errorCodeReferenceDic = new Dictionary<string, string>()
            {

            };
        }

        private bool OnTimer()
        {
            try
            {
                //return true;
                _connection.MonitorTimeout();

                if (!_connection.IsConnected || _connection.IsCommunicationError)
                {
                    lock (_locker)
                    {
                        _lstHandler.Clear();
                    }

                    _trigRetryConnect.CLK = !_connection.IsConnected;
                    if (_trigRetryConnect.Q)
                    {
                        if (!_connection.Connect())
                        {
                            EV.PostAlarmLog(Module, $"Can not connect with {Module} {_connection.Address}");
                        }
                        else
                        {
                            //_lstHandler.AddLast(new SunwayRobotQueryPinHandler(this, _deviceAddress));
                            //_lstHandler.AddLast(new SunwayRobotSetCommModeHandler(this, _deviceAddress, EnumRfPowerCommunicationMode.Host));
                        }
                    }
                    return true;
                }

                HandlerBase handler = null;
                if (!_connection.IsBusy)
                {
                    lock (_locker)
                    {
                        if (_lstHandler.Count > 0)
                        {
                            handler = _lstHandler.First.Value;

                            _lstHandler.RemoveFirst();
                        }
                    }

                    if (handler != null)
                    {
                        _connection.Execute(handler);
                    }
                }
            }
            catch (Exception ex)
            {
                LOG.Write(ex);
            }

            return true;
        }
        public override bool IsReady()
        {
            return RobotState == RobotStateEnum.Idle && !IsBusy && !_connection.IsBusy && _lstHandler.Count == 0;
        }
        protected override void SubscribeWaferLocation()
        {
        }
        internal void SetRobotSV(string axis,bool isOn) 
        {
            lock (_locker) 
            {
                _lstHandler.AddLast(new HiwinRobotOpeOrCloseMotorHandler(this, RobotModuleName, isOn, axis));
            }
        }

        public override void Reset()
        {
            _trigError.RST = true;

            _connection.SetCommunicationError(false, "");
            _trigCommunicationError.RST = true;
            _trigWarningMessage.RST = true;

            //_enableLog = SC.GetValue<bool>($"{ScBasePath}.{Name}.EnableLogMessage");

            _trigRetryConnect.RST = true;

            base.Reset();
        }
        #region Note Functions
        private R_TRIG _trigWarningMessage = new R_TRIG();

        public void NoteError(string reason)
        {
            if (reason != null)
            {
                _trigWarningMessage.CLK = true;
                var content = reason;
                if (_errorCodeReferenceDic.ContainsKey(reason))
                    content = _errorCodeReferenceDic[reason];
                if (_trigWarningMessage.Q)
                {
                    lock (_locker)
                    {
                        _lstHandler.AddLast(new HiwinRobotGetAllErrHandler(this));
                    }
                    if (RobotHasError == null)
                    {
                        EV.PostAlarmLog(Module, $"{Module} error, {reason} {content}");
                    }
                    else
                    {
                        //RobotHasError.Description = $"{Module}.{Name} error, {reason} {content}";
                        RobotHasError.Set($"{Module} error, {reason} {content}");
                    }
                }
                _isAlarm = true;
                ErrorCode = reason;
            }
            else
            {
                _isAlarm = false;
            }
        }

        protected override bool fStartInit(object[] param)
        {
            _connection.EnableLog(SC.GetValue<bool>($"{_scRoot}.EnableLogMessage"));
            lock (_locker)
            {
                _lstHandler.AddLast(new HiwinRobotHomeALLHandler(this, RobotModuleName));
                _lstHandler.AddLast(new HiwinRobotSetSpeedHandler(this, SC.GetValue<int>($"{_scRoot}.Speed")));
                _lstHandler.AddLast(new HiwinRobotQueryWaferPresentHandler(this));
                _lstHandler.AddLast(new HiwinRobotGetAllErrHandler(this));
            }
            return true;
        }
        protected override bool fMonitorInit(object[] param)
        {
            if (_lstHandler.Count == 0
                && !_connection.IsBusy)
            {
                IsBusy = false;
                return true;
            }
            return false;
        }

        protected override bool fClear(object[] param)
        {
            //lock (_locker)
            //{
            //    _lstHandler.AddLast(new SunwayRobotResetHandler(this));
            //}

            return true;
        }

        public override bool ReadParameter(object[] param)
        {
            IsBusy = true;
            return CheckToPostMessage((int)RobotMsg.ReadData, param);
        }

        public override bool GoTo(object[] param)
        {
         
            return false;
        }

        protected override bool fStartReadData(object[] param)
        {
            return true;
        }
        protected override bool fMonitorReadData(object[] param)
        {
            IsBusy = false;
            return _lstHandler.Count == 0 && !_connection.IsBusy;
        }

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

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

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

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

        //(arm, module, slot, isRetract, isZaxisDown)
        protected override bool fStartGoTo(object[] param)
        {
            return true;
        }
        protected override bool fMonitorGoTo(object[] param)
        {
            if (_lstHandler.Count == 0 && !_connection.IsBusy)
            {
                IsBusy = false;

                return true;
            }
            return false;
        }

        protected override bool fStartMapWafer(object[] param)
        {
            IsBusy = true;

            ModuleName tempmodule = (ModuleName)Enum.Parse(typeof(ModuleName), param[0].ToString());

            if (!ModuleStationDic.ContainsKey($"{ tempmodule}.Map"))
            {
                EV.PostAlarmLog(Module, $"{Module} target module {tempmodule} is not teach");
            }
            string station = ModuleStationDic[$"{ tempmodule}.Map"];

            lock (_locker)
            {
                _lstHandler.AddLast(new HiWinWaferScanActionHandler(this, $"{station}"));
                _lstHandler.AddLast(new HiwinRobotGetMapResultHandler(this));
                _lstHandler.AddLast(new HiwinRobotGetAllErrHandler(this));
            }
            return true;
        }
        protected override bool fMonitorMap(object[] param)
        {
            if (_lstHandler.Count == 0 && !_connection.IsBusy)
            {
                ModuleName tempmodule = (ModuleName)Enum.Parse(typeof(ModuleName), CurrentParamter[0].ToString());
                var wafers = WaferManager.Instance.GetWafers(tempmodule);
                if(wafers != null && wafers.Any())
                {
                    foreach(var wafer in wafers)
                    {
                        if (wafer == null || wafer.IsEmpty)
                            continue;
                        WaferManager.Instance.DeleteWafer(tempmodule, wafer.OriginSlot);
                    }
                }
                if(!string.IsNullOrEmpty(MapResult))
                {
                    for(int i=0; i<MapResult.Length; i++)
                    {
                        switch(MapResult[i])
                        {
                            case 'X'://unkonwn wafer
                                WaferManager.Instance.CreateWafer(tempmodule, i, WaferStatus.Unknown);
                                EV.PostAlarmLog(Module, $"{Module} {tempmodule} map result slot={i+1} has unkonwn wafer");
                                break;
                            case '0'://no wafer
                                break;
                            case '1'://has wafer
                                WaferManager.Instance.CreateWafer(tempmodule, i, WaferStatus.Normal);
                                break;
                            case '2'://cross wafer
                                WaferManager.Instance.CreateWafer(tempmodule, i, WaferStatus.Crossed);
                                EV.PostAlarmLog(Module, $"{Module} {tempmodule} map result slot={i + 1} has cross wafer");
                                break;
                            case '3'://double wafer
                                WaferManager.Instance.CreateWafer(tempmodule, i, WaferStatus.Double);
                                EV.PostAlarmLog(Module, $"{Module} {tempmodule} map result slot={i + 1} has double wafer");
                                break;
                            case '4'://薄 wafer
                                WaferManager.Instance.CreateWafer(tempmodule, i, WaferStatus.Unknown);
                                EV.PostAlarmLog(Module, $"{Module} {tempmodule} map result slot={i + 1} has thin wafer");
                                break;
                        }
                    }
                }

                IsBusy = false;
                return true;
            }
            return false;
        }

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

        //(arm, module, slot)
        protected override bool fStartPlaceWafer(object[] param)
        {
            IsBusy = true;

            ModuleName tempmodule = (ModuleName)Enum.Parse(typeof(ModuleName), param[0].ToString());

            int slotindex = int.Parse(param[1].ToString()) + 1;

            RobotArmEnum arm = (RobotArmEnum)param[2];
            CmdRobotArm = arm;
            if (!ModuleStationDic.ContainsKey($"{ tempmodule}.Place"))
            {
                EV.PostAlarmLog(Module, $"{Module} target module {tempmodule} is not teach");
            }
            string station = ModuleStationDic[$"{ tempmodule}.Place"];

            lock (_locker)
            {
                _lstHandler.AddLast(new SunwayRobotRawCommandHandler(this, "PUT", $"{station},{slotindex}"));
                _lstHandler.AddLast(new HiwinRobotQueryWaferPresentHandler(this));
                _lstHandler.AddLast(new HiwinRobotGetAllErrHandler(this));
            }
            Blade1Target = tempmodule;
            Blade2Target = tempmodule;

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

            return true;
        }
        protected override bool fMonitorPlace(object[] param)
        {
            if (_lstHandler.Count == 0 && !_connection.IsBusy)
            {
                ModuleName destModule;
                if (!Enum.TryParse(CurrentParamter[0].ToString(), out destModule)) return false;
                int destSlotIndex;
                if (!int.TryParse(CurrentParamter[1].ToString(), out destSlotIndex)) return false;
                RobotArmEnum arm = (RobotArmEnum)CurrentParamter[2];

                if (arm == RobotArmEnum.Lower || arm == RobotArmEnum.Blade1)
                    WaferManager.Instance.WaferMoved(RobotModuleName, 0, destModule, destSlotIndex);
                if (arm == RobotArmEnum.Upper || arm == RobotArmEnum.Blade2)
                    WaferManager.Instance.WaferMoved(RobotModuleName, 1, destModule, destSlotIndex);
                if (arm == RobotArmEnum.Both)
                {
                    WaferManager.Instance.WaferMoved(RobotModuleName, 0, destModule, destSlotIndex);
                    WaferManager.Instance.WaferMoved(RobotModuleName, 1, destModule, destSlotIndex);
                }

                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(),
                };
                IsBusy = false;
                return true;
            }

            return false;
        }

        //(arm, module, slot)
        protected override bool fStartPickWafer(object[] param)
        {
            IsBusy = true;
            
            ModuleName tempmodule = (ModuleName)Enum.Parse(typeof(ModuleName), param[0].ToString());

            int slotindex = int.Parse(param[1].ToString()) + 1;

            RobotArmEnum arm = (RobotArmEnum)param[2];
            CmdRobotArm = arm;
            if (!ModuleStationDic.ContainsKey($"{ tempmodule}.Pick"))
            {
                EV.PostAlarmLog(Module, $"{Module} target module {tempmodule} is not teach");
            }
            string station = ModuleStationDic[$"{ tempmodule}.Pick"];

            lock (_locker)
            {
                _lstHandler.AddLast(new SunwayRobotRawCommandHandler(this, "GET", $"{station},{slotindex}"));
                _lstHandler.AddLast(new HiwinRobotQueryWaferPresentHandler(this));
                _lstHandler.AddLast(new HiwinRobotGetAllErrHandler(this));
            }
            Blade1Target = tempmodule;
            Blade2Target = tempmodule;

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

            return true;
        }
        protected override bool fMonitorPick(object[] param)
        {
            if (_lstHandler.Count == 0 && !_connection.IsBusy)
            {
                ModuleName sourcemodule;
                if (!Enum.TryParse(CurrentParamter[0].ToString(), out sourcemodule)) return false;
                int SourceslotIndex;
                if (!int.TryParse(CurrentParamter[1].ToString(), out SourceslotIndex)) return false;
                RobotArmEnum arm = (RobotArmEnum)CurrentParamter[2];

                if (arm == RobotArmEnum.Lower || arm == RobotArmEnum.Blade1)
                    WaferManager.Instance.WaferMoved(sourcemodule, SourceslotIndex, RobotModuleName, 0);
                if (arm == RobotArmEnum.Upper || arm == RobotArmEnum.Blade2)
                    WaferManager.Instance.WaferMoved(sourcemodule, SourceslotIndex, RobotModuleName, 1);
                if (arm == RobotArmEnum.Both)
                {
                    WaferManager.Instance.WaferMoved(sourcemodule, SourceslotIndex, RobotModuleName, 0);
                    WaferManager.Instance.WaferMoved(sourcemodule, SourceslotIndex, RobotModuleName, 1);
                }

                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(),
                };
                IsBusy = false;
                return true;
            }

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

        protected override bool fReset(object[] param)
        {
            _trigError.RST = true;
            _trigWarningMessage.RST = true;

            _connection.SetCommunicationError(false, "");
            _trigCommunicationError.RST = true;

            //_enableLog = SC.GetValue<bool>($"{ScBasePath}.{Name}.EnableLogMessage");

            _trigRetryConnect.RST = true;
            if (_isAlarm)
            {
                //lock (_locker)
                //{
                //    _lstHandler.AddLast(new SunwayRobotResetHandler(this));
                //}

                //Thread.Sleep(100);
                CheckToPostMessage((int)RobotMsg.ActionDone);
            }

            IsBusy = false;

            return true;
        }

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

        protected override bool fError(object[] param)
        {
            return true;
        }
        public override RobotArmWaferStateEnum GetWaferState(RobotArmEnum arm)
        {
            if (arm == RobotArmEnum.Blade1)
            {
                return IsWaferPresenceOnBlade1 ? RobotArmWaferStateEnum.Present : RobotArmWaferStateEnum.Absent;
            }
            if (arm == RobotArmEnum.Blade2)
            {
                return IsWaferPresenceOnBlade2 ? RobotArmWaferStateEnum.Present : RobotArmWaferStateEnum.Absent;
            }

            return RobotArmWaferStateEnum.Unknown;
        }

        protected override bool fStop(object[] param)
        {
            lock (_locker)
            {
                _lstHandler.AddLast(new HiwinRobotAxisStopMoveHandler(this, RobotModuleName));
            }
            return true;
        }

        protected override bool fStartExtendForPick(object[] param)
        {
            throw new NotImplementedException();
        }

        protected override bool fStartExtendForPlace(object[] param)
        {
            throw new NotImplementedException();
        }

        protected override bool fStartRetractFromPick(object[] param)
        {
            throw new NotImplementedException();
        }

        protected override bool fStartRetractFromPlace(object[] param)
        {
            throw new NotImplementedException();
        }

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

        #endregion
    }
   
}