测试贴合可行版本

TeamAAS-VM/Core/Management.cs

@@ -1,63 +1,30 @@
-using Basler.Pylon;
-using Cognex.VisionPro;
-using Cognex.VisionPro.ToolBlock;
-using CSScripting;
-using iTextSharp.testutils;
-using MathNet.Numerics.Distributions;
+using Cognex.VisionPro.ToolBlock;
 using MathNet.Numerics.LinearAlgebra;
-using MathNet.Numerics.RootFinding;
-using NPOI.SS.Formula.Functions;
-using NPOI.Util;
-using Opc.Ua;
 using OpenCvSharp;
-using OpenCvSharp.Flann;
 using Prism.Events;
 using Prism.Ioc;
 using Prism.Mvvm;
 using Prism.Regions;
-using Prism.Services.Dialogs;
 using System;
-using System.Collections;
 using System.Collections.Generic;
 using System.Collections.ObjectModel;
-using System.ComponentModel;
-using System.Diagnostics;
 using System.Drawing;
 using System.IO;
 using System.Linq;
-using System.Net.Sockets;
-using System.Reflection.Metadata;
-using System.Security.Cryptography;
 using System.Text;
 using System.Threading;
 using System.Threading.Tasks;
-using System.Windows;
-using System.Windows.Documents;
-using System.Windows.Interop;
 using System.Windows.Media;
-using System.Windows.Media.Media3D;
-using Team.FFFeederService;
 using Team.FFFeederService.Interfaces;
-using TeamAAS_VP;
 using TeamAAS_VP.Core.PLCs;
-using TeamAAS_VP.Core.Robots;
 using TeamAAS_VP.Core.ScrewDriver;
-using TeamAAS_VP.Data;
 using TeamAAS_VP.Enums;
 using TeamAAS_VP.Events;
 using TeamAAS_VP.Interfaces;
 using TeamAAS_VP.Models;
-using TeamAAS_VP.Models.Calibration;
-using TeamAAS_VP.Models.Feeder;
-using TeamAAS_VP.Models.Product;
 using TeamAAS_VP.Resources.Languages;
-using TeamAAS_VP.Services;
-using TeamAAS_VP.ViewModels.Home;
 using TouchSocket.Core;
 using TouchSocket.Sockets;
-using static MaterialDesignThemes.Wpf.Theme.ToolBar;
-using static Org.BouncyCastle.Math.EC.ECCurve;
-using static System.Windows.Forms.AxHost;
 
 namespace TeamAAS_VP.Core
 {
@@ -1170,7 +1137,7 @@ namespace TeamAAS_VP.Core
         /// <summary>
         /// 二次定位相机的工具坐标
         /// </summary>
-        PointF DownTool = new PointF();
+        ToolCoord DownTool = new ToolCoord();
         double _FixedUpCameraAngle = 0;
         double _MoveCameraAngle = 0;
 
@@ -1224,7 +1191,7 @@ namespace TeamAAS_VP.Core
 
                 bool isDryMode = plc.ReadNode<bool>(addressConfig.In_DryMode.Address);
 
-                // 触发下相机拍照
+                // 触发下相机拍照--下相机拍屏幕，预定位
                 if (addressConfig.In_DowmCameraExe.Address.Contains(tuple.nodeId))
                 {
                     if (tuple.value is bool state)
@@ -1282,11 +1249,14 @@ namespace TeamAAS_VP.Core
                                     if (visionResult.IsSucceed)
                                     {
 
-                                        float toolX = (float)(visionResult.X - currentPosition.X);
-                                        float toolY = (float)(visionResult.Y - currentPosition.Y);
+                                        //float toolX = (float)(visionResult.X - currentPosition.X);
+                                        //float toolY = (float)(visionResult.Y - currentPosition.Y);
+
+                                        DowmCameraResults[0] = new OpenCvSharp.Point3f((float)visionResult.X, (float)visionResult.Y, (float)visionResult.U);
+                                        //DownTool = new PointF(toolX, toolY);
+                                        //以LCD的的上边中心创建工具坐标
+                                        DownTool.ComputeTool(currentPosition.X, currentPosition.Y, currentPosition.U, DowmCameraResults[0].X, DowmCameraResults[0].Y);
 
-                                        DowmCameraResults[cameraIndex] = new OpenCvSharp.Point3f(toolX, toolY, (float)visionResult.U);
-                                        DownTool = new PointF(toolX, toolY);
                                         await plc.WriteNodeAsync(addressConfig.Out_DownCameraStatus.Address, (Int16)1);
                                         return;
                                     }
@@ -1321,8 +1291,8 @@ namespace TeamAAS_VP.Core
                                     }
                                     // 执行视觉任务
                                     var _cts = new CancellationTokenSource();
-                                    var task1 = _remoteCommandService.ExecutePhotoGetSinglePoint(process1, null, new double[] { currentPosition.X, currentPosition.Y, currentPosition.U }, _cts.Token, 1, null);
-                                    var task2 = _remoteCommandService.ExecutePhotoGetSinglePoint(process2, null, new double[] { currentPosition.X, currentPosition.Y, currentPosition.U }, _cts.Token, 2, null);
+                                    var task1 = _remoteCommandService.ExecutePhotoGetSinglePoint(process1, null, null, _cts.Token, 1, null);
+                                    var task2 = _remoteCommandService.ExecutePhotoGetSinglePoint(process2, null, null, _cts.Token, 2, null);
                                     var visionResults = await Task.WhenAll(task1, task2);
                                     if (isDryMode)
                                     {
@@ -1334,8 +1304,13 @@ namespace TeamAAS_VP.Core
                                         float toolX = (float)(visionResults[0].X + visionResults[1].X) / 2;
                                         float toolY = (float)(visionResults[0].Y + visionResults[1].Y) / 2;
                                         _FixedUpCameraAngle = Common.CalculateAngleBetweenPointsDegrees(new PointF((float)visionResults[0].X, (float)visionResults[0].Y), new PointF((float)visionResults[1].X, (float)visionResults[1].Y));
-                                        DowmCameraResults[cameraIndex] = new OpenCvSharp.Point3f(toolX, toolY, (float)_FixedUpCameraAngle);
-                                        DownTool = new PointF(toolX, toolY);
+                                        SendTaskMessage($"屏幕中心点位：{toolX:F3}，{toolY:F4}，{_FixedUpCameraAngle:F4}", MessageLevel.Info);
+                                        SendTaskMessage($"当前机器人点位：{currentPosition.X:F3}，{currentPosition.Y:F4}，{currentPosition.U:F4}", MessageLevel.Info);
+                                        DowmCameraResults[0] = new OpenCvSharp.Point3f(toolX, toolY, (float)_FixedUpCameraAngle);
+                                        //DownTool = new PointF(toolX, toolY);
+                                        //以LCD的的上边中心创建工具坐标
+                                        DownTool.ComputeTool(currentPosition.X, currentPosition.Y, currentPosition.U, DowmCameraResults[0].X, DowmCameraResults[0].Y);
+                                        SendTaskMessage($"屏幕Tool：{DownTool.X:F3}，{DownTool.Y:F4}", MessageLevel.Info);
                                         await plc.WriteNodeAsync(addressConfig.Out_DownCameraStatus.Address, (Int16)1);
                                         return;
                                     }
@@ -1431,7 +1406,7 @@ namespace TeamAAS_VP.Core
                                         float toolY = (float)(visionResults[0].Y + visionResults[1].Y) / 2;
                                         _FixedUpCameraAngle = Common.CalculateAngleBetweenPointsDegrees(new PointF((float)visionResults[0].X, (float)visionResults[0].Y), new PointF((float)visionResults[1].X, (float)visionResults[1].Y));
                                         DowmCameraResults[cameraIndex] = new OpenCvSharp.Point3f(toolX, toolY, (float)_FixedUpCameraAngle);
-                                        DownTool = new PointF(toolX, toolY);
+                                        //DownTool = new PointF(toolX, toolY);
                                         await plc.WriteNodeAsync(addressConfig.Out_DownCameraStatus.Address, (Int16)1);
                                         return;
                                     }
