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;

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

        private RState _status;
        private bool _IsHomed;
        public RState Status { get { return _status; } }
        public bool IsHomed { get { return _IsHomed; } }

        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 ? "A" : "B";

        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 const string EOF = "\r\n";

        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");
        }
        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 Goto(ModuleName station, int slot, Hand hand)
        {
            if (CheckRobotStatus() == false)
                return false;

            _currentOP = OPStep.Goto;
            _status = RState.Running;
            return _SendCommand($"GOTO N {_StationNumbers[station]} 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");
        }
        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");
        }
        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");
        }
        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");
        }
        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")
                        {
                            _currentOP = OPStep.Idle;
                            _status = RState.End;
                        }
                        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;
                default:
                    ReportWrongMsg(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}";
        }
    }
}