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

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

namespace Venus_RT.Devices
{
    class SIASUNRobot : ITransferRobot
    {
        enum OPStep
        {
            Idle,
            Home,
            Goto,
            MoveTo,
            CheckLoad_ArmA,
            CheckLoad_ArmB,
            Pick,
            Place,
            PickExtend,
            PickRetract,
            PlaceExtent,
            PlaceRetract,
            QueryAwc
        }

        private RState _status;
        private bool _IsHomed;
        private bool _HasReceiveMsg;
        public RState Status { get { return _status; } }
        public bool IsHomed { get { return _IsHomed; } }
        private double offset_x = 0;
        private double offset_y = 0;
        public double Offset_X => offset_x;
        public double Offset_Y => offset_y;
        public double Offset_D => Math.Round(Math.Sqrt(Math.Pow(offset_x, 2) + Math.Pow(offset_y, 2)), 2);//欧式距离 保留后两位

        public RobotMoveInfo TMRobotMoveInfo { get { return _robotMoveInfo; } }

        private readonly AsyncSocket _socket;
        private OPStep _currentOP = OPStep.Idle;
        private Dictionary<ModuleName, int> _StationNumbers = new Dictionary<ModuleName, int>();
        private readonly int _checkLoadStation = 1;
        private RobotMoveInfo _robotMoveInfo = new RobotMoveInfo();

        private string Hand2Arm(Hand hand) => hand == Hand.Blade1 ? "B" : "A";

        private readonly Regex _rex_check_load = new Regex(@"LOAD\s+(A|B)\s+(\w+)\s*");
        private readonly Regex _rex_error_code = new Regex(@"_ERR\s+(\d+)\s*");
        private readonly Regex _rex_event_offset = new Regex(@"_EVENT\sROBOT\s[0-9|\s]*.*");
        private readonly Regex _rex_event_getoffset = new Regex(@"(?<=_EVENT\sROBOT\s)(.+?)(?=\sB)");
        private readonly Regex _rex_query_awc = new Regex(@"WAF_CEN\sRT[-|0-9|\s]*LFT[-|0-9|\s]*OFFSET[-|0-9|\s]*");
        private readonly Regex _rex_query_getoffset = new Regex(@"(?<=WAF_CEN\sRT[-|0-9|\s]*LFT[-|0-9|\s]*OFFSET\s)(.*)");

        private const string EOF = "\r\n";