@@ -1833,7 +1808,7 @@ namespace TeamAAS_VP.Core
                         }
                     }
                 }
-                // 固定相机取料拍照--外壳拍照
+                // 固定相机取料拍照--屏幕拍照,取上边沿中心（防呆作用）
                 else if (addressConfig.In_FixedCameraPickExe.Address.Contains(tuple.nodeId))
                 {
                     if (tuple.value is bool state)
@@ -1895,24 +1870,25 @@ namespace TeamAAS_VP.Core
                             }
                             if (visionResults[0].IsSucceed && visionResults[1].IsSucceed && visionResults[2].IsSucceed && visionResults[3].IsSucceed)
                             {
+                                Vector<double> p1 = Vector<double>.Build.Dense(new double[] { visionResults[0].X, visionResults[0].Y });
+                                Vector<double> p2 = Vector<double>.Build.Dense(new double[] { visionResults[1].X, visionResults[1].Y });
+                                Vector<double> p3 = Vector<double>.Build.Dense(new double[] { visionResults[2].X, visionResults[2].Y });
+                                Vector<double> p4 = Vector<double>.Build.Dense(new double[] { visionResults[3].X, visionResults[3].Y });
+
+                                //var center = RectangleCenterCalculator.CalculateByDirectMethod(new List<Vector<double>> { p1, p2, p3, p4 });
+                                var center = RectangleCenterCalculator.CalculateOptimalCenter(new List<Vector<double>> { p1, p2, p3, p4 });//0215修改，不考虑点顺序
+                                //UpCameraPutResults[1].X = (float)center.Center[0];
+                                //UpCameraPutResults[1].Y = (float)center.Center[1];
+                                //UpCameraPutResults[1].Z = (float)center.MajorAxisAngle;
+                                UpCameraPutResults[1].X = ((float)center.TopLeft[0] + (float)center.TopRight[0]) / 2;
+                                UpCameraPutResults[1].Y = ((float)center.TopLeft[1] + (float)center.TopRight[1]) / 2;
+                                UpCameraPutResults[1].Z = (float)center.MajorAxisAngle;
+                                SendTaskMessage($"LCD拍照结果：X={UpCameraPutResults[1].X},Y={UpCameraPutResults[1].Y},Angle={UpCameraPutResults[1].Z}", MessageLevel.Info);
 
-                                Vector<double> p1 = Vector<double>.Build.Dense(new double[] { visionResults[1].X, visionResults[1].Y });
-                                Vector<double> p2 = Vector<double>.Build.Dense(new double[] { visionResults[2].X, visionResults[2].Y });
-                                Vector<double> p3 = Vector<double>.Build.Dense(new double[] { visionResults[3].X, visionResults[3].Y });
-                                Vector<double> p4 = Vector<double>.Build.Dense(new double[] { visionResults[0].X, visionResults[0].Y });
-
-                                LogHelper.WriteLogInfo($"P1:{visionResults[1].X};{visionResults[1].Y};");
-                                LogHelper.WriteLogInfo($"P2:{visionResults[2].X};{visionResults[2].Y};");
-                                LogHelper.WriteLogInfo($"P3:{visionResults[3].X};{visionResults[3].Y};");
-                                LogHelper.WriteLogInfo($"P4:{visionResults[0].X};{visionResults[0].Y};");
-                                var pp1 = new Point2d(visionResults[1].X, visionResults[1].Y);
-                                var pp2 = new Point2d(visionResults[2].X, visionResults[2].Y);
-                                var pp3 = new Point2d(visionResults[3].X, visionResults[3].Y);
-                                var pp4 = new Point2d(visionResults[0].X, visionResults[0].Y);
-                                var dist1 = Point2d.Distance(pp1, pp2);//长1
-                                var dist2 = Point2d.Distance(pp2, pp3);//宽1
-                                var dist3 = Point2d.Distance(pp3, pp4);//长2
-                                var dist4 = Point2d.Distance(pp1, pp4);//宽2
+                                var dist1 = RectangleCenterCalculator.Distance(center.TopLeft, center.TopRight);//长1
+                                var dist2 = RectangleCenterCalculator.Distance(center.TopRight, center.BottomRight);//宽1
+                                var dist3 = RectangleCenterCalculator.Distance(center.BottomLeft, center.BottomRight);//长2
+                                var dist4 = RectangleCenterCalculator.Distance(center.TopLeft, center.BottomLeft);//宽2
                                 LogHelper.WriteLogInfo($"dist1={dist1};dist2={dist2};dist3={dist3};dist4={dist4};");
                                 if (currentProduct.GlobalParamListCollection.Count >= 4)
                                 {
@@ -1928,7 +1904,7 @@ namespace TeamAAS_VP.Core
                                             await plc.WriteNodeAsync(addressConfig.Out_FixedCameraPickStatus.Address, (Int16)2);
                                             return;
                                         }
-                                        SendTaskMessage($"屏幕长度：长1={dist1},长2={dist3}，para1={length},para2={limtDown},para3={LimtUp}", MessageLevel.Info);
+                                        SendTaskMessage($"屏幕长度防呆：长1={dist1},长2={dist3}，para1={length},para2={limtDown},para3={LimtUp}", MessageLevel.Info);
                                     }
                                     param = currentProduct.GlobalParamListCollection.FirstOrDefault(p => p.Number == 3);
                                     if (param != null)//宽
