409 lines
16 KiB
C#
409 lines
16 KiB
C#
|
using MainShell.Common;
|
|||
|
|
using MainShell.Hardware;
|
|||
|
|
using MainShell.Log;
|
|||
|
|
using MaxwellFramework.Core.Attributes;
|
|||
|
|
using System;
|
|||
|
|
using System.Collections.Generic;
|
|||
|
|
using System.Linq;
|
|||
|
|
using System.Threading;
|
|||
|
|
using System.Threading.Tasks;
|
|||
|
|
using System.Windows;
|
|||
|
|
|
|||
|
|
namespace MainShell.Motion
|
|||
|
|
{
|
|||
|
|
/// <summary>
|
|||
|
|
/// <20>ƽ<EFBFBD><C6BD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʵ<EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD>ʽ<EFBFBD>Ķ<EFBFBD><C4B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨
|
|||
|
|
/// </summary>
|
|||
|
|
|
|||
|
|
public class ApproachAlignmentService
|
|||
|
|
{
|
|||
|
|
private readonly SafeAxisMotion _safeAxisMotion;
|
|||
|
|
private readonly ICenterRecognizer _centerRecognizer;
|
|||
|
|
private readonly ICoordinateTransformer _coordinateTransformer;
|
|||
|
|
private readonly HardwareManager _hardwareManager;
|
|||
|
|
|
|||
|
|
public ApproachAlignmentService(
|
|||
|
|
SafeAxisMotion safeAxisMotion,
|
|||
|
|
ICenterRecognizer centerRecognizer,
|
|||
|
|
ICoordinateTransformer coordinateTransformer,
|
|||
|
|
HardwareManager hardwareManager)
|
|||
|
|
{
|
|||
|
|
_safeAxisMotion = safeAxisMotion ?? throw new ArgumentNullException(nameof(safeAxisMotion));
|
|||
|
|
_centerRecognizer = centerRecognizer ?? throw new ArgumentNullException(nameof(centerRecognizer));
|
|||
|
|
_coordinateTransformer = coordinateTransformer ?? throw new ArgumentNullException(nameof(coordinateTransformer));
|
|||
|
|
_hardwareManager = hardwareManager ?? throw new ArgumentNullException(nameof(hardwareManager));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// ִ<>бƽ<D0B1><C6BD><EFBFBD>λ
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="request"><3E>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD></param>
|
|||
|
|
/// <param name="cancellationToken">ȡ<><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD></param>
|
|||
|
|
/// <returns><3E>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD></returns>
|
|||
|
|
public async Task<ApproachAlignmentResult> ApproachAlignmentAsync(
|
|||
|
|
ApproachAlignmentRequest request,
|
|||
|
|
CancellationToken cancellationToken = default(CancellationToken))
|
|||
|
|
{
|
|||
|
|
if (request == null)
|
|||
|
|
{
|
|||
|
|
throw new ArgumentNullException(nameof(request));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
ApproachAlignmentResult result = new ApproachAlignmentResult();
|
|||
|
|
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
(double CenterX, double CenterY)? center = null;
|
|||
|
|
|
|||
|
|
// ѭ<><D1AD>ִ<EFBFBD>бƽ<D0B1>
|
|||
|
|
for (int iteration = 0; iteration < request.MaxIterations; iteration++)
|
|||
|
|
{
|
|||
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|||
|
|
|
|||
|
|
$"Approach alignment iteration {iteration + 1}/{request.MaxIterations} started.".LogInfo();
|
|||
|
|
|
|||
|
|
// <20><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>ʶ<EFBFBD><CAB6><EFBFBD><EFBFBD><EFBFBD>ġ<EFBFBD><C4A1>״ε<D7B4><CEB5><EFBFBD>ִ<EFBFBD><D6B4>ʶ<EFBFBD>𣬺<EFBFBD><F0A3ACBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD>ʶ<EFBFBD><CAB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if (center == null)
|
|||
|
|
{
|
|||
|
|
center = await RecognizeCenterAsync(
|
|||
|
|
request.Camera,
|
|||
|
|
request.RecognitionTimeoutMilliseconds,
|
|||
|
|
request.RecognitionParameters,
|
|||
|
|
cancellationToken).ConfigureAwait(false);
|
|||
|
|
if (center == null)
|
|||
|
|
{
|
|||
|
|
throw new InvalidOperationException("Center recognition failed.");
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
$"Center recognized: X={center.Value.CenterX:F4}, Y={center.Value.CenterY:F4}".LogInfo();
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
$"Center reused from previous iteration: X={center.Value.CenterX:F4}, Y={center.Value.CenterY:F4}".LogInfo();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// <20>ڶ<EFBFBD><DAB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
|
Point rulerPoint = BuildRulerPoint(request.Axes);
|
|||
|
|
Point pixelPoint = new Point(center.Value.CenterX, center.Value.CenterY);
|
|||
|
|
string cameraName = GetCameraName(request.Camera);
|
|||
|
|
|
|||
|
|
CoordinateTransformResult transformResult = await _coordinateTransformer.TransformAsync(
|
|||
|
|
rulerPoint,
|
|||
|
|
pixelPoint,
|
|||
|
|
cameraName).ConfigureAwait(false);
|
|||
|
|
|
|||
|
|
if (!transformResult.Succeeded)
|
|||
|
|
{
|
|||
|
|
throw new InvalidOperationException($"Coordinate transformation failed: {transformResult.Message}");
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
$"Coordinate transformation completed: Target=({transformResult.TargetPoint.X:F4}, {transformResult.TargetPoint.Y:F4}).".LogInfo();
|
|||
|
|
|
|||
|
|
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
MotionMoveRequest[] moveRequests = CreateMoveRequests(
|
|||
|
|
request.Axes,
|
|||
|
|
transformResult.TargetPoint,
|
|||
|
|
request.MoveTimeoutMilliseconds);
|
|||
|
|
|
|||
|
|
MotionBatchResult moveResult = await _safeAxisMotion.SafeMoveAsync(cancellationToken, moveRequests).ConfigureAwait(false);
|
|||
|
|
moveResult.EnsureSuccess();
|
|||
|
|
|
|||
|
|
$"Axes moved successfully: {moveResult.Results.Count} axes completed.".LogInfo();
|
|||
|
|
|
|||
|
|
// <20><><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD>ٴ<EFBFBD>ʶ<EFBFBD><CAB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
(double CenterX, double CenterY)? newCenter = await RecognizeCenterAsync(
|
|||
|
|
request.Camera,
|
|||
|
|
request.RecognitionTimeoutMilliseconds,
|
|||
|
|
request.RecognitionParameters,
|
|||
|
|
cancellationToken).ConfigureAwait(false);
|
|||
|
|
if (newCenter == null)
|
|||
|
|
{
|
|||
|
|
throw new InvalidOperationException("Center recognition failed on second attempt.");
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
$"Center recognized again: X={newCenter.Value.CenterX:F4}, Y={newCenter.Value.CenterY:F4}".LogInfo();
|
|||
|
|
|
|||
|
|
// <20><><EFBFBD>岽<EFBFBD><E5B2BD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD>λ<EFBFBD><CEBB>
|
|||
|
|
Point newRulerPoint = BuildRulerPoint(request.Axes);
|
|||
|
|
Point newPixelPoint = new Point(newCenter.Value.CenterX, newCenter.Value.CenterY);
|
|||
|
|
CoordinateTransformResult newTransform = await _coordinateTransformer.TransformAsync(
|
|||
|
|
newRulerPoint,
|
|||
|
|
newPixelPoint,
|
|||
|
|
cameraName).ConfigureAwait(false);
|
|||
|
|
|
|||
|
|
if (!newTransform.Succeeded)
|
|||
|
|
{
|
|||
|
|
throw new InvalidOperationException($"Coordinate transformation failed on second attempt: {newTransform.Message}");
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
Dictionary<string, double> errors = new Dictionary<string, double>();
|
|||
|
|
bool allWithinTolerance = true;
|
|||
|
|
|
|||
|
|
foreach (ApproachAlignmentAxis axis in request.Axes)
|
|||
|
|
{
|
|||
|
|
double currentAxisPos = GetCurrentAxisPosition(axis.AxisName);
|
|||
|
|
double expectedAxisPos = GetExpectedAxisPosition(axis.AxisName, newTransform.TargetPoint);
|
|||
|
|
double error = Math.Abs(currentAxisPos - expectedAxisPos);
|
|||
|
|
|
|||
|
|
errors[axis.AxisName] = error;
|
|||
|
|
result.FinalAxisPositions[axis.AxisName] = currentAxisPos;
|
|||
|
|
|
|||
|
|
if (error > axis.ToleranceValue)
|
|||
|
|
{
|
|||
|
|
allWithinTolerance = false;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
$"Axis: {axis.AxisName}, Current: {currentAxisPos:F4}, Expected: {expectedAxisPos:F4}, Error: {error:F6}, Tolerance: {axis.ToleranceValue:F4}, WithinTolerance: {error <= axis.ToleranceValue}".LogInfo();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
result.FinalErrors.Clear();
|
|||
|
|
foreach (KeyValuePair<string, double> error in errors)
|
|||
|
|
{
|
|||
|
|
result.FinalErrors.Add(error.Key, error.Value);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
result.CompletedIterations = iteration + 1;
|
|||
|
|
|
|||
|
|
if (allWithinTolerance)
|
|||
|
|
{
|
|||
|
|
result.Succeeded = true;
|
|||
|
|
$"Approach alignment succeeded after {result.CompletedIterations} iteration(s).".LogInfo();
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
center = newCenter;
|
|||
|
|
$"Approach alignment iteration {iteration + 1} completed. Not all axes within tolerance, continuing...".LogInfo();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (!result.Succeeded)
|
|||
|
|
{
|
|||
|
|
$"Approach alignment failed after maximum iterations ({request.MaxIterations}).".LogInfo();
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
catch (OperationCanceledException)
|
|||
|
|
{
|
|||
|
|
result.Exception = new OperationCanceledException("Approach alignment was cancelled.");
|
|||
|
|
"Approach alignment cancelled.".LogInfo();
|
|||
|
|
}
|
|||
|
|
catch (Exception ex)
|
|||
|
|
{
|
|||
|
|
result.Exception = ex;
|
|||
|
|
$"Approach alignment error: {ex.Message}".LogSysError();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return result;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public async Task<ApproachAlignmentResult> SingleAlignmentAsync(
|
|||
|
|
ApproachAlignmentRequest request,
|
|||
|
|
CancellationToken cancellationToken = default(CancellationToken))
|
|||
|
|
{
|
|||
|
|
if (request == null)
|
|||
|
|
{
|
|||
|
|
throw new ArgumentNullException(nameof(request));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
ApproachAlignmentResult result = new ApproachAlignmentResult();
|
|||
|
|
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|||
|
|
"Single alignment started.".LogInfo();
|
|||
|
|
|
|||
|
|
(double CenterX, double CenterY)? center = await RecognizeCenterAsync(
|
|||
|
|
request.Camera,
|
|||
|
|
request.RecognitionTimeoutMilliseconds,
|
|||
|
|
request.RecognitionParameters,
|
|||
|
|
cancellationToken).ConfigureAwait(false);
|
|||
|
|
if (center == null)
|
|||
|
|
{
|
|||
|
|
throw new InvalidOperationException("Center recognition failed.");
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Point rulerPoint = BuildRulerPoint(request.Axes);
|
|||
|
|
Point pixelPoint = new Point(center.Value.CenterX, center.Value.CenterY);
|
|||
|
|
string cameraName = GetCameraName(request.Camera);
|
|||
|
|
|
|||
|
|
CoordinateTransformResult transformResult = await _coordinateTransformer.TransformAsync(
|
|||
|
|
rulerPoint,
|
|||
|
|
pixelPoint,
|
|||
|
|
cameraName).ConfigureAwait(false);
|
|||
|
|
if (!transformResult.Succeeded)
|
|||
|
|
{
|
|||
|
|
throw new InvalidOperationException($"Coordinate transformation failed: {transformResult.Message}");
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
MotionMoveRequest[] moveRequests = CreateMoveRequests(
|
|||
|
|
request.Axes,
|
|||
|
|
transformResult.TargetPoint,
|
|||
|
|
request.MoveTimeoutMilliseconds);
|
|||
|
|
|
|||
|
|
MotionBatchResult moveResult = await _safeAxisMotion.SafeMoveAsync(cancellationToken, moveRequests).ConfigureAwait(false);
|
|||
|
|
moveResult.EnsureSuccess();
|
|||
|
|
|
|||
|
|
result.Succeeded = ProcessAlignmentResult(request.Axes, transformResult.TargetPoint, result);
|
|||
|
|
result.CompletedIterations = 1;
|
|||
|
|
|
|||
|
|
if (result.Succeeded)
|
|||
|
|
{
|
|||
|
|
"Single alignment completed successfully.".LogInfo();
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
result.Exception = new InvalidOperationException("Single alignment completed but result exceeded tolerance.");
|
|||
|
|
"Single alignment completed but result exceeded tolerance.".LogInfo();
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
catch (OperationCanceledException)
|
|||
|
|
{
|
|||
|
|
result.Exception = new OperationCanceledException("Single alignment was cancelled.");
|
|||
|
|
"Single alignment cancelled.".LogInfo();
|
|||
|
|
}
|
|||
|
|
catch (Exception ex)
|
|||
|
|
{
|
|||
|
|
result.Exception = ex;
|
|||
|
|
$"Single alignment error: {ex.Message}".LogSysError();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return result;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private async Task<(double CenterX, double CenterY)?> RecognizeCenterAsync(
|
|||
|
|
CameraType camera,
|
|||
|
|
int timeoutMilliseconds,
|
|||
|
|
CenterRecognitionParameters parameters,
|
|||
|
|
CancellationToken cancellationToken)
|
|||
|
|
{
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
return await _centerRecognizer.RecognizeCenterAsync(
|
|||
|
|
camera,
|
|||
|
|
timeoutMilliseconds,
|
|||
|
|
parameters,
|
|||
|
|
cancellationToken).ConfigureAwait(false);
|
|||
|
|
}
|
|||
|
|
catch (Exception ex)
|
|||
|
|
{
|
|||
|
|
$"Center recognition exception: {ex.Message}".LogSysError();
|
|||
|
|
throw;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private double GetCurrentAxisPosition(string axisName)
|
|||
|
|
{
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
MwFramework.Device.IAxis axis = _hardwareManager.GetAxisByName(axisName);
|
|||
|
|
if (axis == null)
|
|||
|
|
{
|
|||
|
|
throw new InvalidOperationException($"Axis '{axisName}' not found.");
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return axis.State != null ? axis.State.ActualPos : axis.GetPositionImmediate();
|
|||
|
|
}
|
|||
|
|
catch (Exception ex)
|
|||
|
|
{
|
|||
|
|
$"Failed to get current position for axis '{axisName}': {ex.Message}".LogSysError();
|
|||
|
|
throw;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private MotionMoveRequest[] CreateMoveRequests(
|
|||
|
|
IReadOnlyList<ApproachAlignmentAxis> axes,
|
|||
|
|
Point targetPoint,
|
|||
|
|
int moveTimeoutMilliseconds)
|
|||
|
|
{
|
|||
|
|
List<MotionMoveRequest> moveRequests = new List<MotionMoveRequest>();
|
|||
|
|
|
|||
|
|
foreach (ApproachAlignmentAxis axis in axes)
|
|||
|
|
{
|
|||
|
|
double targetPosition = GetExpectedAxisPosition(axis.AxisName, targetPoint);
|
|||
|
|
moveRequests.Add(MotionMoveRequest.ForAxisName(axis.AxisName, targetPosition, moveTimeoutMilliseconds));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return moveRequests.ToArray();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private bool ProcessAlignmentResult(
|
|||
|
|
IReadOnlyList<ApproachAlignmentAxis> axes,
|
|||
|
|
Point targetPoint,
|
|||
|
|
ApproachAlignmentResult result)
|
|||
|
|
{
|
|||
|
|
bool allWithinTolerance = true;
|
|||
|
|
|
|||
|
|
result.FinalErrors.Clear();
|
|||
|
|
|
|||
|
|
foreach (ApproachAlignmentAxis axis in axes)
|
|||
|
|
{
|
|||
|
|
double currentAxisPos = GetCurrentAxisPosition(axis.AxisName);
|
|||
|
|
double expectedAxisPos = GetExpectedAxisPosition(axis.AxisName, targetPoint);
|
|||
|
|
double error = Math.Abs(currentAxisPos - expectedAxisPos);
|
|||
|
|
|
|||
|
|
result.FinalErrors[axis.AxisName] = error;
|
|||
|
|
result.FinalAxisPositions[axis.AxisName] = currentAxisPos;
|
|||
|
|
|
|||
|
|
if (error > axis.ToleranceValue)
|
|||
|
|
{
|
|||
|
|
allWithinTolerance = false;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
$"Axis: {axis.AxisName}, Current: {currentAxisPos:F4}, Expected: {expectedAxisPos:F4}, Error: {error:F6}, Tolerance: {axis.ToleranceValue:F4}, WithinTolerance: {error <= axis.ToleranceValue}".LogInfo();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return allWithinTolerance;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private Point BuildRulerPoint(IReadOnlyList<ApproachAlignmentAxis> axes)
|
|||
|
|
{
|
|||
|
|
double x = 0;
|
|||
|
|
double y = 0;
|
|||
|
|
bool hasX = false;
|
|||
|
|
bool hasY = false;
|
|||
|
|
|
|||
|
|
foreach (ApproachAlignmentAxis axis in axes)
|
|||
|
|
{
|
|||
|
|
if (axis == null || string.IsNullOrWhiteSpace(axis.AxisName))
|
|||
|
|
{
|
|||
|
|
continue;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
double currentPosition = GetCurrentAxisPosition(axis.AxisName);
|
|||
|
|
if (IsYAxis(axis.AxisName))
|
|||
|
|
{
|
|||
|
|
if (!hasY)
|
|||
|
|
{
|
|||
|
|
y = currentPosition;
|
|||
|
|
hasY = true;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
if (!hasX)
|
|||
|
|
{
|
|||
|
|
x = currentPosition;
|
|||
|
|
hasX = true;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return new Point(x, y);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private double GetExpectedAxisPosition(string axisName, Point targetPoint)
|
|||
|
|
{
|
|||
|
|
return IsYAxis(axisName) ? targetPoint.Y : targetPoint.X;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private static bool IsYAxis(string axisName)
|
|||
|
|
{
|
|||
|
|
return !string.IsNullOrWhiteSpace(axisName) && axisName.IndexOf("Y", StringComparison.OrdinalIgnoreCase) >= 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private string GetCameraName(CameraType camera)
|
|||
|
|
{
|
|||
|
|
return _hardwareManager.GetCameraName(camera);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|