Kepler-Release/Venus/Venus_RT/Devices/TM/HongHuVR.cs

using Aitex.Core.RT.Log;
using Aitex.Core.RT.SCCore;
using Aitex.Core.Util;
using Aitex.Sorter.Common;
using MECF.Framework.Common.CommonData;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.SubstrateTrackings;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robot;
using System;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.Diagnostics.Tracing;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;
using System.Text.RegularExpressions;
using System.Threading.Tasks;
using Venus_Core;
using Venus_RT.Modules;

namespace Venus_RT.Devices.VCE
{
    //泓浒
    enum VRStep
    {
        Idle,
        Home,
        Move,
        Halt,
        Release,
        Goto,
        Pick,
        PickExtend,
        PickRetract,
        Place,
        PlaceExtend,
        PlaceRetract,
        Xfer,
        CheckLoad_ArmA,
        CheckLoad_ArmB,
        SetLoad,
        ReQueryLoadA,
        ReQueryLoadB,
    }
    public class HongHuVR : ITransferRobot
    {
        private readonly AsyncSocket _socket;
        private const string EOF = "\n";
        private RState _status;
        private bool _IsHomed;
        private VRStep _currentStep = VRStep.Idle;
        private Dictionary<ModuleName, int> _StationNumbers = new Dictionary<ModuleName, int>();
        public RState Status { get { return _status; } }
        public bool IsHomed { get { return _IsHomed; } }
        private string Hand2Arm(Hand hand) => hand == Hand.Blade2 ? "B" : "A";

        //private readonly Regex _rex_check_load = new Regex(@"LOAD\s+(A|B)\s+(\w+)\s*");
        private readonly Regex _rex_check_load = new Regex(@"CHECK LOAD\s*");

        private readonly Regex _rex_error_code = new Regex(@"_ERR\s+(\d+)\s*");
        private RobotMoveInfo _robotMoveInfo = new RobotMoveInfo();
        public RobotMoveInfo TMRobotMoveInfo { get { return _robotMoveInfo; } }

        public double Offset_X => 0;
                                  
        public double Offset_Y => 0;
                                  
        public double Offset_D => 0;

        public Dictionary<string, string> _error2msg = new Dictionary<string, string>()
        {
            {"221"  , "手臂选择无效,检查指令中的对应参数" },
            {"222"  , "无效的 Pan 参数"},
            {"305"  , "无法识别的命令,检查指令" },
            {"309"  , "不支持的指令,检查指令" },
            {"350"  , "解析器错误，堆栈溢出,检查指令" },
            {"402"  , "槽位参数异常，检查指令中的对应参数" },
            {"407"  , "T轴参数异常，检查指令中的对应参数" },
            {"408"  , "R轴参数异常，检查指令中的对应参数" },
            {"409"  , "Z轴参数异常，检查指令中的对应参数" },
            {"416"  , "站点未初始化，检查指令中的对应参数" },
            {"417"  , "offset 值过大，检查指令中的对应参数" },
            {"418"  , "AWC 功能检测到误差过大,检查晶圆是否过偏或检查 AWC 传感器标定参数" },
            {"550"  , "站参数超出范围，检查站点配置参数" },
            {"551"  , "ARM 不在站点，检查当前手臂实际位置与逻辑位置" },
            {"560"  , "机械臂处于 HALT 状态，拒绝接受新的指令" },
            {"561"  , "当前机械臂状态机与指令不匹配，当前状态机不满足执行该指令的条件" },
            {"600"  , "PAligner通讯异常，检查与 PA 的通讯电缆，上电情况等" },
            {"603"  , "收到急停指令" },
            {"608"  , "机械臂急停中" },
            {"700"  , "当前手臂有晶圆，检查指令动作要求的晶圆状态，与手臂内部存储的晶圆状态是否匹配" },
            {"701" , "当前手臂无晶圆，检查指令动作要求的晶圆状态，与手臂内部存储的晶圆状态是否匹配"},
            {"711" , "站点互锁错误，检查站点的“允许机械臂进入信号”是否就绪"},
            {"721" , "取片失败，检查手臂晶圆状态，或者晶圆检测传感器状态"},
            {"722" , "放片失败，检查手臂晶圆状态，或者晶圆检测传感器状态"},
            {"791" , "查询 AWC 数据失败，未曾执行过 AWC 相关功能的操作"},
            {"799" , "放片失败，检查手臂晶圆状态，或者晶圆检测传感器状态"},
            {"803" , "机械臂上电失败 HOME ALL 指令，或者其他上电操作，驱动器未能正常响应上电"},
            {"1100" , "无法安全回 HOME 操作，检查机械臂是否在安全 HOME 半径内，常见于双臂机械臂，双臂同时伸出超过安全位"},
            {"13004" , "PA报错，检查 PA 运行状态，PA 扫片结果等"},
        };
        private BlockingCollection<RobotAnimationData> blockingCollection = new BlockingCollection<RobotAnimationData>();

