添加 MX-PD-盘古 项目文件
将 MX-PD-盘古 - new 目录下的所有文件添加到主仓库
This commit is contained in:
Shi.Ji
@@ -0,0 +1,408 @@
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user