        private BlockingCollection<RobotAnimationData> blockingCollection = new BlockingCollection<RobotAnimationData>();
        public SIASUNRobot()
        {
            _socket = new AsyncSocket("", EOF);
            _socket.Connect(SC.GetStringValue($"TM.IPAddress"));
            _socket.OnDataChanged += OnReceiveMessage;
            _socket.OnErrorHappened += OnErrorHappen;

            _status = RState.Init;
            _IsHomed = false;

            _StationNumbers[ModuleName.LLA] = SC.GetValue<int>("TM.LLAStationNumber");
            _StationNumbers[ModuleName.LLB] = SC.GetValue<int>("TM.LLBStationNumber");

            _StationNumbers[ModuleName.PMA] = SC.GetValue<int>("TM.PMAStationNumber");
            _StationNumbers[ModuleName.PMB] = SC.GetValue<int>("TM.PMBStationNumber");
            _StationNumbers[ModuleName.PMC] = SC.GetValue<int>("TM.PMCStationNumber");
            _StationNumbers[ModuleName.PMD] = SC.GetValue<int>("TM.PMDStationNumber");
            _StationNumbers[ModuleName.PME] = SC.GetValue<int>("TM.PMEStationNumber");
            _StationNumbers[ModuleName.PMF] = SC.GetValue<int>("TM.PMFStationNumber");

            _checkLoadStation = SC.GetValue<int>("TM.CheckLoadStation");

            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);
                }
            });
        }
        public bool Home()
        {
            _status = RState.Running;
            _currentOP = OPStep.Home;

            return _SendCommand("HOME ALL");
        }
        public bool Halt()
        {
            return _SendCommand("HALT");
        }
        public bool CheckLoad(Hand hand = Hand.Blade1)
        {
            if (_currentOP != OPStep.Home && _currentOP != OPStep.CheckLoad_ArmA && CheckRobotStatus() == false)
                return false;

            _currentOP = hand == Hand.Blade2 ? OPStep.CheckLoad_ArmB : OPStep.CheckLoad_ArmA;
            _status = RState.Running;
            return _SendCommand($"CHECK LOAD {_checkLoadStation} ARM {Hand2Arm(hand)}");
        }

        public bool QueryAwc()
        {
            //检查防止状态交叉
            if (CheckRobotStatus() == false)
                return false;
            offset_x = 0;
            offset_y = 0;
            _currentOP = OPStep.QueryAwc;
            _status = RState.Running;
            _HasReceiveMsg = false;
            return _SendCommand("RQ WAF_CEN DATA");
        }

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

            _currentOP = OPStep.Goto;
            _status = RState.Running;
            SetRobotMovingInfo(RobotAction.Rotating, hand, station);
            return _SendCommand($"GOTO N {_StationNumbers[station]} R RE Z DOWN SLOT {slot + 1} ARM {Hand2Arm(hand)}");
        }
        public bool MoveTo(ModuleName stnFrom, ModuleName stnTo, Hand hand)
        {
            if (CheckRobotStatus() == false)
                return false;

            _currentOP = OPStep.MoveTo;
            _status = RState.Running;
            return _SendCommand($"XFER ARM {Hand2Arm(hand)} {_StationNumbers[stnFrom]} {_StationNumbers[stnTo]}");
        }
        public bool PickExtend(ModuleName chamber, int slot, Hand hand)
        {
            if (CheckRobotStatus() == false)
                return false;

            _currentOP = OPStep.PickExtend;
            _status = RState.Running;
            SetRobotMovingInfo(RobotAction.Extending, hand, chamber);
            return _SendCommand($"PICK {_StationNumbers[chamber]} SLOT {slot + 1} ARM {Hand2Arm(hand)} ENRT NR");
        }
        public bool PickRetract(ModuleName chamber, int slot, Hand hand)
        {
            if (CheckRobotStatus() == false)
                return false;

            _currentOP = OPStep.PickRetract;
            _status = RState.Running;
            SetRobotMovingInfo(RobotAction.Retracting, hand, chamber);
            return _SendCommand($"PICK {_StationNumbers[chamber]} SLOT {slot + 1} ARM {Hand2Arm(hand)} STRT NR");
        }
        public bool PlaceExtend(ModuleName chamber, int slot, Hand hand)
        {
            if (CheckRobotStatus() == false)
                return false;

            _currentOP = OPStep.PlaceExtent;
            _status = RState.Running;
            SetRobotMovingInfo(RobotAction.Extending, hand, chamber);
            return _SendCommand($"PLACE {_StationNumbers[chamber]} SLOT {slot + 1} ARM {Hand2Arm(hand)} ENRT NR");
        }
        public bool PlaceRetract(ModuleName chamber, int slot, Hand hand)
        {
            if (CheckRobotStatus() == false)
                return false;

            _currentOP = OPStep.PlaceExtent;
            _status = RState.Running;
            SetRobotMovingInfo(RobotAction.Retracting, hand, chamber);

            return _SendCommand($"PLACE {_StationNumbers[chamber]} SLOT {slot + 1} ARM {Hand2Arm(hand)} STRT NR");
        }
        public bool Pick(ModuleName station, int slot, Hand hand)
        {
            if (CheckRobotStatus() == false)
                return false;

            _currentOP = OPStep.Pick;
            _status = RState.Running;
            SetRobotMovingInfo(RobotAction.Picking, hand, station);
            return _SendCommand($"PICK {_StationNumbers[station]} SLOT {slot + 1} ARM {Hand2Arm(hand)}");
        }
        public bool Place(ModuleName station, int slot, Hand hand)
        {
            if (CheckRobotStatus() == false)
                return false;

            _currentOP = OPStep.Place;
            _status = RState.Running;
            SetRobotMovingInfo(RobotAction.Placing, hand, station);
            return _SendCommand($"PLACE {_StationNumbers[station]} SLOT {slot + 1} ARM {Hand2Arm(hand)}");
        }

        private bool _SendCommand(string cmd)
        {
            LOG.WriteSingeLine(eEvent.INFO_TM_ROBOT, ModuleName.TMRobot, $"Send Command to SIASUN TM Robot: {cmd}");
            return _socket.Write(cmd + EOF);
        }

        private bool CheckRobotStatus()
        {
            if (Status == RState.Init)
            {
                LOG.Write(eEvent.ERR_TM_ROBOT, ModuleName.TMRobot, "TM Robot is not homed, please home first.");
                return false;
            }
            else if (Status == RState.Running)
            {
                LOG.Write(eEvent.ERR_TM_ROBOT, ModuleName.TMRobot, "TM Robot is busy, please wait a minute");
                return false;
            }
            else if (Status == RState.Failed || Status == RState.Timeout)
            {
                LOG.Write(eEvent.ERR_TM_ROBOT, ModuleName.TMRobot, "TM Robot 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 SIASUN TM Robot: {RevMsg}, while {_currentOP}");

            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 (_currentOP)
            {
                case OPStep.Goto:
                case OPStep.MoveTo:
                case OPStep.Pick:
                case OPStep.PickExtend:
                case OPStep.PickRetract:
                case OPStep.Place:
                case OPStep.PlaceExtent:
                case OPStep.PlaceRetract:
                    {
                        if (RevMsg.Trim() == "_RDY" || (RevMsg.Contains("_RDY") && !RevMsg.Contains("_ERR")))
                        {
                            _currentOP = OPStep.Idle;
                            _status = RState.End;
                            if (RevMsg.Contains("_EVENT"))
                            {
                                GetEventMsg(RevMsg);
                            }
                        }
                        else
                        {
                            ReportWrongMsg(RevMsg);
                        }

                        if (_currentOP != OPStep.PickExtend && _currentOP != OPStep.PlaceExtent)
                        {
                            SetRobotMovingInfo(RobotAction.None, Hand.Both, ModuleName.TMRobot);
                        }

                    }
                    break;
                case OPStep.Home:
                    {

                        if (RevMsg.Trim() == "_RDY")
                        {
                            //CheckLoad(Hand.Blade1);
                            _currentOP = OPStep.Idle;
                            _status = RState.End;
                            _IsHomed = true;
                        }
                        else
                            ReportWrongMsg(RevMsg);
                    }
                    break;
                case OPStep.CheckLoad_ArmA:
                    {
                        if (_rex_check_load.IsMatch(RevMsg))
                        {
                            GetCheckLoadResult(RevMsg);

                            CheckLoad(Hand.Blade2);
                        }
                        else
                            ReportWrongMsg(RevMsg);
                    }
                    break;
                case OPStep.CheckLoad_ArmB:
                    {
                        if (_rex_check_load.IsMatch(RevMsg))
                        {
                            GetCheckLoadResult(RevMsg);

                            _currentOP = OPStep.Idle;
                            _status = RState.End;
                            _IsHomed = true;
                        }
                    }
                    break;
                case OPStep.QueryAwc:
                    QueryAwcData(RevMsg);
                    break;
                default:
                    if (!RevMsg.Contains("_EVENT"))
                        ReportWrongMsg(RevMsg);
                    else
                        GetEventMsg(RevMsg);
                    break;
            }
        }

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

            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 OnErrorHappen(ErrorEventArgs args)
        {
            Singleton<RouteManager>.Instance.TM.PostMsg(TMEntity.MSG.Error);
            LOG.Write(eEvent.ERR_TM_ROBOT, ModuleName.TMRobot, $"SIASUN TM Robot Error: {args.Reason} while {_currentOP}");
        }

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

        private void ErrorMessageHandler(string errCode)
        {
            int ErrCode;
            string ErrorInfo;
            if (int.TryParse(errCode, out ErrCode))
            {
                switch (ErrCode)
                {
                    // 系统及硬件错误信息
                    case 901:
                        ErrorInfo = $"_Err {errCode}: 主电柜急停启动";
                        break;
                    case 902:
                        ErrorInfo = $"_Err {errCode}: 示教盒急停启动";
                        break;
                    case 862:
                        ErrorInfo = $"_Err {errCode}: 驱动器 RDY 信号断开";
                        break;

                    // 执行错误信息
                    case 3001:
                        ErrorInfo = $"_Err {errCode}: 系统发生碰撞，按取消恢复";
                        break;
                    case 7300:
                        ErrorInfo = $"_Err {errCode}: 旋转信号不允许";
                        break;
                    case 7301:
                        ErrorInfo = $"_Err {errCode}: 伸缩信号不允许";
                        break;
                    case 2200:
                        ErrorInfo = $"_Err {errCode}: 输出端口 NO.不存在";
                        break;
                    case 3100:
                        ErrorInfo = $"_Err {errCode}: 关节 N 位置超界";
                        break;
                    case 3120:
                        ErrorInfo = $"_Err {errCode}: 关节 N 速度超界";
                        break;
                    case 100:
                        ErrorInfo = $"_Err {errCode}: 手臂电源上电失败（手臂电源未打开）";
                        break;

                    // 通信错误信息
                    case 7307:
                        ErrorInfo = $"_Err {errCode}: GOTO 工位号超范围";
                        break;
                    case 7308:
                        ErrorInfo = $"_Err {errCode}: 不支持的传感器类型";
                        break;
                    case 7312:
                        ErrorInfo = $"_Err {errCode}: PICK工位号超范围";
                        break;
                    case 7313:
                        ErrorInfo = $"_Err {errCode}: PLACE工位号超范围";
                        break;
                    case 7314:
                        ErrorInfo = $"_Err {errCode}: XFER工位号超范围";
                        break;
                    case 7315:
                        ErrorInfo = $"_Err {errCode}: REMOVE不支持的IO类型";
                        break;
                    case 7316:
                        ErrorInfo = $"_Err {errCode}: RQ INTLCK不识别的参数";
                        break;
                    case 7317:
                        ErrorInfo = $"_Err {errCode}: RQ IO不识别的参数";
                        break;
                    case 7319:
                        ErrorInfo = $"_Err {errCode}: RQ STN工位号超范围";
                        break;
                    case 7320:
                        ErrorInfo = $"_Err {errCode}: wafre(WAF_SEN)参数未设置";
                        break;
                    case 7321:
                        ErrorInfo = $"_Err {errCode}: wafex(RETRACT_PIN)参数未设置";
                        break;
                    case 7322:
                        ErrorInfo = $"_Err {errCode}: svlv(SBIT_SVLV_SEN)参数未设置";
                        break;
                    case 7323:
                        ErrorInfo = $"_Err {errCode}: ens(EX_ENABLE)参数未设置";
                        break;
                    case 7324:
                        ErrorInfo = $"_Err {errCode}: RQ命令不支持的参数";
                        break;
                    case 7325:
                        ErrorInfo = $"_Err {errCode}: SET INTLOCK WAF_SEN参数错";
                        break;
                    case 7326:
                        ErrorInfo = $"_Err {errCode}: SET INTLOCK RZ参数错";
                        break;
                    case 7327:
                        ErrorInfo = $"_Err {errCode}: SET INTLOCK参数错";
                        break;
                    case 7328:
                        ErrorInfo = $"_Err {errCode}: SET IO ECHO参数错";
                        break;
                    case 7329:
                        ErrorInfo = $"_Err {errCode}: SET IO STATE不支持";
                        break;
                    case 7330:
                        ErrorInfo = $"_Err {errCode}: SET IO不支持的参数";
                        break;
                    case 7331:
                        ErrorInfo = $"_Err {errCode}: SET STN工位号超范围";
                        break;
                    case 7332:
                        ErrorInfo = $"_Err {errCode}: 手臂参数读取错误";
                        break;
                    case 7333:
                        ErrorInfo = $"_Err {errCode}: WAF_SEN不识别的参数";
                        break;
                    case 7334:
                        ErrorInfo = $"_Err {errCode}: SET不支持的传感器类型";
                        break;
                    case 7335:
                        ErrorInfo = $"_Err {errCode}: SET 指令输入不完整";
                        break;
                    case 7336:
                        ErrorInfo = $"_Err {errCode}: STORE IO命令不支持该参数";
                        break;
                    case 7337:
                        ErrorInfo = $"_Err {errCode}: STORE LOAD命令不支持该参数";
                        break;
                    case 7338:
                        ErrorInfo = $"_Err {errCode}: STORE STN指令工位号大于20";
                        break;
                    case 7339:
                        ErrorInfo = $"_Err {errCode}: STORE STN命令手臂参数错误";
                        break;
                    case 7340:
                        ErrorInfo = $"_Err {errCode}: STORE不支持的传感器类型";
                        break;
                    case 7341:
                        ErrorInfo = $"_Err {errCode}: STORE指令输入不完整";
                        break;
                    case 7342:
                        ErrorInfo = $"_Err {errCode}: 无法识别的命令";
                        break;
                    case 7343:
                        ErrorInfo = $"_Err {errCode}: HOME参数未指定";
                        break;
                    case 7344:
                        ErrorInfo = $"_Err {errCode}: GOTO指令R轴参数未指定";
                        break;
                    case 7345:
                        ErrorInfo = $"_Err {errCode}: GOTO指令Z轴参数未指定";
                        break;
                    case 7346:
                        ErrorInfo = $"_Err {errCode}: ARM参数错误";
                        break;
                    case 7347:
                        ErrorInfo = $"_Err {errCode}: GOTO指令未指定参数";
                        break;
                    case 7349:
                        ErrorInfo = $"_Err {errCode}: MOVE指令未指定模式或轴";
                        break;
                    case 7350:
                        ErrorInfo = $"_Err {errCode}: MOVE 指令中字段名字错";
                        break;
                    case 7351:
                        ErrorInfo = $"_Err {errCode}: PICK未指定参数";
                        break;
                    case 7352:
                        ErrorInfo = $"_Err {errCode}: PLACE未指定参数";
                        break;
                    case 7353:
                        ErrorInfo = $"_Err {errCode}: REMOVE未指定参数";
                        break;
                    case 7354:
                        ErrorInfo = $"_Err {errCode}: 指令执行未结束";
                        break;
                    case 7355:
                        ErrorInfo = $"_Err {errCode}: GOTO指令未指定工位号";
                        break;
                    case 7356:
                        ErrorInfo = $"_Err {errCode}: PICK指令未指定工位号";
                        break;
                    case 7357:
                        ErrorInfo = $"_Err {errCode}: PLACE指令未指定工位号";
                        break;
                    case 7358:
                        ErrorInfo = $"_Err {errCode}: ABS未指定数值";
                        break;
                    case 7359:
                        ErrorInfo = $"_Err {errCode}: REL未指定数值";
                        break;
                    case 7360:
                        ErrorInfo = $"_Err {errCode}: 没有指定主程序";
                        break;
                    case 7361:
                        ErrorInfo = $"_Err {errCode}: 当前没有打开作业";
                        break;
                    case 7362:
                        ErrorInfo = $"_Err {errCode}: 当前作业不是主作业";
                        break;
                    case 7363:
                        ErrorInfo = $"_Err {errCode}: ex_ena(EX_ENABLE_SEN)参数未设置";
                        break;
                    case 7364:
                        ErrorInfo = $"_Err {errCode}: stable(STABLE_ SIGNAL)参数未设置";
                        break;
                    case 7365:
                        ErrorInfo = $"_Err {errCode}: VIA参数设置有误";
                        break;
                    case 7366:
                        ErrorInfo = $"_Err {errCode}: 系统处于非启动状态";
                        break;
                    case 7367:
                        ErrorInfo = $"_Err {errCode}: extend(EX_SIGNAL)参数未设置";
                        break;
                    case 7368:
                        ErrorInfo = $"_Err {errCode}: retract(RE_ SIGNAL)参数未设置";
                        break;
                    case 7371:
                        ErrorInfo = $"_Err {errCode}: place动作前：未检测到晶圆";
                        break;
                    case 7372:
                        ErrorInfo = $"_Err {errCode}: pick动作前：检测到晶圆";
                        break;
                    case 7373:
                        ErrorInfo = $"_Err {errCode}: place动作后：检测到晶圆";
                        break;
                    case 7374:
                        ErrorInfo = $"_Err {errCode}: pick动作后：未检测到晶圆";
                        break;
                    case 7375:
                        ErrorInfo = $"_Err {errCode}: 系统未上电或当前不是执行模式";
                        break;
                    case 7376:
                        ErrorInfo = $"_Err {errCode}: 参数中存在非数字";
                        break;
                    case 7385:
                        ErrorInfo = $"_Err {errCode}: 驱动器异常停止";
                        break;
                    case 7387:
                        ErrorInfo = $"_Err {errCode}: 驱动器ID1报警";
                        break;
                    case 7388:
                        ErrorInfo = $"_Err {errCode}: 驱动器ID2报警";
                        break;
                    case 7389:
                        ErrorInfo = $"_Err {errCode}: 驱动器ID3报警";
                        break;
                    case 7391:
                        ErrorInfo = $"_Err {errCode}: AWC工位号超范围";
                        break;
                    case 7392:
                        ErrorInfo = $"_Err {errCode}: 偏差过大AWC报警";
                        break;
                    case 7393:
                        ErrorInfo = $"_Err {errCode}: 标定失败，请重新标定";
                        break;
                    case 7398:
                        ErrorInfo = $"_Err {errCode}: 触发点计算半径有误";
                        break;
                    case 7399:
                        ErrorInfo = $"_Err {errCode}: 驱动器锁存AWC数据个数有误";
                        break;
                    case 7401:
                        ErrorInfo = $"_Err {errCode}: 手指上可能有晶圆";
                        break;
                    case 7402:
                        ErrorInfo = $"_Err {errCode}: 手指上可能无晶圆";
                        break;
                    case 7403:
                        ErrorInfo = $"_Err {errCode}: load当前状态为ON，不正确";
                        break;
                    case 7404:
                        ErrorInfo = $"_Err {errCode}: load当前状态为OFF，不正确";
                        break;
                    case 7405:
                        ErrorInfo = $"_Err {errCode}: 当前slot不存在！";
                        break;
                    case 7495:
                        ErrorInfo = $"_Err {errCode}: ID1码盘反馈错误";
                        break;
                    case 7496:
                        ErrorInfo = $"_Err {errCode}: ID2码盘反馈错误";
                        break;
                    case 7497:
                        ErrorInfo = $"_Err {errCode}: ID3码盘反馈错误";
                        break;
                    default:
                        ErrorInfo = $"_Err {errCode}: 不能识别的错误码";
                        break;

                }

                LOG.Write(eEvent.ERR_TM_ROBOT, ModuleName.TMRobot, ErrorInfo);
            }
            else
            {
                LOG.Write(eEvent.ERR_TM_ROBOT, ModuleName.TMRobot, $"Try Parse the receive error code faild:{errCode}");
            }
        }

        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}";
            blockingCollection.Add(new RobotAnimationData(action,hand,target));
        }

        private void GetEventMsg(string revMsg)
        {
            //在包含数据的前提下
            switch (_robotMoveInfo.Action)
            {
                case RobotAction.Picking:
                case RobotAction.Placing:
                case RobotAction.Extending:
                case RobotAction.Retracting:
                    if (_rex_event_offset.IsMatch(revMsg))
                    {
                        //offset_x = _rex_event_getoffset.Match(revMsg).Value.Split(' ')[0];
                        //offset_y = _rex_event_getoffset.Match(revMsg).Value.Split(' ')[1];
                    }
                    break;
                default:
                    break;
            }
        }

        private void QueryAwcData(string revMsg)
        {
            //不沾包
            if (revMsg.Trim() == "_RDY")
            {
                if (_HasReceiveMsg)
                {
                    _currentOP = OPStep.Idle;
                    _status = RState.End;
                }
                else
                {
                    _status = RState.Failed;
                    LOG.Write(eEvent.ERR_TM_ROBOT, ModuleName.TMRobot, $"Query Awc failed because not find valided data");
                }
            }
            //单条查询处理
            GetAwcMsg(revMsg);
            //沾包
            if (revMsg.Trim() != "_RDY" && revMsg.Contains("_RDY"))
            {
                if (!revMsg.Contains("_ERR"))
                {
                    foreach (string msg in revMsg.Split('\n'))
                        GetAwcMsg(msg);
                    _currentOP = OPStep.Idle;
                    _status = RState.End;
                }
                else
                {
                    foreach (string msg in revMsg.Split('\n'))
                        if (msg.Contains("_ERR"))
                            ErrorMessageHandler(_rex_error_code.Match(revMsg.Trim()).Value);

                    _status = RState.Failed;
                }
            }
        }

        private void GetAwcMsg(string revMsg)
        {
            revMsg = revMsg.Trim();
            if (_rex_query_awc.IsMatch(revMsg))
            {
                _HasReceiveMsg = true;
                string offset_r_t = _rex_query_getoffset.Match(revMsg).Value;
                //最大仅6位 不超过int范围
                int offset_r;
                int offset_t;
                if (int.TryParse(offset_r_t.Split(' ')[0], out offset_r) && int.TryParse(offset_r_t.Split(' ')[1], out offset_t))
                {
                    // 9/26 新松暂未提供转换公式 暂时使用相关数据
                    offset_x = offset_r * Math.Cos(offset_t);
                    offset_y = offset_r * Math.Sin(offset_t);
                }
                else
                {
                    LOG.Write(eEvent.ERR_TM_ROBOT, ModuleName.TMRobot, $"TM Robot returned illegal offset data! Raw Data:{revMsg}");
                }
            }
            else if(!_HasReceiveMsg)
            {
                _status = RState.Failed;
                LOG.Write(eEvent.ERR_TM_ROBOT, ModuleName.TMRobot, $"The awc parameter format returned by TM Robot is incorrect! Raw Data:{revMsg}");
            }
        }
    }
    
}