        public HongHuVR()
        {
            _socket = new AsyncSocket("", EOF);
            _socket.Connect(SC.GetStringValue($"SETM.IPAddress"));
            _socket.OnDataChanged += OnReceiveMessage;
            _socket.OnErrorHappened += OnErrorHappen;

            _status = RState.Init;
            _IsHomed = false;

            _StationNumbers[ModuleName.VCE1] = SC.GetValue<int>("SETM.VCE1StationNumber");
            _StationNumbers[ModuleName.VPA] = SC.GetValue<int>("SETM.VPAStationNumber");

            _StationNumbers[ModuleName.PMA] = SC.GetValue<int>("SETM.PMAStationNumber");
            _StationNumbers[ModuleName.PMB] = SC.GetValue<int>("SETM.PMBStationNumber");
            _StationNumbers[ModuleName.PMC] = SC.GetValue<int>("SETM.PMCStationNumber");
            _StationNumbers[ModuleName.PMD] = SC.GetValue<int>("SETM.PMDStationNumber");

            WaferManager.Instance.SubscribeLocation(ModuleName.TMRobot, 2);
            Task.Run(() =>
            {
                foreach (var data in blockingCollection.GetConsumingEnumerable())
                {
                    _robotMoveInfo.Action = data.Action;
                    _robotMoveInfo.ArmTarget = data.Hand == Hand.Blade1 ? RobotArm.ArmA : (data.Hand == Hand.Both ? RobotArm.Both : RobotArm.ArmB);
                    _robotMoveInfo.BladeTarget = $"{_robotMoveInfo.ArmTarget}.{data.Target}";
                    System.Threading.Thread.Sleep(300);
                }
            });
        }
        //初始化某个轴
        //1.清错
        //2.设备上电
        //3.各轴按顺序运动
        public bool Home()
        {
            _status = RState.Running;
            _currentStep = VRStep.Home;
            return _SendCommand("HOME ALL");
        }
        public bool Halt()
        {
            _status = RState.Running;
            _currentStep = VRStep.Halt;
            return _SendCommand("HALT");
        }
        public bool Release()
        {
            _status = RState.Running;
            _currentStep = VRStep.Release;
            return _SendCommand("RELEASE");
        }
        //public bool MOVE()
        //{
        //}
        public bool Pick(ModuleName station, int slot, Hand hand)
        {
            if (!CheckRobotStatus())
                return false;

            _currentStep = VRStep.Pick;
            _status = RState.Running;
            SetRobotMovingInfo(RobotAction.Picking, hand, station);
            return _SendCommand($"PICK {_StationNumbers[station]} ARM {Hand2Arm(hand)}");
        }

        public bool PickExtend(ModuleName station, int slot, Hand hand)
        {
            if (!CheckRobotStatus())
                return false;

            _currentStep = VRStep.PickExtend;
            _status = RState.Running;
            SetRobotMovingInfo(RobotAction.Extending, hand, station);
            return _SendCommand($"PICK {_StationNumbers[station]} ARM {Hand2Arm(hand)} ENRT NR");
        }