@@ -1942,15 +1918,58 @@ namespace TeamAAS_VP.Core
                                             await plc.WriteNodeAsync(addressConfig.Out_FixedCameraPickStatus.Address, (Int16)2);
                                             return;
                                         }
-                                        SendTaskMessage($"屏幕宽度：宽1={dist2},宽2={dist4}，para1={length},para2={limtDown},para3={LimtUp}", MessageLevel.Info);
+                                        SendTaskMessage($"屏幕宽度防呆：宽1={dist2},宽2={dist4}，para1={length},para2={limtDown},para3={LimtUp}", MessageLevel.Info);
                                     }
-                                }
 
-                                var center = RectangleCenterCalculator.CalculateByPCAWithKnownOrder(new List<Vector<double>> { p1, p2, p3, p4 });
-                                UpCameraPutResults[1].X = (float)center.Center[0];
-                                UpCameraPutResults[1].Y = (float)center.Center[1];
-                                UpCameraPutResults[1].Z = (float)center.MajorAxisAngle;
-                                SendTaskMessage($"LCD拍照结果：X={UpCameraPutResults[1].X},Y={UpCameraPutResults[1].Y},Angle={UpCameraPutResults[1].Z}", MessageLevel.Info);
+                                    if (currentProduct.GlobalParamListCollection.Count >= 7)
+                                    {
+                                        param = currentProduct.GlobalParamListCollection.FirstOrDefault(p => p.Number == 4);
+                                        if (param != null)//X防呆
+                                        {
+                                            var Limt_val = Double.Parse(param.Param1);
+                                            var limtDown = Double.Parse(param.Param2);
+                                            var LimtUp = Double.Parse(param.Param3);
+                                            var dist = UpCameraPutResults[1].X - UpCameraPutResults[0].X;
+                                            if (!(((Limt_val + limtDown) <= dist) && (dist <= (Limt_val + LimtUp))))
+                                            {
+                                                SendTaskMessage($"贴合X防呆超限：X实际差值={dist},X允许差值={Limt_val},下限={limtDown},上限={LimtUp}", MessageLevel.Info);
+                                                await plc.WriteNodeAsync(addressConfig.Out_FixedCameraPickStatus.Address, (Int16)2);
+                                                return;
+                                            }
+                                            SendTaskMessage($"贴合X防呆：X实际差值={dist},X允许差值={Limt_val},下限={limtDown},上限={LimtUp}", MessageLevel.Info);
+                                        }
+                                        param = currentProduct.GlobalParamListCollection.FirstOrDefault(p => p.Number == 5);
+                                        if (param != null)//Y防呆
+                                        {
+                                            var Limt_val = Double.Parse(param.Param1);
+                                            var limtDown = Double.Parse(param.Param2);
+                                            var LimtUp = Double.Parse(param.Param3);
+                                            var dist = UpCameraPutResults[1].Y - UpCameraPutResults[0].Y;
+                                            if (!(((Limt_val + limtDown) <= dist) && (dist <= (Limt_val + LimtUp))))
+                                            {
+                                                SendTaskMessage($"贴合Y防呆超限：Y实际差值={dist},Y允许差值={Limt_val},下限={limtDown},上限={LimtUp}", MessageLevel.Info);
+                                                await plc.WriteNodeAsync(addressConfig.Out_FixedCameraPickStatus.Address, (Int16)2);
+                                                return;
+                                            }
+                                            SendTaskMessage($"贴合Y防呆：Y实际差值={dist},Y允许差值={Limt_val},下限={limtDown},上限={LimtUp}", MessageLevel.Info);
+                                        }
+                                        param = currentProduct.GlobalParamListCollection.FirstOrDefault(p => p.Number == 6);
+                                        if (param != null)//U防呆
+                                        {
+                                            var Limt_val = Double.Parse(param.Param1);
+                                            var limtDown = Double.Parse(param.Param2);
+                                            var LimtUp = Double.Parse(param.Param3);
+                                            var dist = UpCameraPutResults[1].Z - UpCameraPutResults[0].Z;
+                                            if (!(((Limt_val + limtDown) <= dist) && (dist <= (Limt_val + LimtUp))))
+                                            {
+                                                SendTaskMessage($"贴合U防呆超限：U实际差值={dist},X允许差值={Limt_val},下限={limtDown},上限={LimtUp}", MessageLevel.Info);
+                                                await plc.WriteNodeAsync(addressConfig.Out_FixedCameraPickStatus.Address, (Int16)2);
+                                                return;
+                                            }
+                                            SendTaskMessage($"贴合U防呆：U实际差值={dist},X允许差值={Limt_val},下限={limtDown},上限={LimtUp}", MessageLevel.Info);
+                                        }
+                                    }
+                                }
                             }
                             else
                             {
@@ -1968,7 +1987,7 @@ namespace TeamAAS_VP.Core
                         }
                     }
                 }
