修复OPT相机图像采集

TeamAAS-VM/Core/Cameras/OptCamera.cs

@@ -1,5 +1,6 @@
 using Cognex.VisionPro;
 using Microsoft.Win32;
+using NPOI.POIFS.Crypt.Dsig;
 using NPOI.SS.Formula.Eval;
 using SciCamera.Net;
 using System;
@@ -121,7 +122,7 @@ namespace TeamAAS_VP.Core.Cameras
 
                 if (devTlType == SciCam.SciCamTLType.SciCam_TLType_Gige)
                 {
-                    
+
                     SciCam.SCI_DEVICE_GIGE_INFO gigeDevInfo = (SciCam.SCI_DEVICE_GIGE_INFO)SciCam.ByteToStruct(device.info.gigeInfo, typeof(SciCam.SCI_DEVICE_GIGE_INFO));
                     uint ip1 = gigeDevInfo.ip;
                     int nIp1 = (int)(ip1 & 0x000000ff);
@@ -337,7 +338,7 @@ namespace TeamAAS_VP.Core.Cameras
                         throw new Exception("没有发现相机");
                     }
                 }
-                SciCam.SCI_DEVICE_INFO sCI_DEVICE_INFO= CameraInfo;
+                SciCam.SCI_DEVICE_INFO sCI_DEVICE_INFO = CameraInfo;
                 nReVal = mvCameraAcq.CreateDevice(ref sCI_DEVICE_INFO);
                 if (nReVal != SciCam.SCI_CAMERA_OK)
                 {
@@ -348,12 +349,22 @@ namespace TeamAAS_VP.Core.Cameras
                 nReVal = mvCameraAcq.OpenDevice();
                 // ch:设置触发模式为off | en:Set trigger mode as off
                 nReVal = mvCameraAcq.SetEnumValueByStringEx(SciCam.SciCamDeviceXmlType.SciCam_DeviceXml_Camera, "TriggerMode", "Off");
+                nReVal = mvCameraAcq.SetEnumValueByString("TriggerSource", "Software");
+                // ch:注册事件回调 | en:Register event callback
+                //nReVal = mvCameraAcq.RegisterPayloadCallBack(new SciCam.fnOnPayloadDelegate(ImageCallBack), IntPtr.Zero, true);
+                // ch:设置采集策略 | en:Set Grab Strategy
+                nReVal = mvCameraAcq.SetGrabStrategy(SciCam.SciCamGrabStrategy.SciCam_GrabStrategy_Latest);
+                if (nReVal != SciCam.SCI_CAMERA_OK)
+                {
+                    Console.WriteLine("Register Event CallBack fail! nRet {0}", nReVal);
+                }
                 if (!IsConnected)
                 {
                     CameraConnectChangedEvent?.Invoke(ID, true);
                 }
+                mvCameraAcq.SetGrabTimeout(2000);
                 IsConnected = mvCameraAcq.IsDeviceOpen();
-                if (IsConnected) 
+                if (IsConnected)
                 {
                     mvCameraAcq.StartGrabbing();
                 }
@@ -415,7 +426,14 @@ namespace TeamAAS_VP.Core.Cameras
                         {
                             OpenDevice();
                         }
+                        //if (!mvCameraAcq.IsGrabbing())
+                        //{
+                        //    mvCameraAcq.StartGrabbing();
+                        //}
                         //nReVal = mvCameraAcq.StartGrabbing();
+                        //mvCameraAcq.ClearPayloadBuffer();
+                        // ch:发送软触发命令 | en:Send Trigger Software command
+                        //nReVal = mvCameraAcq.SetCommandValueEx(SciCam.SciCamDeviceXmlType.SciCam_DeviceXml_Camera, "TriggerSoftware");
                         nReVal = mvCameraAcq.Grab(ref payload);
                         if (nReVal == SciCam.SCI_CAMERA_OK)
                         {
@@ -423,11 +441,12 @@ namespace TeamAAS_VP.Core.Cameras
                             mvCameraAcq.FreePayload(payload);
                             //nReVal = mvCameraAcq.StopGrabbing();
                             TotalTime = sw.Elapsed;
+                            ErrorMessage = $"{i + 1}";
                             return Image;
                         }
                         else
                         {
-                            nReVal = mvCameraAcq.FreePayload(payload);
+                            //nReVal = mvCameraAcq.FreePayload(payload);
                             //nReVal = mvCameraAcq.StopGrabbing();
                             //_operationSemaphore.Release();
                             CloseDevice();
@@ -456,6 +475,27 @@ namespace TeamAAS_VP.Core.Cameras
             }
         }
 
