优化三点定位

Rs.DeweyTester/Flow/NormalFlow/ThreePointLocationFlow.cs

@@ -53,10 +53,11 @@ namespace Rs.MotionPlat.Flow.NormalFlow
         public void Location(ETrayType trayType,bool needStop=true)
         {
             step = EThreePointLocationFlowStep.到拍照位;
-            List<int> points = new List<int>() { 1, 16, 241 };
+            List<int> points = new List<int>() { 1, 16,241 };
             SlotPoint sp;
             if(finished)
             {
+                vResult.Clear();
                 finished = false;
             }
             else
@@ -257,23 +258,108 @@ namespace Rs.MotionPlat.Flow.NormalFlow
             double sx1 = TrayPointManager.GetSlotPoint(trayType, 1).X;
             double sy1 = TrayPointManager.GetSlotPoint(trayType, 1).Y ;
 
-            double sx2 = TrayPointManager.GetSlotPoint(trayType, 16).X ;
+            double sx9 = TrayPointManager.GetSlotPoint(trayType, 9).X;
-            double sy2 = TrayPointManager.GetSlotPoint(trayType, 16).Y ;
+            double sy9 = TrayPointManager.GetSlotPoint(trayType, 9).Y;
 
-            double sx3 = TrayPointManager.GetSlotPoint(trayType, 241).X ;
-            double sy3 = TrayPointManager.GetSlotPoint(trayType, 241).Y ;
 
+            double sx16 = TrayPointManager.GetSlotPoint(trayType, 16).X ;
+            double sy16 = TrayPointManager.GetSlotPoint(trayType, 16).Y ;
 
-            double x1 = TrayPointManager.GetSlotPoint(trayType, 1).X + 0.1;// vResult[0].OffsetX;
+            double sx129 = TrayPointManager.GetSlotPoint(trayType, 129).X;
-            double y1 = TrayPointManager.GetSlotPoint(trayType, 1).Y + 0.07;// vResult[0].OffsetY;
+            double sy129 = TrayPointManager.GetSlotPoint(trayType, 129).Y;
 
-            double x2 = TrayPointManager.GetSlotPoint(trayType, 16).X + 0.1;// vResult[1].OffsetX;
+            double sx137 = TrayPointManager.GetSlotPoint(trayType, 137).X;
-            double y2 = TrayPointManager.GetSlotPoint(trayType, 16).Y + 0.07;// vResult[1].OffsetY;
+            double sy137 = TrayPointManager.GetSlotPoint(trayType, 137).Y;
 
-            double x3 = TrayPointManager.GetSlotPoint(trayType, 241).X + 0.1;// vResult[2].OffsetX;
+            double sx144 = TrayPointManager.GetSlotPoint(trayType, 144).X;
-            double y3 = TrayPointManager.GetSlotPoint(trayType, 241).Y + 0.07;// vResult[2].OffsetY;
+            double sy144 = TrayPointManager.GetSlotPoint(trayType, 144).Y;
 
-            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);
+            double sx241 = TrayPointManager.GetSlotPoint(trayType, 241).X ;
+            double sy241 = TrayPointManager.GetSlotPoint(trayType, 241).Y ;
+
+            double sx249 = TrayPointManager.GetSlotPoint(trayType, 249).X;
+            double sy249 = TrayPointManager.GetSlotPoint(trayType, 249).Y;
+
+            double sx256 = TrayPointManager.GetSlotPoint(trayType, 256).X;
+            double sy256 = TrayPointManager.GetSlotPoint(trayType, 256).Y;
+
+
+            double x1 = TrayPointManager.GetSlotPoint(trayType, 1).X +  vResult[0].OffsetX;
+            double y1 = TrayPointManager.GetSlotPoint(trayType, 1).Y +  vResult[0].OffsetY;
+
+            double x9 = TrayPointManager.GetSlotPoint(trayType, 9).X +  vResult[1].OffsetX;
+            double y9 = TrayPointManager.GetSlotPoint(trayType, 9).Y +  vResult[1].OffsetY;
+
+            double x16 = TrayPointManager.GetSlotPoint(trayType, 16).X + vResult[2].OffsetX;
+            double y16 = TrayPointManager.GetSlotPoint(trayType, 16).Y + vResult[2].OffsetY;
+
+
+
+            double x129 = TrayPointManager.GetSlotPoint(trayType, 129).X + vResult[3].OffsetX;
+            double y129 = TrayPointManager.GetSlotPoint(trayType, 129).Y + vResult[3].OffsetY;
+
+            double x137 = TrayPointManager.GetSlotPoint(trayType, 137).X + vResult[4].OffsetX;
+            double y137 = TrayPointManager.GetSlotPoint(trayType, 137).Y + vResult[4].OffsetY;
+
+            double x144 = TrayPointManager.GetSlotPoint(trayType, 144).X + vResult[5].OffsetX;
+            double y144 = TrayPointManager.GetSlotPoint(trayType, 144).Y + vResult[5].OffsetY;
+
+
+            double x241 = TrayPointManager.GetSlotPoint(trayType, 241).X +  vResult[6].OffsetX;
+            double y241 = TrayPointManager.GetSlotPoint(trayType, 241).Y +  vResult[6].OffsetY;
+
+            double x249 = TrayPointManager.GetSlotPoint(trayType, 249).X+vResult[7].OffsetX;
+            double y249 = TrayPointManager.GetSlotPoint(trayType, 249).Y + vResult[7].OffsetY;
+
+            double x256 = TrayPointManager.GetSlotPoint(trayType, 256).X+vResult[8].OffsetX;
+            double y256 = TrayPointManager.GetSlotPoint(trayType, 256).Y + vResult[8].OffsetY;
+            HTuple px = new HTuple();
+            px.Append(sx1);
+            //px.Append(sx9);
+            px.Append(sx16);
+            //px.Append(sx129);
+            //px.Append(sx137);
+            //px.Append(sx144);
+            px.Append(sx241);
+            //px.Append(sx249);
+            //px.Append(sx256);
+
+            HTuple py = new HTuple();
+            py.Append(sy1);
+            //py.Append(sy9);
+            py.Append(sy16);
+            //py.Append(sy129);
+            //py.Append(sy137);
+            //py.Append(sy144);
+            py.Append(sy241);
+            //py.Append(sy249);
+            //py.Append(sy256);
+
+            HTuple qx = new HTuple();
+            qx.Append(x1);
+            //qx.Append(x9);
+            qx.Append(x16);
+            //qx.Append(x129);
+            //qx.Append(x137);
+            //qx.Append(x144);
+            qx.Append(x241);
+            //qx.Append(x249);
+            //qx.Append(x256);
+
+            HTuple qy = new HTuple();
+            qy.Append(y1);
+            //qy.Append(y9);
+            qy.Append(y16);
+            //qy.Append(y129);
+            //qy.Append(y137);
+            //qy.Append(y144);
+            qy.Append(y241);
+            //qy.Append(y249);
+            //qy.Append(y256);
+
+            HOperatorSet.VectorToHomMat2d(px, py, qx, qy, out HTuple homMat2D);
+
+            //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();
@@ -288,14 +374,14 @@ namespace Rs.MotionPlat.Flow.NormalFlow
                 xx.Append(point.X);
                 yy.Append(point.Y);
             }
