Add the driver of HongHuVR

Venus/Venus_RT/Devices/VCE/HongHuVR.cs

@@ -0,0 +1,346 @@
+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.LoadPorts;
+using System;
+using System.Collections.Generic;
+using System.Linq;
+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,
+        ReQueryLoad,
+    }
+    public class HongHuVR
+    {
+        private readonly AsyncSocket _socket;
+        private const string EOF = "\r";
+        private RState _status;
+        private bool _IsHomed;
+        private VRStep _currentStep = VRStep.Idle;
+        private Dictionary<ModuleName, int> _StationNumbers = new Dictionary<ModuleName, int>();
+        private RobotMoveInfo _robotMoveInfo = new RobotMoveInfo();
+        public RState Status { get { return _status; } }
+        public bool IsHomed { get { return _IsHomed; } }
+        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*");
+        public RobotMoveInfo VaccumRobotMoveInfo { get { return _robotMoveInfo; } }
+
+        public Dictionary<string, string> _error2msg = new Dictionary<string, string>()
+        {
+            { "701" , "设备检查互锁，发现无法执行"},
+            { "722" , "设备动作，但是发生异常"},
+        };
+
+        public HongHuVR()
+        {
+
+            _socket = new AsyncSocket("", EOF);
+            _socket.Connect(SC.GetStringValue($"VacRobot.IPAddress"));
+            _socket.OnDataChanged += OnReceiveMessage;
+            _socket.OnErrorHappened += OnErrorHappen;
+
+            _status = RState.Init;
+            _IsHomed = false;
+        }
+        //初始化某个轴
+        //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]} SLOT {slot} ARM {Hand2Arm(hand)}");
+        }
+
+        public bool PickExtend(ModuleName station, int slot, Hand hand)
+        {
+            if (!CheckRobotStatus())
+                return false;
+
+            _currentStep = VRStep.PickExtend;
+            _status = RState.Running;
+            SetRobotMovingInfo(RobotAction.Picking, hand, station);
+            return _SendCommand($"PICK {_StationNumbers[station]} SLOT {slot} 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.Picking, hand, station);
+            return _SendCommand($"PICK {_StationNumbers[station]} SLOT {slot} 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.Picking, hand, station);
+            return _SendCommand($"PLACE {_StationNumbers[station]} SLOT {slot} ARM {Hand2Arm(hand)}");
+        }
+        public bool PlaceExtend(ModuleName station, int slot, Hand hand)
+        {
+            if (!CheckRobotStatus())
+                return false;
+
+            _currentStep = VRStep.Place;
+            _status = RState.Running;
+            SetRobotMovingInfo(RobotAction.Picking, hand, station);
+            return _SendCommand($"PLACE {_StationNumbers[station]} SLOT {slot} 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.Picking, hand, station);
+            return _SendCommand($"PLACE {_StationNumbers[station]} SLOT {slot} 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;
+            return _SendCommand($"CHECK LOAD ARM {Hand2Arm(hand)}");
+        }
+        //public bool SETLOAD()
+        //{
+
+        //}
+        //public bool RQLOAD()
+        //{
+
+        //}
+        //public bool PICKALGN(){}
+        private bool _SendCommand(string cmd)
+        {
+            LOG.WriteSingeLine(eEvent.INFO_TM_ROBOT, ModuleName.VaccumRobot, $"Send Command to HongHu VaccumRobot: {cmd}");
+            return _socket.Write(cmd + EOF);
+        }
+
+        private bool CheckRobotStatus()
+        {
+            if (Status == RState.Init)
+            {
+                LOG.Write(eEvent.ERR_DEVICE_INFO, ModuleName.VaccumRobot, "VaccumRobot is not homed, please home first.");
+                return false;
+            }
+            else if (Status == RState.Running)
+            {
+                LOG.Write(eEvent.ERR_DEVICE_INFO, ModuleName.VaccumRobot, "VaccumRobot is busy, please wait a minute");
+                return false;
+            }
+            else if (Status == RState.Failed || Status == RState.Timeout)
+            {
+                LOG.Write(eEvent.ERR_DEVICE_INFO, ModuleName.VaccumRobot, "VaccumRobot 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.VaccumRobot, $"Receive message from HongHu VaccumRobot: {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.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.VaccumRobot);
+                    }
+                    break;
+                case VRStep.Home:
+                    {
+
+                        if (RevMsg.Trim() == "_RDY")
+                        {
+                            //CheckLoad(Hand.Blade1);
+                            _currentStep = VRStep.Idle;
+                            _status = RState.End;
+                            _IsHomed = true;
+                        }
+                        else
+                            ReportWrongMsg(RevMsg);
+                    }
+                    break;
+                case VRStep.CheckLoad_ArmA:
+                    {
+                        if (_rex_check_load.IsMatch(RevMsg))
+                        {
+                            GetCheckLoadResult(RevMsg);
+
+                            CheckLoad(Hand.Blade2);
+                        }
+                        else
+                            ReportWrongMsg(RevMsg);
+                    }
+                    break;
+                case VRStep.CheckLoad_ArmB:
+                    {
+                        if (_rex_check_load.IsMatch(RevMsg))
+                        {
+                            GetCheckLoadResult(RevMsg);
+
+                            _currentStep = VRStep.Idle;
+                            _status = RState.End;
+                            _IsHomed = true;
+                        }
+                    }
+                    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.VaccumRobot, 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.VaccumRobot, ErrorInfo);
+            }
+            else
+            {
+                LOG.Write(eEvent.ERR_DEVICE_INFO, ModuleName.VaccumRobot, $"Dictionary Not Contains error code:{errorcode}");
+            }
+        }
+
+        private void OnErrorHappen(ErrorEventArgs args)
+        {
+            Singleton<RouteManager>.Instance.TM.PostMsg(TMEntity.MSG.Error);
+            LOG.Write(eEvent.ERR_DEVICE_INFO, ModuleName.VaccumRobot, $"HongHu VaccumRobot Error: {args.Reason} while {_currentStep}");
+        }
+
+        private void ReportWrongMsg(string revMsg)
+        {
+            LOG.Write(eEvent.ERR_DEVICE_INFO, ModuleName.VaccumRobot, $"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}";
+        }
+
+    }
+}