-                // 固定相机锁附/组装拍照--屏幕拍照
+                // 固定相机锁附/组装拍照--外壳拍照
                 else if (addressConfig.In_FixedCameraPutExe.Address.Contains(tuple.nodeId))
                 {
                     if (tuple.value is bool state)
@@ -2029,24 +2048,24 @@ namespace TeamAAS_VP.Core
                             }
                             if (visionResults[0].IsSucceed && visionResults[1].IsSucceed && visionResults[2].IsSucceed && visionResults[3].IsSucceed)
                             {
+                                Vector<double> p1 = Vector<double>.Build.Dense(new double[] { visionResults[0].X, visionResults[0].Y });
+                                Vector<double> p2 = Vector<double>.Build.Dense(new double[] { visionResults[1].X, visionResults[1].Y });
+                                Vector<double> p3 = Vector<double>.Build.Dense(new double[] { visionResults[2].X, visionResults[2].Y });
+                                Vector<double> p4 = Vector<double>.Build.Dense(new double[] { visionResults[3].X, visionResults[3].Y });
+
+                                //var center = RectangleCenterCalculator.CalculateByDirectMethod(new List<Vector<double>> { p1, p2, p3, p4 });
+                                var center = RectangleCenterCalculator.CalculateOptimalCenter(new List<Vector<double>> { p1, p2, p3, p4 });//0215修改，不考虑点顺序
+                                //UpCameraPutResults[0].X = (float)center.Center[0];
+                                //UpCameraPutResults[0].Y = (float)center.Center[1];
+                                UpCameraPutResults[0].X = ((float)center.TopLeft[0] + (float)center.TopRight[0]) / 2;
+                                UpCameraPutResults[0].Y = ((float)center.TopLeft[1] + (float)center.TopRight[1]) / 2;
+                                UpCameraPutResults[0].Z = (float)center.MajorAxisAngle;
+                                SendTaskMessage($"产品拍照结果：X={UpCameraPutResults[0].X},Y={UpCameraPutResults[0].Y},Angle={UpCameraPutResults[0].Z}", MessageLevel.Info);
 
-                                Vector<double> p1 = Vector<double>.Build.Dense(new double[] { visionResults[1].X, visionResults[1].Y });
-                                Vector<double> p2 = Vector<double>.Build.Dense(new double[] { visionResults[2].X, visionResults[2].Y });
-                                Vector<double> p3 = Vector<double>.Build.Dense(new double[] { visionResults[3].X, visionResults[3].Y });
-                                Vector<double> p4 = Vector<double>.Build.Dense(new double[] { visionResults[0].X, visionResults[0].Y });
-
-                                LogHelper.WriteLogInfo($"P1:{visionResults[1].X};{visionResults[1].Y};");
-                                LogHelper.WriteLogInfo($"P2:{visionResults[2].X};{visionResults[2].Y};");
-                                LogHelper.WriteLogInfo($"P3:{visionResults[3].X};{visionResults[3].Y};");
-                                LogHelper.WriteLogInfo($"P4:{visionResults[0].X};{visionResults[0].Y};");
-                                var pp1 = new Point2d(visionResults[1].X, visionResults[1].Y);
-                                var pp2 = new Point2d(visionResults[2].X, visionResults[2].Y);
-                                var pp3 = new Point2d(visionResults[3].X, visionResults[3].Y);
-                                var pp4 = new Point2d(visionResults[0].X, visionResults[0].Y);
-                                var dist1 = Point2d.Distance(pp1, pp2);//长1
-                                var dist2 = Point2d.Distance(pp2, pp3);//宽1
-                                var dist3 = Point2d.Distance(pp3, pp4);//长2
-                                var dist4 = Point2d.Distance(pp1, pp4);//宽2
+                                var dist1 = RectangleCenterCalculator.Distance(center.TopLeft, center.TopRight);//长1
+                                var dist2 = RectangleCenterCalculator.Distance(center.TopRight, center.BottomRight);//宽1
+                                var dist3 = RectangleCenterCalculator.Distance(center.BottomLeft, center.BottomRight);//长2
+                                var dist4 = RectangleCenterCalculator.Distance(center.TopLeft, center.BottomLeft);//宽2
                                 LogHelper.WriteLogInfo($"dist1={dist1};dist2={dist2};dist3={dist3};dist4={dist4};");
                                 if (currentProduct.GlobalParamListCollection.Count >= 4)
                                 {
@@ -2062,7 +2081,7 @@ namespace TeamAAS_VP.Core
                                             await plc.WriteNodeAsync(addressConfig.Out_FixedCameraPutStatus.Address, (Int16)2);
                                             return;
                                         }
-                                        SendTaskMessage($"外壳长度：长1={dist1},长2={dist3}，para1={length},para2={limtDown},para3={LimtUp}", MessageLevel.Info);
+                                        SendTaskMessage($"外壳长度防呆：长1={dist1},长2={dist3}，para1={length},para2={limtDown},para3={LimtUp}", MessageLevel.Info);
                                     }
                                     param = currentProduct.GlobalParamListCollection.FirstOrDefault(p => p.Number == 1);
                                     if (param != null)//宽
