From dc5b1b94a5a48dc3ef653fc64840c3eabce4702f Mon Sep 17 00:00:00 2001 From: gebo Date: Fri, 21 Nov 2025 13:46:29 +0800 Subject: [PATCH] add nash --- WaferAdjust/FitCircleToolBlock.cs | 3 +- WaferAdjust/Form1.Designer.cs | 6 +- WaferAdjust/Form1.cs | 118 +++++++++++++++++++--------- WaferAdjust/GetCircleToolBlock.cs | 8 ++ WaferAdjust/TranslateCirclePoint.cs | 29 ++++++- 5 files changed, 122 insertions(+), 42 deletions(-) diff --git a/WaferAdjust/FitCircleToolBlock.cs b/WaferAdjust/FitCircleToolBlock.cs index 8299586..ddf3463 100644 --- a/WaferAdjust/FitCircleToolBlock.cs +++ b/WaferAdjust/FitCircleToolBlock.cs @@ -10,7 +10,7 @@ using System.Threading.Tasks; namespace WaferAdjust { - public delegate void OnFitCircleResult(double x, double y, double rms); + public delegate void OnFitCircleResult(double x, double y, double r, double rms); internal class FitCircleToolBlock { private CogToolBlock cogToolBlock; @@ -47,6 +47,7 @@ namespace WaferAdjust FitCircleImage = cogToolBlock.Outputs["OutputImage"].Value as CogImage8Grey; OnFitCircleResult?.Invoke(Math.Round((double)cogToolBlock.Outputs["CenterX"].Value, 3), Math.Round((double)cogToolBlock.Outputs["CenterY"].Value, 3), + Math.Round((double)cogToolBlock.Outputs["Radius"].Value, 3), Math.Round((double)cogToolBlock.Outputs["RMSError"].Value, 3)); } catch (Exception ex) diff --git a/WaferAdjust/Form1.Designer.cs b/WaferAdjust/Form1.Designer.cs index 5dd5359..2fc1009 100644 --- a/WaferAdjust/Form1.Designer.cs +++ b/WaferAdjust/Form1.Designer.cs @@ -289,7 +289,7 @@ this.textBox2.Name = "textBox2"; this.textBox2.Size = new System.Drawing.Size(55, 21); this.textBox2.TabIndex = 7; - this.textBox2.Text = "64"; + this.textBox2.Text = "82"; // // label1 // @@ -323,7 +323,7 @@ this.textBox5.Name = "textBox5"; this.textBox5.Size = new System.Drawing.Size(55, 21); this.textBox5.TabIndex = 13; - this.textBox5.Text = "1"; + this.textBox5.Text = "-6"; // // label2 // @@ -349,7 +349,7 @@ this.textBox4.Name = "textBox4"; this.textBox4.Size = new System.Drawing.Size(55, 21); this.textBox4.TabIndex = 11; - this.textBox4.Text = "1"; + this.textBox4.Text = "-8"; // // groupBox1 // diff --git a/WaferAdjust/Form1.cs b/WaferAdjust/Form1.cs index e0cef2d..7c16fbb 100644 --- a/WaferAdjust/Form1.cs +++ b/WaferAdjust/Form1.cs @@ -6,6 +6,7 @@ using Cognex.VisionPro.ImageProcessing; using Cognex.VisionPro.PMAlign; using Cognex.VisionPro.ToolBlock; using QWhale.Common; +using QWhale.Syntax; using System; using System.Collections.Generic; using System.ComponentModel; @@ -19,6 +20,7 @@ using System.Threading; using System.Threading.Tasks; using System.Windows.Forms; using System.Windows.Interop; +using System.Windows.Media; using System.Xml.Serialization; @@ -52,6 +54,7 @@ namespace WaferAdjust getCircleMachineToolBlock = new GetCircleToolBlock(); getCircleMachineToolBlock.OnGetCircleResult += GetCircleMachineToolBlock_OnGetCircleResult; + getCircleMachineToolBlock.OnGetNashResult += GetCircleMachineToolBlock_OnGetNashResult; getCircleMachineToolBlock.OnToolReady += GetCircleMachineToolBlock_OnToolReady; getCircleMachineToolBlock.Initialize("vpp\\GetCircleM.vpp"); @@ -70,31 +73,88 @@ namespace WaferAdjust } } + private void GetCircleMachineToolBlock_OnGetNashResult(double x, double y) + { + translateCirclePoint.AddNashPoint(x, y); + ShowMessage($"Update Nash X:{x}, Y:{y}{Environment.NewLine}"); + } + private void FitCircleMachineToolBlock_OnToolReady(bool ready) { } - - private void FitCircleMachineToolBlock_OnFitCircleResult(double x, double y, double rms) + private void AddPointMarker(CogGraphicCollection graphicCollection, double x, double y, CogColorConstants color = CogColorConstants.Green) { - CogPointMarker marker_1 = new CogPointMarker(); + CogPointMarker marker = new CogPointMarker(); + marker.X = x; + marker.Y = y; + marker.Color = color; + marker.Interactive = false; + graphicCollection.Add(marker); + } + private void AddLineMarker(CogGraphicCollection graphicCollection, double x1, double y1, double x2, double y2, CogColorConstants color = CogColorConstants.Green) + { + CogLineSegment line = new CogLineSegment(); + line.SetStartEnd((int)x1, (int)y1, (int)x2, (int)y2); + line.Color = color; + line.Interactive = false; + graphicCollection.Add(line); + } + private void AddLabelMarker(CogGraphicCollection graphicCollection, string text, double x, double y, CogColorConstants color = CogColorConstants.Green) + { + CogGraphicLabel label = new CogGraphicLabel(); + label.Text = text; + label.X = x; + label.Y = y; + label.Color = color; + graphicCollection.Add(label); + } + /// + /// 计算从 lineA 到 lineB 的逆时针有向角度（0-360度） + /// 坐标系：向右为正，向下为正（标准屏幕坐标系） + /// + public double GetCounterClockwiseAngle360(double x, double y, double x1, double y1, double x2, double y2) + { + // 2. 获取方向向量（dy 向下为正） + double dxA = x1 - x; + double dyA = y1 - y; - marker_1.X = x; - marker_1.Y = y; - marker_1.Color = CogColorConstants.Green; - marker_1.Interactive = false; - m_graphics2.Add(marker_1); + double dxB = x2 - x; + double dyB = y2 - y; - CogPointMarker marker_2 = new CogPointMarker(); + // 3. 将Y轴翻转，转换为标准数学坐标系（dy向上为正） + // 这样 Math.Atan2 返回的就是标准数学角度（逆时针为正） + double angleA = Math.Atan2(-dyA, dxA); // 注意：y取负号 + double angleB = Math.Atan2(-dyB, dxB); // 注意：y取负号 - marker_2.X = Convert.ToDouble(textBox4.Text); - marker_2.Y = Convert.ToDouble(textBox5.Text); - marker_2.Color = CogColorConstants.Red; - marker_2.Interactive = false; - m_graphics2.Add(marker_2); + // 4. 计算逆时针角度差（弧度） + double angleDiff = angleB - angleA; + + // 5. 转换为角度并规范化为 [0, 360) + double angleDegrees = angleDiff * 180.0 / Math.PI; + angleDegrees = angleDegrees % 360.0; + if (angleDegrees < 0) angleDegrees += 360.0; + + return Math.Round(angleDegrees, 3); + } + private void FitCircleMachineToolBlock_OnFitCircleResult(double x, double y, double r, double rms) + { + AddPointMarker(m_graphics2, x, y); + AddPointMarker(m_graphics2, translateCirclePoint.GetRotateX(), translateCirclePoint.GetRotateY(), CogColorConstants.Red); + + PointInfo nPoint = translateCirclePoint.GetNashPoint(); + if (nPoint != null) + { + AddPointMarker(m_graphics2, nPoint.X, nPoint.Y, CogColorConstants.Yellow); + AddLineMarker(m_graphics2, x, y, x, y - r); + AddLineMarker(m_graphics2, x, y, nPoint.X, nPoint.Y, CogColorConstants.Yellow); + var aa = GetCounterClockwiseAngle360(x, y, x, y - r, nPoint.X, nPoint.Y); + AddLabelMarker(m_graphics2, aa.ToString(), x, y, CogColorConstants.Yellow); + ShowMessage($"{Environment.NewLine}nash degree:{aa}{Environment.NewLine}"); + } ShowMessage($"{Environment.NewLine}X:{x}, Y:{y}, RMS:{rms}{Environment.NewLine}"); - ShowMessage($"{Environment.NewLine}DiffX:{x - Convert.ToDouble(textBox4.Text)}, DiffY:{y - Convert.ToDouble(textBox5.Text)}, RMS:{rms}{Environment.NewLine}"); + ShowMessage($"{Environment.NewLine}DiffX:{Math.Round((x - Convert.ToDouble(textBox4.Text)), 3)}, DiffY:{Math.Round((y - Convert.ToDouble(textBox5.Text)), 3)}, RMS:{rms}{Environment.NewLine}"); ImageDisplay(FitCircleMachineToolBlock.FitCircleImage); ImageDisplay(m_graphics); ImageDisplay(m_graphics2); @@ -118,15 +178,9 @@ namespace WaferAdjust } - private void FitCircleCameraToolBlock_OnFitCircleResult(double x, double y, double rms) + private void FitCircleCameraToolBlock_OnFitCircleResult(double x, double y, double r, double rms) { - CogPointMarker marker_1 = new CogPointMarker(); - - marker_1.X = x; - marker_1.Y = y; - marker_1.Color = CogColorConstants.Green; - marker_1.Interactive = false; - m_graphics2.Add(marker_1); + AddPointMarker(m_graphics2, x, y); ShowMessage($"{Environment.NewLine}X:{x}, Y:{y}, RMS:{rms}{Environment.NewLine}"); ImageDisplay(fitCircleCameraToolBlock.FitCircleImage); ImageDisplay(m_graphics); @@ -196,13 +250,7 @@ namespace WaferAdjust m_graphics = new CogGraphicCollection(); foreach (PointInfo pointInfo in aa) { - CogPointMarker marker_1 = new CogPointMarker();//圆 - - marker_1.X = pointInfo.X; - marker_1.Y = pointInfo.Y; - marker_1.Color = CogColorConstants.Green; - marker_1.Interactive = false; - m_graphics.Add(marker_1); + AddPointMarker(m_graphics, pointInfo.X, pointInfo.Y); } fitCircleCameraToolBlock.Run(m_lastBMP, aa); } @@ -284,20 +332,16 @@ namespace WaferAdjust ShowMessage("转换机械坐标系下圆弧各点的原始坐标：\n"); var points = translateCirclePoint.DoTranslatePoint(); + translateCirclePoint.DoTranslateNashPoint(); ShowMessage("拟合机械坐标系下的圆中心：\n"); m_graphics = new CogGraphicCollection(); m_graphics2 = new CogGraphicCollection(); foreach (PointInfo pointInfo in points) { - CogPointMarker marker_1 = new CogPointMarker();//圆 - - marker_1.X = pointInfo.X; - marker_1.Y = pointInfo.Y; - marker_1.Color = CogColorConstants.Green; - marker_1.Interactive = false; - m_graphics.Add(marker_1); + AddPointMarker(m_graphics, pointInfo.X, pointInfo.Y); } + FitCircleMachineToolBlock.Run(m_lastBMP, points); } diff --git a/WaferAdjust/GetCircleToolBlock.cs b/WaferAdjust/GetCircleToolBlock.cs index 9de918e..da336d1 100644 --- a/WaferAdjust/GetCircleToolBlock.cs +++ b/WaferAdjust/GetCircleToolBlock.cs @@ -13,12 +13,14 @@ namespace WaferAdjust { public delegate void OnGetCircleResult(double x, double y, double r); public delegate void OnToolReady(bool ready); + public delegate void OnGetNashResult(double x, double y); internal class GetCircleToolBlock { private CogToolBlock cogToolBlock; private bool initialized = false; public event OnToolReady OnToolReady; public event OnGetCircleResult OnGetCircleResult; + public event OnGetNashResult OnGetNashResult; public List PointInfos; public void Initialize(string vpp) { @@ -62,6 +64,12 @@ namespace WaferAdjust OnGetCircleResult?.Invoke(Math.Round((double)cogToolBlock.Outputs["CenterX"].Value, 3), Math.Round((double)cogToolBlock.Outputs["CenterY"].Value, 3), Math.Round((double)cogToolBlock.Outputs["Radius"].Value, 3)); + + if ((int)cogToolBlock.Outputs["Results_Count"].Value == 1) + { + OnGetNashResult?.Invoke(Math.Round((double)cogToolBlock.Outputs["TranslationX"].Value, 3), + Math.Round((double)cogToolBlock.Outputs["TranslationY"].Value, 3)); + } } catch (Exception ex) { diff --git a/WaferAdjust/TranslateCirclePoint.cs b/WaferAdjust/TranslateCirclePoint.cs index 8b3f7e8..d685a63 100644 --- a/WaferAdjust/TranslateCirclePoint.cs +++ b/WaferAdjust/TranslateCirclePoint.cs @@ -14,6 +14,7 @@ namespace WaferAdjust List totalCenters; private int circleIndex; private double perAngle; + PointInfo nashPoint; public void SetRotateXY(double x, double y, double angle) { rotateX = x; rotateY = y; @@ -21,6 +22,15 @@ namespace WaferAdjust totalCenters = new List(); circleIndex = 0; perAngle = angle; + nashPoint = null; + } + public double GetRotateX() + { + return rotateX; + } + public double GetRotateY() + { + return rotateY; } public void AddCirclePoint(List pointInfos) { @@ -33,6 +43,10 @@ namespace WaferAdjust { circleIndex++; } + public void AddNashPoint(double x, double y) + { + nashPoint = new PointInfo(x, y, circleIndex * perAngle); + } public void AddCircleCenter(double x, double y, double r) { totalCenters.Add(new PointInfo(x, y, r)); @@ -44,10 +58,23 @@ namespace WaferAdjust { var res = Rotation2D.GetOriginalPointDegrees(item.X, item.Y, rotateX, rotateY, item.Radius, true, false); - Console.WriteLine($"{item.X} {item.Y} {item.Radius} ==> {res.x} {res.y}"); trans.Add(new PointInfo(res.x, res.y, item.Radius)); } return trans; } + public void DoTranslateNashPoint() + { + if (nashPoint == null) + return; + + var res = Rotation2D.GetOriginalPointDegrees(nashPoint.X, nashPoint.Y, + rotateX, rotateY, nashPoint.Radius, true, false); + + nashPoint = new PointInfo(res.x, res.y, nashPoint.Radius); + } + public PointInfo GetNashPoint() + { + return nashPoint; + } } }