290 lines
12 KiB
C#
290 lines
12 KiB
C#
|
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();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|