@@ -2076,15 +2095,9 @@ namespace TeamAAS_VP.Core
                                             await plc.WriteNodeAsync(addressConfig.Out_FixedCameraPutStatus.Address, (Int16)2);
                                             return;
                                         }
-                                        SendTaskMessage($"外壳宽度：宽1={dist2},宽2={dist4}，para1={length},para2={limtDown},para3={LimtUp}", MessageLevel.Info);
+                                        SendTaskMessage($"外壳宽度防呆：宽1={dist2},宽2={dist4}，para1={length},para2={limtDown},para3={LimtUp}", MessageLevel.Info);
                                     }
                                 }
-
-                                var center = RectangleCenterCalculator.CalculateByPCAWithKnownOrder(new List<Vector<double>> { p1, p2, p3, p4 });
-                                UpCameraPutResults[0].X = (float)center.Center[0];
-                                UpCameraPutResults[0].Y = (float)center.Center[1];
-                                UpCameraPutResults[0].Z = (float)center.MajorAxisAngle;
-                                SendTaskMessage($"产品拍照结果：X={UpCameraPutResults[0].X},Y={UpCameraPutResults[0].Y},Angle={UpCameraPutResults[0].Z}", MessageLevel.Info);
                             }
                             else
                             {
@@ -2204,7 +2217,7 @@ namespace TeamAAS_VP.Core
                             if (isDryMode)//空跑模式
                             {
                                 var point1 = currentProduct.ScrewPoints[0].Clone();
-                                SendTaskMessage($"点：1，{point1.X_Position:F3}，{point1.Y_Position:F4}，{point1.U_Position:F4}", MessageLevel.Info);
+                                SendTaskMessage($"空跑模式点：1，{point1.X_Position:F3}，{point1.Y_Position:F4}，{point1.U_Position:F4}", MessageLevel.Info);
                                 await point1.WriteToPlcAddress(addressConfig.Out_Screw, plc);
                                 await plc.WriteNodeAsync(addressConfig.Out_WorkNum.Address, (Int16)1);
                                 await plc.WriteNodeAsync(addressConfig.Out_CameraDone.Address, (Int16)1);
@@ -2222,26 +2235,68 @@ namespace TeamAAS_VP.Core
                             }
                             var currentPosition = robot.GetRobotPos();
 
+                            //已经通过下相机拍屏幕两角获取--DownTool
                             //以LCD的中线创建工具坐标
-                            ToolCoord toolCoord = new ToolCoord();
-                            toolCoord.ComputeTool(currentPosition.X, currentPosition.Y, currentPosition.U, UpCameraPutResults[1].X, UpCameraPutResults[1].Y);
-
+                            //ToolCoord toolCoord = new ToolCoord();
+                            //toolCoord.ComputeTool(currentPosition.X, currentPosition.Y, currentPosition.U, UpCameraPutResults[1].X, UpCameraPutResults[1].Y);
 
-                            //计算放料角度
-                            double angle = currentPosition.U - UpCameraPutResults[1].Z + UpCameraPutResults[0].Z;
+                            //添加新的补偿算法--20260213
+                            double offset_X = 0, offset_Y = 0, offset_U = 0;
+                            var current = _productService.GetCurrentProduct();
+                            if (current.ScrewPoints == null || current.ScrewPoints.Count <= 0)
+                            {
+                                SendTaskMessage($"未找到任务贴合补偿参数", MessageLevel.Alarm);
+                                await plc.WriteNodeAsync(addressConfig.Out_DownCameraStatus.Address, (Int16)2);
+                                return;
+                            }
+                            offset_X = current.ScrewPoints[0].X_Offset;
+                            offset_Y = current.ScrewPoints[0].Y_Offset;
+                            offset_U = current.ScrewPoints[0].U_Offset;
+                            SendTaskMessage($"放料补偿：X={offset_X},Y={offset_Y}，U={offset_U}", MessageLevel.Info);
+                            //利用补偿偏差算出，补偿后的壳体中心，p_new
+                            ToolCoord toolCoord2 = new ToolCoord();
+                            toolCoord2.SetTool(offset_X, offset_Y);
+                            var p_new_Vector = Vector<double>.Build.Dense(new double[] { UpCameraPutResults[0].X, UpCameraPutResults[0].Y });
+                            var p_new = toolCoord2.GetToolnCoord(p_new_Vector, UpCameraPutResults[0].Z);
+                            SendTaskMessage($"放料原点：1，{UpCameraPutResults[0].X:F3}，{UpCameraPutResults[0].Y:F4}，{UpCameraPutResults[0].Z:F4}", MessageLevel.Info);
+                            SendTaskMessage($"放料补偿点：2，{p_new[0]:F3}，{p_new[1]:F4}，{UpCameraPutResults[0].Z:F4}", MessageLevel.Info);
+
+                            //计算放料角度--带入下相机的角度
+                            double angle = currentPosition.U - DowmCameraResults[0].Z + UpCameraPutResults[0].Z + offset_U;
 
                             //将产品的中心点坐标转换为Tool0下的坐标
                             var point = currentProduct.ScrewPoints[0].Clone();
-                            var pn = Vector<double>.Build.Dense(new double[] { UpCameraPutResults[0].X, UpCameraPutResults[0].Y });
-                            var p0 = toolCoord.GetTool0Coord(pn, angle);
+                            //var pn = Vector<double>.Build.Dense(new double[] { UpCameraPutResults[0].X, UpCameraPutResults[0].Y });
+                            var pn = p_new;
+                            //var p0 = toolCoord.GetTool0Coord(pn, angle);
+                            var p0 = DownTool.GetTool0Coord(pn, angle);//下相机拍照tool，动态Tool
                             point.X_Position = (float)p0[0];
                             point.Y_Position = (float)p0[1];
                             point.U_Position = (float)angle;
                             PlaceActualCoord = new OpenCvSharp.Point3f(point.X_Position, point.Y_Position, point.U_Position);
-                            SendTaskMessage($"点：1，{point.X_Position:F3}，{point.Y_Position:F4}，{point.U_Position:F4}", MessageLevel.Info);
-                            await point.WriteToPlcAddress(addressConfig.Out_Screw, plc);
+                            //SendTaskMessage($"点：1，{point.X_Position:F3}，{point.Y_Position:F4}，{point.U_Position:F4}", MessageLevel.Info);
+                            SendTaskMessage($"目标点：3，{point.X_Position:F3}，{point.Y_Position:F4}，{point.U_Position:F4}", MessageLevel.Info);
+                            await point.WriteToPlcAddressEx2(addressConfig.Out_Screw, plc);//补偿参数提前代入计算，直接发最终点位
                             await plc.WriteNodeAsync(addressConfig.Out_WorkNum.Address, (Int16)1);
                             await plc.WriteNodeAsync(addressConfig.Out_CameraDone.Address, (Int16)1);
+
+
+                            //原计算方法--20260213
+                            ////计算放料角度
+                            //double angle = currentPosition.U - UpCameraPutResults[1].Z + UpCameraPutResults[0].Z;
+
+                            ////将产品的中心点坐标转换为Tool0下的坐标
+                            //var point = currentProduct.ScrewPoints[0].Clone();
+                            //var pn = Vector<double>.Build.Dense(new double[] { UpCameraPutResults[0].X, UpCameraPutResults[0].Y });
+                            //var p0 = toolCoord.GetTool0Coord(pn, angle);
+                            //point.X_Position = (float)p0[0];
+                            //point.Y_Position = (float)p0[1];
+                            //point.U_Position = (float)angle;
+                            //PlaceActualCoord = new OpenCvSharp.Point3f(point.X_Position, point.Y_Position, point.U_Position);
+                            //SendTaskMessage($"点：1，{point.X_Position:F3}，{point.Y_Position:F4}，{point.U_Position:F4}", MessageLevel.Info);
+                            //await point.WriteToPlcAddress(addressConfig.Out_Screw, plc);
+                            //await plc.WriteNodeAsync(addressConfig.Out_WorkNum.Address, (Int16)1);
+                            //await plc.WriteNodeAsync(addressConfig.Out_CameraDone.Address, (Int16)1);
                         }
                         else
                         {
@@ -2615,7 +2670,7 @@ namespace TeamAAS_VP.Core
                                     await plc.WriteNodeAsync(string.Format(addressConfig.Out_MesStatus.Address, i), (Int16)3);
                                     LogHelper.WriteLogMes($"【mes结束ADD】");
                                     SendTaskMessage($"mes结束ADD", MessageLevel.Info);
-                                    
+
                                     return;
                                 }
                                 else
