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.ComponentModel;
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<string, int> _StationNumbers = new Dictionary<string, 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_rq_load_A = new Regex(@"LOAD\sA\s.*");
        private readonly Regex _rex_rq_load_B = new Regex(@"LOAD\sB\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"  , "槽位参数异常，检查指令中的对应参数" },
            {"403"  , "Slot编号错误，指令的参数中指定的 Slot 编号超过了有效范围" },
            {"407"  , "旋转轴超过限位，检查指令中的对应参数" },
            {"408"  , "伸展轴超过限位，检查指令中的对应参数" },
            {"409"  , "升降轴超过限位，检查指令中的对应参数" },
            {"416"  , "无法执行 CheckLoad 操作，检查指令中的对应参数" },
            {"417"  , "Offset 参数参数超限，检查指令中指定的 offset 值是否在允许范围内" },
            {"418"  , "AWC 功能检测到误差过大,检查晶圆是否过偏或检查AWC传感器标定参数" },
            {"420"  , "副臂没有在HOME位置,请检查副臂的位置，可以使用HOME ALL使得所有关节姿态回到HOME位置" },
            {"451"  , "取片后 AWC校验异常,机械臂取片后缩回时，检测到晶圆偏心过大，或者 AWC 校验失败" },
            {"550"  , "Station参数超出范围，检查站点配置参数" },
            {"551"  , "ARM 不在站点，检查当前手臂实际位置与逻辑位置" },
            {"560"  , "机械臂处于 HALT 状态，拒绝接受新的指令" },
            {"561"  , "当前机械臂状态机与指令不匹配，当前状态机不满足执行该指令的条件" },
            {"600"  , "PAligner通讯异常，检查与 PA 的通讯电缆，上电情况等" },
            {"603"  , "收到急停指令" },
            {"608"  , "机械臂急停中" },
            {"610"  , "手持示教器急停按钮按下" },
            {"692"  , "检测左 PAN 上有片，与逻辑状态不匹配" },
            {"693"  , "检测右 PAN 上有片，与逻辑状态不匹配" },
            {"694"  , "检测左 PAN 上无片，与逻辑状态不匹配" },
            {"695"  , "检测右 PAN 上有片，与逻辑状态不匹配" },
            {"700"  , "当前手臂有晶圆 ，检查指令动作要求的晶圆状态，与手臂内部存储的晶圆状态是否匹配" },
            {"701" , "当前手臂无晶圆，检查指令动作要求的晶圆状态，与手臂内部存储的晶圆状态是否匹配"},
            {"711" , "站点互锁错误，检查站点的“允许机械臂进入信号”是否就绪"},
            {"721" , "取片失败，检查手臂晶圆状态，或者晶圆检测传感器状态"},
            {"722" , "放片失败，检查手臂晶圆状态，或者晶圆检测传感器状态"},
            {"771" , "取片时 AWC 传感器检测失败，检查 AWC 传感器状态，或者 AWC 配置相关是否正确"},
            {"791" , "查询 AWC 数据失败，未曾执行过 AWC 相关功能的操作"},
            {"792" , "AWC 传感器检测失败，检查AWC传感器状态，或者AWC配置相关是否正确"},
            {"799" , "放片失败，检查手臂晶圆状态，或者晶圆检测传感器状态"},
            {"802" , "伺服报错，伺服提示异常，目前暂时无法读取伺服错误。开机盖可以读到伺服信息。一般整机断电重启。"},
            {"803" , "机械臂上电失败，HOME ALL 指令，或者其他上电操作，驱动器未能正常响应上电"},
            {"805" , "第1个伺服收敛异常，伺服没有在规定时间内运动到位置，或者停稳。"},
            {"806" , "第2个伺服收敛异常，伺服没有在规定时间内运动到位置，或者停稳。"},
            {"807" , "第3个伺服收敛异常，伺服没有在规定时间内运动到位置，或者停稳。"},
            {"808" , "第4个伺服收敛异常，伺服没有在规定时间内运动到位置，或者停稳。"},
            {"1100" , "无法安全回 HOME 操作，检查机械臂是否在安全 HOME 半径内，常见于双臂机械臂，双臂同时伸出超过安全位"},
            {"1805" , "当前处于示教器模式，检查示教器档位，一般此时处于 Teach 档位"},
            {"13004" , "PA报错，检查 PA 运行状态，PA 扫片结果等"},
        };
        private BlockingCollection<RobotAnimationData> blockingCollection = new BlockingCollection<RobotAnimationData>();
        private ModuleName _module;
        public HongHuVR(ModuleName module)
        {
            _module = module;
            _socket = new AsyncSocket("", EOF);
            _socket.Connect(SC.GetStringValue($"{_module}.IPAddress"));
            _socket.OnDataChanged += OnReceiveMessage;
            _socket.OnErrorHappened += OnErrorHappen;

            _status = RState.Init;
            _IsHomed = false;

            _StationNumbers[ModuleName.VCE1.ToString()] = SC.GetValue<int>($"{_module}.VCE1StationNumber");
            _StationNumbers[ModuleName.VCEA.ToString()] = SC.GetValue<int>($"{_module}.VCEAStationNumber");
            _StationNumbers[ModuleName.VCEB.ToString()] = SC.GetValue<int>($"{_module}.VCEBStationNumber");
            _StationNumbers[ModuleName.Aligner1.ToString()] =  SC.GetValue<int>($"{_module}.VPAStationNumber");

            _StationNumbers[ModuleName.PMA.ToString()] = SC.GetValue<int>($"{_module}.PMAStationNumber");
            _StationNumbers[ModuleName.PMB.ToString()] = SC.GetValue<int>($"{_module}.PMBStationNumber");
            _StationNumbers[ModuleName.PMC.ToString()] = SC.GetValue<int>($"{_module}.PMCStationNumber");
            _StationNumbers[ModuleName.PMD.ToString()] = SC.GetValue<int>($"{_module}.PMDStationNumber");
            _StationNumbers["CheckLoad"] = SC.GetValue<int>($"{_module}.CheckStationNumber");

            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.ToString()]} 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.ToString()]} 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.ToString()]} 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.ToString()]} 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.ToString()]} 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.ToString()]} 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.ToString()]} {_StationNumbers[tostation.ToString()]}");

        }
        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["CheckLoad"]} {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.ToString()]} 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.ToString()]} 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.ReQueryLoadB;
            _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)
        {
            string[] ResMsgs = revMsg.Split('\n');
            foreach (string revRawMsg in ResMsgs)
            {
                if (string.IsNullOrWhiteSpace(revRawMsg)) continue;
                string RevMsg = revRawMsg.Trim();
                LOG.WriteSingeLine(eEvent.INFO_TM_ROBOT, ModuleName.TMRobot, $"Receive message from TMRobot: {RevMsg}, while {_currentStep}");
                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;
                    case VRStep.ReQueryLoadA:
                        if (_rex_rq_load_A.IsMatch(RevMsg))
                        {
                            string WaferStatus = RevMsg.Split(' ')[2];
                            //LOG.Write(eEvent.WARN_DEFAULT_WARN, ModuleName.TMRobot, WaferStatus);
                            if (WaferStatus.Contains("ON") && WaferManager.Instance.CheckNoWafer(ModuleName.TMRobot, 0))
                            {
                                WaferManager.Instance.CreateWafer(ModuleName.TMRobot, 0, Aitex.Core.Common.WaferStatus.Normal);
                            }
                            if (WaferStatus.Contains("OFF"))
                            {
                                //LOG.Write(eEvent.WARN_DEFAULT_WARN, ModuleName.TMRobot, "contains off");
                                WaferManager.Instance.DeleteWafer(ModuleName.TMRobot, 0);
                            }

                        }
                        else
                        {
                            if (RevMsg.Trim() == "_RDY" || (RevMsg.Contains("_RDY") && !RevMsg.Contains("_ERR")))
                            {
                                _currentStep = VRStep.Idle;
                                _status = RState.End;
                            }
                            else
                            {
                                ReportWrongMsg(RevMsg);
                            }
                        }
                        break;

                    case VRStep.ReQueryLoadB:
                        if (_rex_rq_load_B.IsMatch(RevMsg))
                        {

                            string WaferStatus = RevMsg.Split(' ')[2];
                            //LOG.Write(eEvent.WARN_DEFAULT_WARN, ModuleName.TMRobot, WaferStatus);
                            if (WaferStatus.Contains("ON") && WaferManager.Instance.CheckNoWafer(ModuleName.TMRobot, 1))
                            {
                                WaferManager.Instance.CreateWafer(ModuleName.TMRobot, 1, Aitex.Core.Common.WaferStatus.Normal);
                            }
                            if (WaferStatus.Contains("OFF"))
                            {
                                WaferManager.Instance.DeleteWafer(ModuleName.TMRobot, 1);
                            }

                        }
                        else
                        {
                            if (RevMsg.Trim() == "_RDY" || (RevMsg.Contains("_RDY") && !RevMsg.Contains("_ERR")))
                            {
                                _currentStep = VRStep.Idle;
                                _status = RState.End;
                            }
                            else
                            {
                                ReportWrongMsg(RevMsg);
                            }
                        }
                        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}";
        }

    }
}