-            HOperatorSet.AffineTransPoint2d(homMat2D, xx, yy, out HTuple qx, out HTuple qy);
+            HOperatorSet.AffineTransPoint2d(homMat2D, xx, yy, out HTuple qxx, out HTuple qyy);
 
             int l = xx.Length;
 
             for(int j=0;j<xx.Length;j++)
             {
-                double x = qx[j];
+                double x = qxx[j];
-                double y = qy[j];
+                double y = qyy[j];
                 string key = $"{GlobalVar.CurRecipe}-{trayType}-{j+1}";
                 TrayRunPointManager.AddPoint(key, x, y);
                 TrayRunPointManager.AddToDataTable(key, x, y);

Rs.DeweyTester/TestFrm.Designer.cs

@@ -69,6 +69,7 @@
             this.button8 = new System.Windows.Forms.Button();
             this.button9 = new System.Windows.Forms.Button();
             this.button10 = new System.Windows.Forms.Button();
+            this.button12 = new System.Windows.Forms.Button();
             this.groupBox1.SuspendLayout();
             this.groupBox2.SuspendLayout();
             this.groupBox3.SuspendLayout();
@@ -540,11 +541,22 @@
             this.button10.UseVisualStyleBackColor = true;
             this.button10.Click += new System.EventHandler(this.button10_Click);
             // 
+            // button12
+            // 
+            this.button12.Location = new System.Drawing.Point(966, 555);
+            this.button12.Name = "button12";
+            this.button12.Size = new System.Drawing.Size(75, 23);
+            this.button12.TabIndex = 40;
+            this.button12.Text = "button12";
+            this.button12.UseVisualStyleBackColor = true;
+            this.button12.Click += new System.EventHandler(this.button12_Click);
+            // 
             // TestFrm
             // 
             this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 12F);
             this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font;
             this.ClientSize = new System.Drawing.Size(1336, 854);
+            this.Controls.Add(this.button12);
             this.Controls.Add(this.button10);
             this.Controls.Add(this.button9);
             this.Controls.Add(this.button8);
@@ -615,5 +627,6 @@
         private System.Windows.Forms.Button button8;
         private System.Windows.Forms.Button button9;
         private System.Windows.Forms.Button button10;
+        private System.Windows.Forms.Button button12;
     }
 }

Rs.DeweyTester/TestFrm.cs

@@ -352,5 +352,10 @@ namespace Rs.MotionPlat
             //TrayPointManager.GetAllSlotPoint(ETrayType.Ng);
             //StockLocationFlow.Instance.Grab(ETrayType.Grr, 1);
         }
+
+        private void button12_Click(object sender, EventArgs e)
+        {
+            ThreePointLocationFlow.Instance.CalcTrayRunPoint2(ETrayType.Ng);
+        }
     }
 }