@@ -2650,7 +2705,7 @@ namespace TeamAAS_VP.Core
                                     SendTaskMessage($"收到开始保压信号[{i}]...", MessageLevel.Debug);
                                     // 开始采集压力值
                                     StartPressureCollection(currentProduct.Name, code, i);
-                                    StartCtqTime[i]= DateTime.Now;
+                                    StartCtqTime[i] = DateTime.Now;
                                 }
                                 else
                                 {

TeamAAS-VM/Core/RectangleCenterCalculator.cs

@@ -97,13 +97,7 @@ namespace TeamAAS_VP.Core
             if (corners == null || corners.Count != 4)
                 throw new ArgumentException("需要4个角点");
 
-            // 这里可以添加自动排序逻辑，但既然您已知顺序，直接返回
-            // 如果未来需要自动排序，可以使用以下逻辑：
-            // 1. 找到最左边的两个点作为左边界
-            // 2. 根据Y坐标区分左上和左下
-            // 3. 计算中心点，确定其他点位置
-
-            return corners; // 假设输入已经按正确顺序
+            return SortCornersByTopBottomHeuristic(corners);
         }
 
         /// <summary>
@@ -204,7 +198,20 @@ namespace TeamAAS_VP.Core
         /// </summary>
         public static RectangleResult CalculateByPCAWithKnownOrder(List<Vector<double>> corners)
         {
+            //打印输入点以调试
+            //Console.WriteLine($"输入点 [1] : {corners[0][0]:F3}，{corners[0][1]:F3}");
+            //Console.WriteLine($"输入点 [2] : {corners[1][0]:F3}，{corners[1][1]:F3}");
+            //Console.WriteLine($"输入点 [3] : {corners[2][0]:F3}，{corners[2][1]:F3}");
+            //Console.WriteLine($"输入点 [4] : {corners[3][0]:F3}，{corners[3][1]:F3}");
+
             var orderedCorners = ValidateAndOrderCorners(corners);
+            Console.WriteLine($"");
+
+            //打印输入点以调试
+            //Console.WriteLine($"输出点 [1] : {orderedCorners[0][0]:F3}，{orderedCorners[0][1]:F3}");
+            //Console.WriteLine($"输出点 [2] : {orderedCorners[1][0]:F3}，{orderedCorners[1][1]:F3}");
+            //Console.WriteLine($"输出点 [3] : {orderedCorners[2][0]:F3}，{orderedCorners[2][1]:F3}");
+            //Console.WriteLine($"输出点 [4] : {orderedCorners[3][0]:F3}，{orderedCorners[3][1]:F3}");
 
             int dim = orderedCorners[0].Count;
             // 步骤1：使用所有点进行PCA得到初步估计
@@ -343,7 +350,7 @@ namespace TeamAAS_VP.Core
             int n = axis.Count;
             if (n == 0) throw new ArgumentException("axis must have positive dimension", nameof(axis));
 
-            // 专门处理2D：(-y, x) 是垂直向量
+            // 2D场景：(-y, x) 为垂直向量
             if (n == 2)
             {
                 var perp2 = Vector<double>.Build.DenseOfArray(new[] { -axis[1], axis[0] });
@@ -351,7 +358,7 @@ namespace TeamAAS_VP.Core
                 return perp2.Normalize(2);
             }
 
-            // 一般n维：选一个与axis不共线的标准基向量
+            // 通用n维：选一个不与axis共线的标准基向量
             int idx = 0;
             for (int i = 0; i < n; i++)
             {
@@ -367,7 +374,7 @@ namespace TeamAAS_VP.Core
         /// <summary>
         /// 辅助方法：计算两点距离
         /// </summary>
-        private static double Distance(Vector<double> a, Vector<double> b)
+        public static double Distance(Vector<double> a, Vector<double> b)
         {
             return (a - b).L2Norm();
         }
@@ -426,5 +433,91 @@ namespace TeamAAS_VP.Core
 
             return Math.Sqrt(sumSquared / 4);
         }
+
+        /// <summary>
+        /// 自动排序角点：左上 → 右上 → 右下 → 左下
+        /// </summary>
+        /// <param name="unorderedCorners">任意顺序的四个角点</param>
+        /// <returns>按标准顺序排序的角点列表</returns>
+        public static List<Vector<double>> SortCornersToRectangleOrder(List<Vector<double>> unorderedCorners)
+        {
+            if (unorderedCorners == null || unorderedCorners.Count != 4)
+                throw new ArgumentException("需要4个角点");
+
+            // 方法1：基于上下分组+水平排序的稳健方法
+            return SortCornersByTopBottomHeuristic(unorderedCorners);
+        }
+
+        /// <summary>
+        /// 方法1：基于上下分组与水平排序的稳健方法
+        /// </summary>
+        private static List<Vector<double>> SortCornersByTopBottomHeuristic(List<Vector<double>> corners)
+        {
+            if (corners == null || corners.Count != 4)
+                throw new ArgumentException("需要4个角点");
+
+            var orderHighY = BuildOrderByTopBottom(corners, topIsHigher: true);
+            var orderLowY = BuildOrderByTopBottom(corners, topIsHigher: false);
+
+            double scoreHigh = RectangleOrderScore(orderHighY);
+            double scoreLow = RectangleOrderScore(orderLowY);
+
+            return scoreHigh <= scoreLow ? orderHighY : orderLowY;
+        }
+
+        private static List<Vector<double>> BuildOrderByTopBottom(List<Vector<double>> corners, bool topIsHigher)
+        {
+            var sortedByY = topIsHigher
+                ? corners.OrderByDescending(p => p[1]).ToList()
+                : corners.OrderBy(p => p[1]).ToList();
+
+            var topCandidates = sortedByY.Take(2).ToList();
+            var bottomCandidates = sortedByY.Skip(2).Take(2).ToList();
+
+            var top = topCandidates.OrderBy(p => p[0]).ToList();
+            var bottom = bottomCandidates.OrderBy(p => p[0]).ToList();
+
+            var topLeft = top[0];
+            var topRight = top[1];
+            var bottomLeft = bottom[0];
+            var bottomRight = bottom[1];
+
+            return new List<Vector<double>> { topLeft, topRight, bottomRight, bottomLeft };
+        }
+
+        private static double RectangleOrderScore(List<Vector<double>> ordered)
+        {
+            if (ordered == null || ordered.Count != 4)
+                return double.MaxValue;
+
+            var topLeft = ordered[0];
+            var topRight = ordered[1];
+            var bottomRight = ordered[2];
+            var bottomLeft = ordered[3];
+
+            double top = Distance(topLeft, topRight);
+            double bottom = Distance(bottomLeft, bottomRight);
+            double left = Distance(topLeft, bottomLeft);
+            double right = Distance(topRight, bottomRight);
+            double diag1 = Distance(topLeft, bottomRight);
+            double diag2 = Distance(topRight, bottomLeft);
+
+            double parallelScore = Math.Abs(top - bottom) + Math.Abs(left - right) + Math.Abs(diag1 - diag2);
+
+            double xOrderPenalty = 0.0;
+            if (topLeft[0] > topRight[0]) xOrderPenalty += 1000.0;
+            if (bottomLeft[0] > bottomRight[0]) xOrderPenalty += 1000.0;
+
+            return parallelScore + xOrderPenalty;
+        }
+
+        /// <summary>
+        /// 调整角点顺序为标准矩形顺序（左上→右上→右下→左下）
+        /// </summary>
+        private static List<Vector<double>> AdjustToRectangleOrder(List<Vector<double>> corners)
+        {
+            return SortCornersByTopBottomHeuristic(corners);
+        }
+
     }
 }

