UPDATE

TeamAAS-VM/Core/Management.cs

@@ -1032,7 +1032,8 @@ namespace TeamAAS_VP.Core
         PointF[] DowmCameraResults= new PointF[10];
         PointF[] UpCameraPutResults = new PointF[10];
         PointF DownTool = new PointF();
-        
+        double _FixedUpCameraAngle = 0;
+        double _MoveCameraAngle = 0;
 
         /// <summary>
         /// PLC命令触发时
@@ -1110,7 +1111,7 @@ namespace TeamAAS_VP.Core
                                     {
                                         float toolX = (float)(visionResult.X - currentPosition.X);
                                         float toolY = (float)(visionResult.Y - currentPosition.Y);
-
+                                        
                                         DowmCameraResults[cameraIndex] = new PointF(toolX, toolX);
                                         DownTool= new PointF(toolX, toolX);
                                         await plc.WriteNodeAsync(addressConfig.Out_DownCameraStatus.Address, (Int16)1);
@@ -1147,13 +1148,9 @@ namespace TeamAAS_VP.Core
                                     var visionResults= await Task.WhenAll(task1, task2);
                                     if (visionResults[0].IsSucceed && visionResults[1].IsSucceed)
                                     {
-                                        float tool1X = (float)(visionResults[0].X - visionResults[0].X);
-                                        float tool1Y = (float)(visionResults[0].Y - visionResults[0].Y);
-                                        float tool2X = (float)(visionResults[1].Y - visionResults[1].Y);
-                                        float tool2Y = (float)(visionResults[1].Y - visionResults[1].Y);
-                                        float toolX = (tool1X + tool2X) / 2;
-                                        float toolY = (tool1Y + tool2Y) / 2;
-
+                                        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 PointF(toolX, toolX);
                                         DownTool = new PointF(toolX, toolX);
                                         await plc.WriteNodeAsync(addressConfig.Out_DownCameraStatus.Address, (Int16)1);
@@ -1509,14 +1506,10 @@ namespace TeamAAS_VP.Core
                                 var visionResults = await Task.WhenAll(task1, task2);
                                 if (visionResults[0].IsSucceed && visionResults[1].IsSucceed)
                                 {
-                                    float tool1X = (float)(visionResults[0].X - visionResults[0].X);
-                                    float tool1Y = (float)(visionResults[0].Y - visionResults[0].Y);
-                                    float tool2X = (float)(visionResults[1].Y - visionResults[1].Y);
-                                    float tool2Y = (float)(visionResults[1].Y - visionResults[1].Y);
-                                    float toolX = (tool1X + tool2X) / 2;
-                                    float toolY = (tool1Y + tool2Y) / 2;
-
-                                    UpCameraPutResults[cameraIndex] = new PointF(toolX, toolY);
+                                    float centerX = (float)(visionResults[0].X + visionResults[1].X) / 2;
+                                    float centerY = (float)(visionResults[0].Y + visionResults[1].Y) / 2;
+                                    _MoveCameraAngle = Common.CalculateAngleBetweenPointsDegrees(new PointF((float)visionResults[0].X, (float)visionResults[0].Y), new PointF((float)visionResults[1].X, (float)visionResults[1].Y));
+                                    UpCameraPutResults[cameraIndex] = new PointF(centerX, centerY);
                                     await plc.WriteNodeAsync(addressConfig.Out_UpCameraPutStatus.Address, (Int16)1);
                                     return;
                                 }
@@ -1708,37 +1701,21 @@ namespace TeamAAS_VP.Core
                         if (state == 1)
                         {
                             SendTaskMessage($"收到多相机坐标合并请求，开始合并...", MessageLevel.Debug);
-                            //是拍一个锁一个还是拍两个点锁所有
-                            var systemConfig = _configService.GetSystemConfiguration();
-                            //拍1打1 
-                            if (systemConfig.WorkMode == 1)
-                            {
-                                currentProduct.ScrewPoints[0].X_Position = UpCameraPutResults[1].X+ DowmCameraResults[0].X;
-                                currentProduct.ScrewPoints[0].Y_Position = UpCameraPutResults[1].Y + DowmCameraResults[0].Y;
-                                await currentProduct.ScrewPoints[0].WriteToPlcAddress(addressConfig.Out_Screw, plc);
-                            }
-                            //拍1打所有
-                            else if (systemConfig.WorkMode == 2)
-                            {
-                                //根据UpCameraPutResults[]中的索引1、2，和拍照点对应的Local下的坐标，从创建Local坐标系
-                                PointF worldP1=new PointF(UpCameraPutResults[1].X, UpCameraPutResults[1].Y);
-                                PointF worldP2= new PointF(UpCameraPutResults[2].X, UpCameraPutResults[2].Y);
-                                PointF localP1= new PointF(currentProduct.ScrewCameraLocalPos1.X_Position, currentProduct.ScrewCameraLocalPos1.Y_Position);
-                                PointF localP2= new PointF(currentProduct.ScrewCameraLocalPos2.X_Position, currentProduct.ScrewCameraLocalPos2.Y_Position);
-                                CoordinateTransformer local= new CoordinateTransformer(worldP1, worldP2, localP1, localP2);
-
-                                for (int i = 0; i < currentProduct.ScrewPoints.Count; i++)
-                                {
-                                    var localPoint = new PointF(currentProduct.ScrewPoints[i].X_Position, currentProduct.ScrewPoints[i].Y_Position);
-                                    var worldPoint= local.ToOldCoord(localPoint.X, localPoint.Y);
-                                    var point = currentProduct.ScrewPoints[i].Clone();
-                                    point.X_Position = (float)worldPoint[0];
-                                    point.Y_Position = (float)worldPoint[1];
-                                    SendTaskMessage($"点：{i}，{point.X_Position:F3}，{point.Y_Position:F4}", MessageLevel.Info);
-                                    await point.WriteToPlcAddress(addressConfig.Out_Screw, plc);
-                                }
-                                await plc.WriteNodeAsync(addressConfig.Out_WorkNum.Address, (Int16)currentProduct.ScrewPoints.Count);
-                            }
+                            var point = currentProduct.ScrewPoints[0].Clone();
+                            var pn = Vector<double>.Build.Dense(new double[] { UpCameraPutResults[1].X, UpCameraPutResults[1].Y });
+                            //目标角度：
+                            double angle = point.U_Position + (_FixedUpCameraAngle - currentProduct.FixedUpCameraTempletAngle) + (_MoveCameraAngle - currentProduct.MoveDownCamerTempletAngle);
+
+                            var toolCoord = new ToolCoord();
+                            toolCoord.SetTool(DowmCameraResults[1].X, DowmCameraResults[1].Y);
+                            //将_PutPoint转换为Tool0下要走的点
+                            var p0 = toolCoord.GetTool0Coord(pn, angle);
+                            point.X_Position = (float)p0[0];
+                            point.Y_Position = (float)p0[1];
+                            point.U_Position = (float)angle;
+                            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