优化三点定位算法

Rs.DeweyTester/Commom/TrayPointManager.cs

@@ -63,5 +63,32 @@ namespace Rs.MotionPlat.Commom
             return null;
         }
 
+        /// <summary>
+        /// 获取料盘的点位
+        /// </summary>
+        /// <param name="recipeName"></param>
+        /// <param name="trayName"></param>
+        /// <param name="slotIndex"></param>
+        /// <param name="pointType"></param>
+        /// <returns></returns>
+        public static List<SlotPoint> GetAllSlotPoint(ETrayType trayType)
+        {
+            List<SlotPoint> list = new List<SlotPoint>();
+            for(int i = 0;i<256;i++)
+            {
+                string key = $"{GlobalVar.CurRecipe}-{trayType}-{i+1}";
+                if (trayPoints.ContainsKey(key))
+                {
+                    list.Add(trayPoints[key]);
+                }
+                //if (trayPoints)
+                //if(item.Key.IndexOf($"{GlobalVar.CurRecipe}-{trayType}") >=0)
+                //{
+                //    list.Add(item.Value);
+                //}
+
+            }
+            return list;
+        }
     }
 }

Rs.DeweyTester/Flow/NormalFlow/ThreePointLocationFlow.cs

@@ -1,4 +1,5 @@
-using Newtonsoft.Json;
+using HalconDotNet;
+using Newtonsoft.Json;
 using Rs.DataAccess;
 using Rs.Framework;
 using Rs.Motion;
@@ -249,5 +250,78 @@ namespace Rs.MotionPlat.Flow.NormalFlow
                 //}
             }
         }
+
+
+        public void CalcTrayRunPoint2(ETrayType trayType)
+        {
+            double sx1 = TrayPointManager.GetSlotPoint(trayType, 1).X;
+            double sy1 = TrayPointManager.GetSlotPoint(trayType, 1).Y ;
+
+            double sx2 = TrayPointManager.GetSlotPoint(trayType, 16).X ;
+            double sy2 = TrayPointManager.GetSlotPoint(trayType, 16).Y ;
+
+            double sx3 = TrayPointManager.GetSlotPoint(trayType, 241).X ;
+            double sy3 = TrayPointManager.GetSlotPoint(trayType, 241).Y ;
+
+
+            double x1 = TrayPointManager.GetSlotPoint(trayType, 1).X + 0.1;// vResult[0].OffsetX;
+            double y1 = TrayPointManager.GetSlotPoint(trayType, 1).Y + 0.07;// vResult[0].OffsetY;
+
+            double x2 = TrayPointManager.GetSlotPoint(trayType, 16).X + 0.1;// vResult[1].OffsetX;
+            double y2 = TrayPointManager.GetSlotPoint(trayType, 16).Y + 0.07;// vResult[1].OffsetY;
+
+            double x3 = TrayPointManager.GetSlotPoint(trayType, 241).X + 0.1;// vResult[2].OffsetX;
+            double y3 = TrayPointManager.GetSlotPoint(trayType, 241).Y + 0.07;// vResult[2].OffsetY;
+
+            HOperatorSet.VectorToHomMat2d(new HTuple(sx1).TupleConcat(sx2).TupleConcat(sx3), new HTuple(sy1).TupleConcat(sy2).TupleConcat(sy3), new HTuple(x1).TupleConcat(x2).TupleConcat(x3), new HTuple(y1).TupleConcat(y2).TupleConcat(y3), out HTuple homMat2D);
+            //HOperatorSet.AffineTransPoint2d(homMat2D,)
+           List<SlotPoint> points = TrayPointManager.GetAllSlotPoint(trayType);
+            HTuple xx = new HTuple();
+            HTuple yy = new HTuple();
+            //for(int i=0;i<points.Count; i++)
+            //{
+            //    string key = $"{GlobalVar.CurRecipe}-{trayType}-{i+1}";
+            //    if(points.con)
+            //}
+            foreach (SlotPoint point in points)
+            {
+                xx.Append(point.X);
+                yy.Append(point.Y);
+            }
+            HOperatorSet.AffineTransPoint2d(homMat2D, xx, yy, out HTuple qx, out HTuple qy);
+
+            int l = xx.Length;
+
+            for(int j=0;j<xx.Length;j++)
+            {
+                double x = qx[j];
+                double y = qy[j];
+                string key = $"{GlobalVar.CurRecipe}-{trayType}-{j+1}";
+                TrayRunPointManager.AddPoint(key, x, y);
+                TrayRunPointManager.AddToDataTable(key, x, y);
+            }
+            //double bigSpaceX = (x3 - x1) / 15;
+            //double smallSpaceY = (y3 - y1) / 15;
+
+            //double bigSpaceY = (y2 - y1) / 15;
+            //double smallSpaceX = (x2 - x1) / 15;
+            //int index = 1;
+            //for (int row = 1; row <= 16; row++)
+            //{
+            //    for (int col = 1; col <= 16; col++)
+            //    {
+            //        double x = x1 + (row - 1) * bigSpaceX + (col - 1) * smallSpaceX;
+            //        double y = y1 + (col - 1) * bigSpaceY + (row - 1) * smallSpaceY;
+
+            //        //string insertSql = $"insert into trayrunpoints(recipename,trayname,slotindex,x,y) values('{GlobalVar.CurRecipe}','{trayType}',{index},{x},{y})";
+            //        //sqlList.Add(insertSql);
+            //        //rowAffectNum = db.ExecuteNonQuery(insertSql);
+            //        string key = $"{GlobalVar.CurRecipe}-{trayType}-{index}";
+            //        TrayRunPointManager.AddPoint(key, x, y);
+            //        TrayRunPointManager.AddToDataTable(key, x, y);
+            //        index++;
+            //    }
+            //}
+        }
     }
 }

Rs.DeweyTester/Rs.DeweyTester.csproj

@@ -34,6 +34,10 @@
     <WarningLevel>4</WarningLevel>
   </PropertyGroup>
   <ItemGroup>
+    <Reference Include="halcondotnet, Version=18.11.0.1, Culture=neutral, PublicKeyToken=4973bed59ddbf2b8, processorArchitecture=MSIL">
+      <SpecificVersion>False</SpecificVersion>
+      <HintPath>bin\Debug\halcondotnet.dll</HintPath>
+    </Reference>
     <Reference Include="ICSharpCode.SharpZipLib, Version=0.86.0.518, Culture=neutral, processorArchitecture=MSIL">
       <SpecificVersion>False</SpecificVersion>
       <HintPath>bin\Debug\ICSharpCode.SharpZipLib.dll</HintPath>

Rs.DeweyTester/TestFrm.cs

@@ -346,7 +346,11 @@ namespace Rs.MotionPlat
 
         private void button10_Click(object sender, EventArgs e)
         {
-            StockLocationFlow.Instance.Grab(ETrayType.Grr, 1);
+            ThreePointLocationFlow.Instance.CalcTrayRunPoint2(ETrayType.Ng);
+
+
+            //TrayPointManager.GetAllSlotPoint(ETrayType.Ng);
+            //StockLocationFlow.Instance.Grab(ETrayType.Grr, 1);
         }
     }
 }