TeamAAS-VM/Models/PLC/PlcPoint.cs

@@ -361,6 +361,33 @@ namespace TeamAAS_VP.Models.PLC
             return await pLC.WriteNodesAsync(nodeValues);
         }
 
+        /// <summary>
+        /// 将点位数据写入到PLC地址--移除OffsetX,OffsetY,OffsetU
+        /// </summary>
+        /// <param name="plcAddress"></param>
+        /// <param name="pLC"></param>
+        /// <returns></returns>
+        public async Task<bool> WriteToPlcAddressEx2(PlcPointAddress plcAddress, OpcUaClientPLC pLC)
+        {
+            Dictionary<string, object> nodeValues = new Dictionary<string, object>();
+            nodeValues.Add(string.Format(plcAddress.X_Position.Address, Number), X_Position);
+            nodeValues.Add(string.Format(plcAddress.X_Velocity.Address, Number), X_Velocity);
+            nodeValues.Add(string.Format(plcAddress.Y_Position.Address, Number), Y_Position);
+            nodeValues.Add(string.Format(plcAddress.Y_Velocity.Address, Number), Y_Velocity);
+            nodeValues.Add(string.Format(plcAddress.Z_Position_Start.Address, Number), Z_Position_Start + Z_Start_Offset);
+            nodeValues.Add(string.Format(plcAddress.Z_Velocity_Start.Address, Number), Z_Velocity_Start);
+            nodeValues.Add(string.Format(plcAddress.Z_Position_Stop.Address, Number), Z_Position_Stop + Z_Stop_Offset);
+            nodeValues.Add(string.Format(plcAddress.Z_Velocity_Stop.Address, Number), Z_Velocity_Stop);
+            nodeValues.Add(string.Format(plcAddress.U_Position.Address, Number), U_Position);
+            nodeValues.Add(string.Format(plcAddress.U_Velocity.Address, Number), U_Velocity);
+            nodeValues.Add(string.Format(plcAddress.R_Position.Address, Number), R_Position + R_Offset);
+            nodeValues.Add(string.Format(plcAddress.R_Velocity.Address, Number), R_Velocity);
+            nodeValues.Add(string.Format(plcAddress.Torque.Address, Number), Torque);
+            nodeValues.Add(string.Format(plcAddress.Feeder.Address, Number), Feeder);
+            nodeValues.Add(string.Format(plcAddress.ScrewProNum.Address, Number), ScrewProNum);
+            return await pLC.WriteNodesAsync(nodeValues);
+        }
+
         /// <summary>
         /// 将点位数据写入到PLC地址
         /// </summary>