        public bool PickRetract(ModuleName station, int slot, Hand hand)
        {
            if (!CheckRobotStatus())
                return false;

            _currentStep = VRStep.PickRetract;
            _status = RState.Running;
            SetRobotMovingInfo(RobotAction.Retracting, hand, station);
            return _SendCommand($"PICK {_StationNumbers[station]} ARM {Hand2Arm(hand)} STRT NR");
        }
        public bool Place(ModuleName station, int slot, Hand hand)
        {
            if (!CheckRobotStatus())
                return false;

            _currentStep = VRStep.Place;
            _status = RState.Running;
            SetRobotMovingInfo(RobotAction.Placing, hand, station);
            return _SendCommand($"PLACE {_StationNumbers[station]} ARM {Hand2Arm(hand)}");
        }
        public bool PlaceExtend(ModuleName station, int slot, Hand hand)
        {
            if (!CheckRobotStatus())
                return false;

            _currentStep = VRStep.Place;
            _status = RState.Running;
            SetRobotMovingInfo(RobotAction.Extending, hand, station);
            return _SendCommand($"PLACE {_StationNumbers[station]} ARM {Hand2Arm(hand)} ENRT NR");
        }
        public bool PlaceRetract(ModuleName station, int slot, Hand hand)
        {
            if (!CheckRobotStatus())
                return false;

            _currentStep = VRStep.Place;
            _status = RState.Running;
            SetRobotMovingInfo(RobotAction.Retracting, hand, station);
            return _SendCommand($"PLACE {_StationNumbers[station]} ARM {Hand2Arm(hand)} STRT NR");
        }
        public bool Transfer(ModuleName fromstation, ModuleName tostation, Hand hand)
        {
            if (!CheckRobotStatus())
                return false;

            _currentStep = VRStep.Xfer;
            _status = RState.Running;
            return _SendCommand($"XFER ARM {Hand2Arm(hand)} {_StationNumbers[fromstation]} {_StationNumbers[tostation]}");

        }
        public bool CheckLoad(Hand hand = Hand.Blade1)
        {
            if (_currentStep != VRStep.Home && _currentStep != VRStep.CheckLoad_ArmA && !CheckRobotStatus())
                return false;

            _currentStep = hand == Hand.Blade2 ? VRStep.CheckLoad_ArmB : VRStep.CheckLoad_ArmA;
            _status = RState.Running;
            string arm = hand == Hand.Blade2 ? "B" : "A";

            return _SendCommand($"CHECK LOAD {_StationNumbers[ModuleName.VPA]} {arm}");
        }


        public bool QueryAwc()
        {
            return true;
        }

        public bool Goto(ModuleName station, int slot, Hand hand)
        {
            if (!CheckRobotStatus())
                return false;

            _currentStep = VRStep.Goto;
            _status = RState.Running;
            SetRobotMovingInfo(RobotAction.Rotating, hand, station);
            return _SendCommand($"Goto {_StationNumbers[station]} ARM {Hand2Arm(hand)}");
        }

        public bool MoveTo(ModuleName stnFrom, ModuleName stnTo, Hand hand)
        {
            _currentStep = VRStep.Move;
            _status = RState.Running;
            return _SendCommand($"MOVE R ABS 100 ARM {Hand2Arm(hand)}"); ;
        }
        public bool PickWithOffset(ModuleName station, int slot, Hand hand, int Roffset, int Toffset)
        {
            _currentStep = VRStep.Pick;
            _status = RState.Running;
            return _SendCommand($"PICK {_StationNumbers[station]} ARM {Hand2Arm(hand)} RO {Roffset} TO {Toffset}"); ;
        }
        public bool ReQueryLoadA()
        {
            _currentStep = VRStep.ReQueryLoadA;
            _status = RState.Running;
            return _SendCommand($"RQ LOAD ARM A");
        }
        public bool ReQueryLoadB()
        {
            _currentStep = VRStep.ReQueryLoadA;
            _status = RState.Running;
            return _SendCommand($"RQ LOAD ARM B");
        }
        private bool _SendCommand(string cmd)
        {
            LOG.WriteSingeLine(eEvent.INFO_TM_ROBOT, ModuleName.TMRobot, $"Send Command to TMRobot: {cmd},Connect:{_socket.IsConnected}");
            return _socket.Write(cmd + EOF);
        }

        private bool CheckRobotStatus()
        {
            if (Status == RState.Init)
            {
                LOG.Write(eEvent.ERR_DEVICE_INFO, ModuleName.TMRobot, "TMRobot is not homed, please home first.");
                return false;
            }
            else if (Status == RState.Running)
            {
                LOG.Write(eEvent.ERR_DEVICE_INFO, ModuleName.TMRobot, "TMRobot is busy, please wait a minute");
                return false;
            }
            else if (Status == RState.Failed || Status == RState.Timeout)
            {
                LOG.Write(eEvent.ERR_DEVICE_INFO, ModuleName.TMRobot, "TMRobot has a error, please check and fix the hardware issue and home it");
                return false;
            }

            return true;
        }

