添加 MX-PD-盘古 项目文件
将 MX-PD-盘古 - new 目录下的所有文件添加到主仓库
This commit is contained in:
Shi.Ji
@@ -0,0 +1,290 @@
|
||||
using MainShell.Common;
|
||||
using MainShell.Log;
|
||||
using MainShell.Motion;
|
||||
using MainShell.ProcessResult;
|
||||
using MainShell.Recipe.Models;
|
||||
using MainShell.Recipe.Models.SubstrateParameter;
|
||||
using MW.WorkFlow;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Threading.Tasks;
|
||||
using System.Windows;
|
||||
|
||||
namespace MainShell.Process
|
||||
{
|
||||
public class SubstratePositionMotionService
|
||||
{
|
||||
private readonly ApproachAlignmentService _approachAlignmentService;
|
||||
|
||||
public SubstratePositionMotionService(ApproachAlignmentService approachAlignmentService)
|
||||
{
|
||||
_approachAlignmentService = approachAlignmentService ?? throw new ArgumentNullException(nameof(approachAlignmentService));
|
||||
}
|
||||
|
||||
public async Task ExecuteAsync(WorkflowContext context, ActivityControl activityControl)
|
||||
{
|
||||
if (context == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(context));
|
||||
}
|
||||
|
||||
if (activityControl == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(activityControl));
|
||||
}
|
||||
|
||||
RecipeManager recipeManager = GetRecipeManager(context);
|
||||
ProcessResultManager processResultManager = GetProcessResultManager(context);
|
||||
SubstrateRecipe substrateRecipe = GetCurrentRecipe(recipeManager);
|
||||
List<MarkData> enabledMarks = GetEnabledMarks(substrateRecipe);
|
||||
SubstratePositionProcessResult processResult = InitializeProcessResult(processResultManager, enabledMarks.Count);
|
||||
|
||||
try
|
||||
{
|
||||
LogManager.LogInfo($"SubstratePosition: start mark positioning. Total enabled marks: {enabledMarks.Count}");
|
||||
|
||||
for (int i = 0; i < enabledMarks.Count; i++)
|
||||
{
|
||||
MarkData markData = enabledMarks[i];
|
||||
string markName = string.IsNullOrWhiteSpace(markData?.TemplateName) ? $"Mark_{i}" : markData.TemplateName;
|
||||
|
||||
await CheckCancellationAndPauseAsync(activityControl).ConfigureAwait(false);
|
||||
|
||||
LogManager.LogInfo($"SubstratePosition: executing approach alignment for mark[{i}] '{markName}'");
|
||||
ApproachAlignmentResult alignmentResult = await ExecuteApproachAlignmentAsync(markData, activityControl).ConfigureAwait(false);
|
||||
|
||||
if (!alignmentResult.Succeeded)
|
||||
{
|
||||
string reason = alignmentResult.Message ?? string.Empty;
|
||||
string errorMsg = $"SubstratePosition: approach alignment failed for mark[{i}] '{markName}'. Reason: {reason}";
|
||||
LogManager.LogSysError(errorMsg);
|
||||
throw new LocalizedProcessException(
|
||||
MessageKey.ProcessSubstratePositionAlignmentFailedWithReason,
|
||||
new[] { markName, reason });
|
||||
}
|
||||
|
||||
Point recognizedPos = ExtractRecognizedPosition(alignmentResult, markData);
|
||||
processResult.MarkResults.Add(recognizedPos);
|
||||
processResult.RecognizedMarkCount++;
|
||||
|
||||
LogManager.LogInfo(
|
||||
string.Format(
|
||||
"SubstratePosition: mark[{0}] aligned successfully. Iterations: {1}. Position: ({2:F4}, {3:F4})",
|
||||
i,
|
||||
alignmentResult.CompletedIterations,
|
||||
recognizedPos.X,
|
||||
recognizedPos.Y));
|
||||
}
|
||||
|
||||
processResult.SubstrateAngle = CalculateSubstrateAngle(processResult.MarkResults);
|
||||
processResult.IsSuccess = true;
|
||||
processResult.ErrorMessage = null;
|
||||
processResult.ErrorMessageKey = MessageKey.None;
|
||||
processResult.ErrorMessageArguments = Array.Empty<string>();
|
||||
|
||||
LogManager.LogInfo(
|
||||
string.Format(
|
||||
"SubstratePosition: completed successfully. Recognized: {0}/{1}, Angle: {2:F6}",
|
||||
processResult.RecognizedMarkCount,
|
||||
processResult.AvailableMarkCount,
|
||||
processResult.SubstrateAngle));
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
processResult.IsSuccess = false;
|
||||
processResult.ErrorMessage = null;
|
||||
processResult.ErrorMessageKey = MessageKey.None;
|
||||
processResult.ErrorMessageArguments = Array.Empty<string>();
|
||||
LogManager.LogInfo("SubstratePosition: operation canceled.");
|
||||
throw;
|
||||
}
|
||||
catch (LocalizedProcessException ex)
|
||||
{
|
||||
processResult.IsSuccess = false;
|
||||
processResult.ErrorMessageKey = ex.FailureMessageKey;
|
||||
processResult.ErrorMessageArguments = ex.FailureMessageArguments;
|
||||
processResult.ErrorMessage = ex.Message;
|
||||
LogManager.LogSysError(ex.Message);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
processResult.IsSuccess = false;
|
||||
processResult.ErrorMessageKey = MessageKey.ProcessStepFailedWithReason;
|
||||
processResult.ErrorMessageArguments = new[] { ProcessFlowName.SubstratePositionFlow, ex.Message ?? string.Empty };
|
||||
processResult.ErrorMessage = LanguageResourceHelper.Format(
|
||||
processResult.ErrorMessageKey,
|
||||
LocalizedProcessException.ConvertArguments(processResult.ErrorMessageArguments));
|
||||
LogManager.LogSysError(processResult.ErrorMessage);
|
||||
}
|
||||
finally
|
||||
{
|
||||
WriteResultToContext(context, processResultManager, processResult);
|
||||
}
|
||||
}
|
||||
|
||||
private async Task<ApproachAlignmentResult> ExecuteApproachAlignmentAsync(MarkData markData, ActivityControl activityControl)
|
||||
{
|
||||
if (markData == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(markData));
|
||||
}
|
||||
|
||||
List<ApproachAlignmentAxis> axes = new List<ApproachAlignmentAxis>
|
||||
{
|
||||
new ApproachAlignmentAxis("X", 0.1),
|
||||
new ApproachAlignmentAxis("Y", 0.1)
|
||||
};
|
||||
|
||||
ApproachAlignmentRequest request = new ApproachAlignmentRequest(axes)
|
||||
{
|
||||
MaxIterations = 5,
|
||||
MoveTimeoutMilliseconds = 30000,
|
||||
RecognitionTimeoutMilliseconds = 10000
|
||||
};
|
||||
|
||||
return await _approachAlignmentService
|
||||
.ApproachAlignmentAsync(request, activityControl.CancellationToken)
|
||||
.ConfigureAwait(false);
|
||||
}
|
||||
|
||||
private static Point ExtractRecognizedPosition(ApproachAlignmentResult alignmentResult, MarkData markData)
|
||||
{
|
||||
if (alignmentResult == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(alignmentResult));
|
||||
}
|
||||
|
||||
if (alignmentResult.FinalAxisPositions == null ||
|
||||
!alignmentResult.FinalAxisPositions.TryGetValue("X", out var x) ||
|
||||
!alignmentResult.FinalAxisPositions.TryGetValue("Y", out var y))
|
||||
{
|
||||
throw new LocalizedProcessException(
|
||||
MessageKey.ProcessSubstratePositionAlignmentFailedWithReason,
|
||||
new[]
|
||||
{
|
||||
string.IsNullOrWhiteSpace(markData?.TemplateName) ? string.Empty : markData.TemplateName,
|
||||
"Recognized position is missing X or Y axis data."
|
||||
});
|
||||
}
|
||||
|
||||
return new Point(x, y);
|
||||
}
|
||||
|
||||
private static async Task CheckCancellationAndPauseAsync(ActivityControl activityControl)
|
||||
{
|
||||
activityControl.ThrowIfCancellationRequested();
|
||||
await activityControl.CheckPauseAsync().ConfigureAwait(false);
|
||||
}
|
||||
|
||||
private static RecipeManager GetRecipeManager(WorkflowContext context)
|
||||
{
|
||||
return context.GetData<RecipeManager>(WorkflowContextKeys.RecipeManager);
|
||||
}
|
||||
|
||||
private static ProcessResultManager GetProcessResultManager(WorkflowContext context)
|
||||
{
|
||||
return context.GetData<ProcessResultManager>(WorkflowContextKeys.ProcessResultManager);
|
||||
}
|
||||
|
||||
private static SubstrateRecipe GetCurrentRecipe(RecipeManager recipeManager)
|
||||
{
|
||||
if (recipeManager == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(recipeManager));
|
||||
}
|
||||
|
||||
if (recipeManager.CurrentSubstrateRecipe == null)
|
||||
{
|
||||
throw new LocalizedProcessException(
|
||||
MessageKey.ProcessSubstratePositionRecipeNotLoaded,
|
||||
Array.Empty<string>());
|
||||
}
|
||||
|
||||
return recipeManager.CurrentSubstrateRecipe;
|
||||
}
|
||||
|
||||
private static List<MarkData> GetEnabledMarks(SubstrateRecipe substrateRecipe)
|
||||
{
|
||||
if (substrateRecipe?.SubtrateMarkParameterInfo?.MarkDatas == null)
|
||||
{
|
||||
throw new LocalizedProcessException(
|
||||
MessageKey.ProcessSubstratePositionMarkParameterMissing,
|
||||
Array.Empty<string>());
|
||||
}
|
||||
|
||||
List<MarkData> enabledMarks = substrateRecipe.SubtrateMarkParameterInfo.MarkDatas
|
||||
.Where(mark => mark != null && mark.IsEnabled)
|
||||
.ToList();
|
||||
|
||||
if (enabledMarks.Count == 0)
|
||||
{
|
||||
throw new LocalizedProcessException(
|
||||
MessageKey.ProcessSubstratePositionNoEnabledMarks,
|
||||
Array.Empty<string>());
|
||||
}
|
||||
|
||||
return enabledMarks;
|
||||
}
|
||||
|
||||
private static SubstratePositionProcessResult InitializeProcessResult(ProcessResultManager processResultManager, int availableMarkCount)
|
||||
{
|
||||
if (processResultManager == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(processResultManager));
|
||||
}
|
||||
|
||||
SubstratePositionProcessResult processResult = processResultManager.SubstratePositionResult;
|
||||
processResult.IsSuccess = false;
|
||||
processResult.AvailableMarkCount = availableMarkCount;
|
||||
processResult.RecognizedMarkCount = 0;
|
||||
processResult.ErrorMessage = null;
|
||||
processResult.ErrorMessageKey = MessageKey.None;
|
||||
processResult.ErrorMessageArguments = Array.Empty<string>();
|
||||
processResult.SubstrateAngle = 0;
|
||||
processResult.MarkResults.Clear();
|
||||
return processResult;
|
||||
}
|
||||
|
||||
private static double CalculateSubstrateAngle(List<Point> markResults)
|
||||
{
|
||||
if (markResults == null || markResults.Count < 2)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
double centroidX = markResults.Average(point => point.X);
|
||||
double centroidY = markResults.Average(point => point.Y);
|
||||
|
||||
double sumXX = 0;
|
||||
double sumYY = 0;
|
||||
double sumXY = 0;
|
||||
|
||||
foreach (Point point in markResults)
|
||||
{
|
||||
double dx = point.X - centroidX;
|
||||
double dy = point.Y - centroidY;
|
||||
sumXX += dx * dx;
|
||||
sumYY += dy * dy;
|
||||
sumXY += dx * dy;
|
||||
}
|
||||
|
||||
if (Math.Abs(sumXX) < 1e-9 && Math.Abs(sumYY) < 1e-9 && Math.Abs(sumXY) < 1e-9)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
double angleRadians = 0.5 * Math.Atan2(2 * sumXY, sumXX - sumYY);
|
||||
return angleRadians * 180.0 / Math.PI;
|
||||
}
|
||||
|
||||
private static void WriteResultToContext(
|
||||
WorkflowContext context,
|
||||
ProcessResultManager processResultManager,
|
||||
SubstratePositionProcessResult processResult)
|
||||
{
|
||||
context.SetData(WorkflowContextKeys.SubstratePositionResult, processResult);
|
||||
processResultManager.SaveSubstratePositionResult();
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user