+        // ch:采集回调接口 | en:Grab Callback interface     
+        void ImageCallBack(IntPtr payload, IntPtr tag)
+        {
+            if (payload == IntPtr.Zero) return;
+            uint nReVal = SciCam.SCI_CAMERA_OK;
+
+            SciCam.SCI_CAM_PAYLOAD_ATTRIBUTE attribute = new SciCam.SCI_CAM_PAYLOAD_ATTRIBUTE();
+            nReVal = SciCam.PayloadGetAttribute(payload, ref attribute);
+            if (nReVal != SciCam.SCI_CAMERA_OK || !attribute.isComplete)
+            {
+                Console.WriteLine("Failed to get image attributes or frame is incomplete! nRet:{0}", nReVal);
+            }
+            else
+            {
+                Image = GetConvertedInfo(payload);
+                ImageCallbackEvent?.Invoke(Image, TotalTime, ErrorMessage);
+                //Console.WriteLine("Get One Frame: Width[{0}], Height[{1}], nFrameNum[{2}]",
+                //attribute.imgAttr.width, attribute.imgAttr.height, attribute.frameID);
+            }
+        }
+
         /// <summary>
         /// 开始采集图像
         /// </summary>

TeamAAS-VM/Core/Robots/XYZU_Robot.cs

@@ -587,7 +587,7 @@ namespace TeamAAS_VP.Core.Robots
                     }
 
                     // 判断是否至少有一个轴在最近 Timeout 时间内发生过变化（即认为还在运动）
-                    bool anyAxisMovedRecently = actNodes.Any(node => (now - lastChange[node]).TotalMilliseconds <= 1000);
+                    bool anyAxisMovedRecently = actNodes.Any(node => (now - lastChange[node]).TotalMilliseconds <= 500);
 
                     // 如果没有任何轴在最近 Timeout 时间内发生变化，则认为出现停滞异常
                     if (!anyAxisMovedRecently && actNodes.Length > 0)
@@ -602,13 +602,14 @@ namespace TeamAAS_VP.Core.Robots
                     if (isFinished)
                     {
                         StopMove();
-                        if (isError)
-                        {
-                            LogHelper.WriteLogInfo($"机器人执行移动时发生错误，位置：X={position.X},Y={position.Y},Z={position.Z},U={position.U}");
-                            return false;
-                        }
                         return true;
                     }
+                    if (isError)
+                    {
+                        StopMove();
+                        LogHelper.WriteLogInfo($"机器人执行移动时发生错误，位置：X={position.X},Y={position.Y},Z={position.Z},U={position.U}");
+                        return false;
+                    }
 
                     // 检查整体超时（防止一直有抖动导致 anyAxisMovedRecently 一直为 true）
                     if (sw.ElapsedMilliseconds > 60000) // 保守超时，一分钟，可根据需求调整
@@ -667,10 +668,10 @@ namespace TeamAAS_VP.Core.Robots
                 {
                     keyValues.Add(axis.Command.ManuPositionNode, false);
                 }
-                if (!string.IsNullOrWhiteSpace(axis.Command.StopNode))
-                {
-                    keyValues.Add(axis.Command.StopNode, true);
-                }
+                //if (!string.IsNullOrWhiteSpace(axis.Command.StopNode))
+                //{
+                //    keyValues.Add(axis.Command.StopNode, true);
+                //}
             }
             bool result = Plc.WriteNodes(keyValues);
 
@@ -678,16 +679,16 @@ namespace TeamAAS_VP.Core.Robots
             //Task.Run(async () =>
             //{
             //    await Task.Delay(100);
-            Thread.Sleep(50);
-            Dictionary<string, object> resetValues = new Dictionary<string, object>();
-            foreach (var axis in Axes)
-            {
-                if (!string.IsNullOrWhiteSpace(axis.Command.StopNode))
-                {
-                    resetValues.Add(axis.Command.StopNode, false);
-                }
-            }
-            Plc.WriteNodes(resetValues);
+            //Thread.Sleep(50);
+            //Dictionary<string, object> resetValues = new Dictionary<string, object>();
+            //foreach (var axis in Axes)
+            //{
+            //    if (!string.IsNullOrWhiteSpace(axis.Command.StopNode))
+            //    {
+            //        resetValues.Add(axis.Command.StopNode, false);
+            //    }
+            //}
+            //Plc.WriteNodes(resetValues);
             //});
             return result;
         }