        private void OnReceiveMessage(string RevMsg)
        {
            LOG.WriteSingeLine(eEvent.INFO_TM_ROBOT, ModuleName.TMRobot, $"Receive message from TMRobot: {RevMsg}, while {_currentStep}");
            RevMsg = RevMsg.Trim();
            if (_rex_error_code.IsMatch(RevMsg))
            {
                _IsHomed = false;
                _status = RState.Failed;

                var results = _rex_error_code.Match(RevMsg);
                ErrorMessageHandler(results.Groups[1].Value);
                return;
            }

            switch (_currentStep)
            {
                case VRStep.Goto:
                case VRStep.Halt:
                case VRStep.Move:
                case VRStep.Xfer:
                case VRStep.Pick:
                case VRStep.PickExtend:
                case VRStep.PickRetract:
                case VRStep.Place:
                case VRStep.PlaceExtend:
                case VRStep.PlaceRetract:
                    {
                        if (RevMsg.Trim() == "_RDY" || (RevMsg.Contains("_RDY") && !RevMsg.Contains("_ERR")))
                        {
                            _currentStep = VRStep.Idle;
                            _status = RState.End;
                        }
                        else
                        {
                            ReportWrongMsg(RevMsg);
                        }

                        if (_currentStep != VRStep.PickExtend && _currentStep != VRStep.PlaceExtend)
                            SetRobotMovingInfo(RobotAction.None, Hand.Both, ModuleName.TMRobot);
                    }
                    break;
                case VRStep.Home:
                    {

                        if (RevMsg.Trim() == "_RDY")
                        {
                            //CheckLoad(Hand.Blade1);
                            _currentStep = VRStep.Idle;
                            _status = RState.End;
                            _IsHomed = true;
                            SetRobotMovingInfo(RobotAction.Homing, Hand.Both, ModuleName.SETM);
                        }
                        else
                            ReportWrongMsg(RevMsg);
                    }
                    break;
                case VRStep.CheckLoad_ArmA:
                    {
                        if (_rex_check_load.IsMatch(RevMsg))
                        {
                            GetCheckLoadResult(RevMsg);

                            //CheckLoad(Hand.Blade2);
                        }

                        if (RevMsg.Trim() == "_RDY" || (RevMsg.Contains("_RDY") && !RevMsg.Contains("_ERR")))
                        {
                            _currentStep = VRStep.Idle;
                            _status = RState.End;
                        }
                    }
                    break;
                case VRStep.CheckLoad_ArmB:
                    {
                        if (_rex_check_load.IsMatch(RevMsg))
                        {
                            GetCheckLoadResult(RevMsg);

                            //_currentStep = VRStep.Idle;
                            //_status = RState.End;
                            //_IsHomed = true;
                        }
                        if (RevMsg.Trim() == "_RDY" || (RevMsg.Contains("_RDY") && !RevMsg.Contains("_ERR")))
                        {
                            _currentStep = VRStep.Idle;
                            _status = RState.End;
                        }
                    }
                    break;
                default:
                    if (!RevMsg.Contains("_EVENT"))
                        ReportWrongMsg(RevMsg);
                    break;
            }
        }

        private void GetCheckLoadResult(string revMsg)
        {
            Match result = _rex_check_load.Match(revMsg);

            string Arm = result.Groups[1].Value;
            string WaferStatus = result.Groups[2].Value;
            if (WaferStatus == "ON")
            {
                WaferManager.Instance.CreateWafer(ModuleName.TMRobot, Arm == "A" ? 0 : 1, Aitex.Core.Common.WaferStatus.Unknown);
            }
        }

        private void ErrorMessageHandler(string errorcode)
        {
            string ErrorInfo;
            if (_error2msg.ContainsKey(errorcode))
            {
                ErrorInfo = _error2msg[errorcode];
                LOG.Write(eEvent.ERR_DEVICE_INFO, ModuleName.TMRobot, ErrorInfo);
            }
            else
            {
                LOG.Write(eEvent.ERR_DEVICE_INFO, ModuleName.TMRobot, $"Dictionary Not Contains error code:{errorcode}");
            }

            _IsHomed = false;
        }

        private void OnErrorHappen(ErrorEventArgs args)
        {
            //Singleton<RouteManager>.Instance.SETM.PostMsg(SETMEntity.MSG.Error);
            LOG.Write(eEvent.ERR_DEVICE_INFO, ModuleName.TMRobot, $"HongHu TMRobot Error: {args.Reason} while {_currentStep}");
        }

        private void ReportWrongMsg(string revMsg)
        {
            LOG.Write(eEvent.WARN_DEVICE_INFO, ModuleName.TMRobot, $"Receive wrong message:{revMsg} while {_currentStep}");
        }

        public void SetRobotMovingInfo(RobotAction action, Hand hand, ModuleName target)
        {
            _robotMoveInfo.Action = action;
            _robotMoveInfo.ArmTarget = hand == Hand.Blade1 ? RobotArm.ArmA : (hand == Hand.Both ? RobotArm.Both : RobotArm.ArmB);
            _robotMoveInfo.BladeTarget = $"{_robotMoveInfo.ArmTarget}.{target}";
        }

    }
}