TeamAAS-VM/ViewModels/MainWindowViewModel.cs

@@ -1,7 +1,4 @@
-using Basler.Pylon;
-using CSScriptLib;
-using NPOI.SS.Formula.Functions;
-using NPOI.XSSF.Streaming.Values;
+using MathNet.Numerics.LinearAlgebra;
 using Prism.Commands;
 using Prism.Events;
 using Prism.Ioc;
@@ -9,20 +6,13 @@ using Prism.Mvvm;
 using Prism.Regions;
 using Prism.Services.Dialogs;
 using System;
-using System.Collections.Generic;
 using System.Collections.ObjectModel;
-using System.Globalization;
 using System.Linq;
-using System.Text;
-using System.Text.RegularExpressions;
 using System.Threading;
 using System.Threading.Tasks;
 using System.Windows;
-using System.Windows.Controls;
-using System.Windows.Controls.Primitives;
 using System.Windows.Media;
 using Team.FFFeederService.Interfaces;
-using TeamAAS_VP;
 using TeamAAS_VP.Controls;
 using TeamAAS_VP.Core;
 using TeamAAS_VP.Data;
@@ -31,11 +21,7 @@ using TeamAAS_VP.Events;
 using TeamAAS_VP.Interfaces;
 using TeamAAS_VP.Models;
 using TeamAAS_VP.Resources.Languages;
-using TeamAAS_VP.Services;
 using TeamAAS_VP.ViewModels.Home;
-using TeamAAS_VP.Views;
-using WPFLocalizeExtension.Engine;
-using static MaterialDesignThemes.Wpf.Theme.ToolBar;
 
 namespace TeamAAS_VP.ViewModels
 {