机器人调试修改

DeviceLibrary/deviceLibrary/urRobot/URRobotClient.cs

@@ -10,6 +10,10 @@ namespace URSoldering.DeviceLibrary
 {
     public class URRobotClient
     {
+        public delegate void SafetModePro(string mode);
+        public static event SafetModePro SafetModeFun;
+        public delegate void RobotModePro(string mode);
+        public static event RobotModePro RobotModeFun;
         private static URTcpClient listenClient = null;  
         private static int Port = 30003;
         public static string  LastMoveCMD = "";
@@ -88,9 +92,10 @@ namespace URSoldering.DeviceLibrary
                 doubleList.Add(messageSize);
                 if (maxdouble > 61)
                 {
+                    int[] array = new int[] { 55,56,57,58,59,60,94,101,103,132};
                     for (int i = 0; i < maxdouble; i++)
                     {
-                        if (i >= 55 && i <= 61)
+                        if (array.Contains(i))
                         {
                             if (reviceData.Length >= (12 + i * 8))
                             {
@@ -118,11 +123,16 @@ namespace URSoldering.DeviceLibrary
                         double ry = doubleList[60] * 1;
                         double rz = doubleList[61] * 1;
                         URPointValue newp = new URPointValue(x, y, z, rx, ry, rz);
-
-                        //string reviceMsg = ByteToStr(reviceData);
+                        int robotMode =(int) doubleList[95];
+                        int safetyMode =(int) doubleList[102];
+                        string robotModeStr = GetRobotMode(robotMode);
+                        string safetyModeStr = GetSafetyMode(safetyMode);
+                        int programState = (int)doubleList[132];
+                        SafetModeFun?.Invoke(safetyModeStr);
+                        RobotModeFun?.Invoke(robotModeStr);
                         if (!URRobotControl.IsSamePoint(newp, URRobotControl.LastPoint))
                         {
-                            LogUtil.URLInfo(LogName + "length[" + messageSize + "]，data长[" + reviceData.Length + "]坐标" + newp.ToShowStr());
+                            LogUtil.URLInfo(LogName + "length[" + messageSize + "]，data长[" + reviceData.Length + "]，【" + programState + "】【" + robotModeStr + "】【" + safetyModeStr + "】坐标" + newp.ToShowStr()+"");
                         }
                         URRobotControl.LastPoint = newp;
                     }
@@ -139,6 +149,57 @@ namespace URSoldering.DeviceLibrary
                 StopListen();
             }
         }
-
+        public static string GetRobotMode(int type)
+        {
+            switch (type)
+            {
+                case 0:
+                    return "ROBOT_MODE_DISCONNECTED";
+                case 1:
+                    return "ROBOT_MODE_CONFIRM_SAFETY";
+                case 2:
+                    return "ROBOT_MODE_BOOTING";
+                case 3:
+                    return "ROBOT_MODE_POWER_OFF";
+                case 4:
+                    return "ROBOT_MODE_POWER_ON";
+                case 5:
+                    return "ROBOT_MODE_IDLE";
+                case 6:
+                    return "ROBOT_MODE_BACKDRIVE";
+                case 7:
+                    return "ROBOT_MODE_RUNNING";
+                case 8:
+                    return "ROBOT_MODE_UPDATING_FIRMWARE";
+                default:
+                    return "";
+            }
+        }
+        public static string GetSafetyMode(int type)
+        {
+            switch (type)
+            {
+                case 1:
+                    return "SAFETY_MODE_NORMAL";
+                case 2:
+                    return "SAFETY_MODE_REDUCED";
+                case 3:
+                    return "SAFETY_MODE_PROTECTIVE_STOP";
+                case 4:
+                    return "SAFETY_MODE_RECOVERY";
+                case 5:
+                    return "SAFETY_MODE_SAFEGUARD_STOP";
+                case 6:
+                    return "SAFETY_MODE_SYSTEM_EMERGENCY_STOP";
+                case 7:
+                    return "SAFETY_MODE_ROBOT_EMERGENCY_STOP";
+                case 8:
+                    return "SAFETY_MODE_VIOLATION";
+                case 9:
+                    return "SAFETY_MODE_FAULT";
+                default:
+                    return "";
+            }
+        }
     }
 }