添加 MX-PD-盘古 项目文件
将 MX-PD-盘古 - new 目录下的所有文件添加到主仓库
This commit is contained in:
Shi.Ji
@@ -0,0 +1,255 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
using MainShell.Common;
|
||||
using SemiconductorVisionAlgorithm.SemiParams;
|
||||
|
||||
namespace MainShell.Motion
|
||||
{
|
||||
#region 数据结构
|
||||
|
||||
/// <summary>
|
||||
/// 逼近对位的单轴配置
|
||||
/// </summary>
|
||||
public class ApproachAlignmentAxis
|
||||
{
|
||||
public ApproachAlignmentAxis(string axisName, double toleranceValue)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(axisName))
|
||||
{
|
||||
throw new ArgumentNullException(nameof(axisName));
|
||||
}
|
||||
|
||||
if (toleranceValue <= 0)
|
||||
{
|
||||
throw new ArgumentException("Tolerance value must be positive.", nameof(toleranceValue));
|
||||
}
|
||||
|
||||
AxisName = axisName;
|
||||
ToleranceValue = toleranceValue;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 轴名称（如 "X", "Y"）
|
||||
/// </summary>
|
||||
public string AxisName { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 该轴的允许误差（单位与轴位置一致，通常为 mm）
|
||||
/// </summary>
|
||||
public double ToleranceValue { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 轴的描述信息（用于日志）
|
||||
/// </summary>
|
||||
public string Description { get; set; }
|
||||
}
|
||||
|
||||
public enum CenterRecognitionType
|
||||
{
|
||||
Template,
|
||||
EdgeCircle
|
||||
}
|
||||
|
||||
public class CenterRecognitionParameters
|
||||
{
|
||||
|
||||
|
||||
public CenterRecognitionParameters()
|
||||
{
|
||||
MinScore = 0.8d;
|
||||
TemplatePath = "Template/Default";
|
||||
Type = CenterRecognitionType.Template;
|
||||
}
|
||||
|
||||
public string TemplatePath { get; set; }
|
||||
|
||||
public double MinScore { get; set; }
|
||||
|
||||
public bool UseRoi { get; set; }
|
||||
|
||||
public string RoiName { get; set; }
|
||||
|
||||
public CenterRecognitionType Type { get; set; }
|
||||
|
||||
public Rectangle1 rectangle { get; set; }
|
||||
}
|
||||
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 逼近对位请求参数
|
||||
/// </summary>
|
||||
public class ApproachAlignmentRequest
|
||||
{
|
||||
private IReadOnlyList<ApproachAlignmentAxis> _axes;
|
||||
|
||||
public ApproachAlignmentRequest(IEnumerable<ApproachAlignmentAxis> axes, CameraType camera = CameraType.TopPositionCamera)
|
||||
{
|
||||
if (axes == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(axes));
|
||||
}
|
||||
|
||||
var axesList = axes.ToList();
|
||||
if (axesList.Count == 0)
|
||||
{
|
||||
throw new ArgumentException("At least one axis is required.", nameof(axes));
|
||||
}
|
||||
|
||||
Axes = axesList.AsReadOnly();
|
||||
Camera = camera;
|
||||
MaxIterations = 5;
|
||||
MoveTimeoutMilliseconds = 30000;
|
||||
RecognitionTimeoutMilliseconds = 100000;
|
||||
RecognitionParameters = new CenterRecognitionParameters();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 逼近轴配置列表
|
||||
/// </summary>
|
||||
public IReadOnlyList<ApproachAlignmentAxis> Axes
|
||||
{
|
||||
get { return _axes; }
|
||||
private set { _axes = value; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 相机标识（用于采图和识别）
|
||||
/// </summary>
|
||||
public CameraType Camera { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 最大循环次数（默认 5）
|
||||
/// </summary>
|
||||
public int MaxIterations { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 轴运动的超时时间（毫秒，默认 30000）
|
||||
/// </summary>
|
||||
public int MoveTimeoutMilliseconds { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 识别中心的超时时间（毫秒，默认 10000）
|
||||
/// </summary>
|
||||
public int RecognitionTimeoutMilliseconds { get; set; }
|
||||
|
||||
public CenterRecognitionParameters RecognitionParameters { get; set; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 逼近对位的结果
|
||||
/// </summary>
|
||||
public class ApproachAlignmentResult
|
||||
{
|
||||
public ApproachAlignmentResult()
|
||||
{
|
||||
FinalErrors = new Dictionary<string, double>();
|
||||
FinalAxisPositions = new Dictionary<string, double>();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 是否成功（所有轴都在允许误差内）
|
||||
/// </summary>
|
||||
public bool Succeeded { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 实际完成的迭代次数
|
||||
/// </summary>
|
||||
public int CompletedIterations { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 最终的每轴误差（轴名 -> 误差值）
|
||||
/// </summary>
|
||||
public Dictionary<string, double> FinalErrors { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 最终的轴位置（轴名 -> 位置值，用于调试）
|
||||
/// </summary>
|
||||
public Dictionary<string, double> FinalAxisPositions { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 异常信息（如果失败）
|
||||
/// </summary>
|
||||
public Exception Exception { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取出错消息
|
||||
/// </summary>
|
||||
public string Message => Exception?.Message ?? (Succeeded ? "Approach alignment succeeded." : "Approach alignment failed.");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 坐标转换结果
|
||||
/// </summary>
|
||||
public class CoordinateTransformResult
|
||||
{
|
||||
/// <summary>
|
||||
/// 转换后的目标点坐标
|
||||
/// </summary>
|
||||
public System.Windows.Point TargetPoint { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 转换是否成功
|
||||
/// </summary>
|
||||
public bool Succeeded { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 错误或警告信息
|
||||
/// </summary>
|
||||
public string Message { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 转换过程中的异常
|
||||
/// </summary>
|
||||
public Exception Exception { get; set; }
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region 接口定义
|
||||
|
||||
/// <summary>
|
||||
/// 中心识别器接口：通过图像算法识别目标中心
|
||||
/// </summary>
|
||||
public interface ICenterRecognizer
|
||||
{
|
||||
/// <summary>
|
||||
/// 识别图像中心点
|
||||
/// </summary>
|
||||
/// <param name="camera">相机标识（用于指定采图源）</param>
|
||||
/// <param name="timeoutMilliseconds">识别超时时间（毫秒）</param>
|
||||
/// <param name="cancellationToken">取消令牌</param>
|
||||
/// <returns>中心点 (CenterX, CenterY) 或 null 表示识别失败</returns>
|
||||
Task<(double CenterX, double CenterY)?> RecognizeCenterAsync(
|
||||
CameraType camera,
|
||||
int timeoutMilliseconds,
|
||||
CancellationToken cancellationToken = default(CancellationToken));
|
||||
|
||||
Task<(double CenterX, double CenterY)?> RecognizeCenterAsync(
|
||||
CameraType camera,
|
||||
int timeoutMilliseconds,
|
||||
CenterRecognitionParameters parameters,
|
||||
CancellationToken cancellationToken = default(CancellationToken));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 坐标转换器接口：将图像中心点转换为各轴的目标位置
|
||||
/// </summary>
|
||||
public interface ICoordinateTransformer
|
||||
{
|
||||
/// <summary>
|
||||
/// 将图像中心点坐标转换为轴位置
|
||||
/// </summary>
|
||||
/// <param name="centerX">图像中心 X 坐标</param>
|
||||
/// <param name="centerY">图像中心 Y 坐标</param>
|
||||
/// <param name="axes">目标轴列表</param>
|
||||
/// <returns>转换结果，包含各轴的目标位置</returns>
|
||||
Task<CoordinateTransformResult> TransformAsync(
|
||||
System.Windows. Point ruler, System.Windows.Point pixel, string cameraName);
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,173 @@
|
||||
using MainShell.Common;
|
||||
using MainShell.Log;
|
||||
using MainShell.Vision;
|
||||
using System;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
using System.Windows;
|
||||
|
||||
namespace MainShell.Motion
|
||||
{
|
||||
/// <summary>
|
||||
/// 中心识别实现
|
||||
/// 通过模板匹配服务识别目标中心
|
||||
/// </summary>
|
||||
public class CenterRecognizer : ICenterRecognizer
|
||||
{
|
||||
private readonly IFindTemplateService _findTemplateService;
|
||||
private readonly IFindEdgeCircleService _findEdgeCircleService;
|
||||
|
||||
public CenterRecognizer(
|
||||
IFindTemplateService findTemplateService,
|
||||
IFindEdgeCircleService findEdgeCircleService)
|
||||
{
|
||||
_findTemplateService = findTemplateService ?? throw new ArgumentNullException(nameof(findTemplateService));
|
||||
_findEdgeCircleService = findEdgeCircleService ?? throw new ArgumentNullException(nameof(findEdgeCircleService));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 识别图像中心坐标
|
||||
/// </summary>
|
||||
public Task<(double CenterX, double CenterY)?> RecognizeCenterAsync(
|
||||
CameraType camera,
|
||||
int timeoutMilliseconds,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
return RecognizeCenterAsync(
|
||||
camera,
|
||||
timeoutMilliseconds,
|
||||
new CenterRecognitionParameters(),
|
||||
cancellationToken);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 识别图像中心坐标
|
||||
/// </summary>
|
||||
public async Task<(double CenterX, double CenterY)?> RecognizeCenterAsync(
|
||||
CameraType camera,
|
||||
int timeoutMilliseconds,
|
||||
CenterRecognitionParameters parameters,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
if (parameters == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(parameters));
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
using (CancellationTokenSource cts = new CancellationTokenSource(timeoutMilliseconds))
|
||||
using (CancellationTokenSource linkedCts = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken, cts.Token))
|
||||
{
|
||||
string.Format(
|
||||
"CenterRecognizer: Starting template recognition from camera '{0}' (timeout={1}ms, template='{2}', minScore={3}).",
|
||||
camera,
|
||||
timeoutMilliseconds,
|
||||
parameters.TemplatePath,
|
||||
parameters.MinScore).LogInfo();
|
||||
VisionProcessResult result = null;
|
||||
double centerX = 0;
|
||||
double centerY = 0;
|
||||
switch (parameters.Type)
|
||||
{
|
||||
case CenterRecognitionType.Template:
|
||||
{
|
||||
FindTemplateRequest request = BuildFindTemplateRequest(camera, timeoutMilliseconds,parameters);
|
||||
var templateResult = await _findTemplateService.ProcessAsync(request,linkedCts.Token).ConfigureAwait(false);
|
||||
if (templateResult.Succeeded && templateResult.Data != null)
|
||||
{
|
||||
centerX = templateResult.Data.CenterX;
|
||||
centerY = templateResult.Data.CenterY;
|
||||
}
|
||||
else
|
||||
{
|
||||
string.Format(
|
||||
"CenterRecognizer: Template recognition failed for camera '{0}': {1}",
|
||||
camera,
|
||||
templateResult.Message).LogSysError();
|
||||
return null;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case CenterRecognitionType.EdgeCircle:
|
||||
{
|
||||
FindEdgeCircleRequest request = BuildFindEdgeCircleRequest(camera,timeoutMilliseconds, parameters);
|
||||
var circleResult = await _findEdgeCircleService.ProcessAsync(request,linkedCts.Token).ConfigureAwait(false);
|
||||
if (circleResult.Succeeded && circleResult.Data != null)
|
||||
{
|
||||
centerX = circleResult.Data.CenterX;
|
||||
centerY = circleResult.Data.CenterY;
|
||||
}
|
||||
else
|
||||
{
|
||||
string.Format(
|
||||
"CenterRecognizer: Edge circle recognition failed for camera '{0}': {1}",
|
||||
camera,
|
||||
circleResult.Message).LogSysError();
|
||||
return null;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
string.Format(
|
||||
"CenterRecognizer: Recognition completed: X={0:F4}, Y={1:F4}",
|
||||
centerX,
|
||||
centerY).LogInfo();
|
||||
|
||||
return (centerX, centerY);
|
||||
}
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
string.Format(
|
||||
"CenterRecognizer: Template recognition timed out for camera '{0}'.",
|
||||
camera).LogSysError();
|
||||
return null;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
string.Format(
|
||||
"CenterRecognizer: Template recognition failed for camera '{0}': {1}",
|
||||
camera,
|
||||
ex.Message).LogSysError();
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
private static FindTemplateRequest BuildFindTemplateRequest(
|
||||
CameraType camera,
|
||||
int timeoutMilliseconds,
|
||||
CenterRecognitionParameters parameters)
|
||||
{
|
||||
FindTemplateParameters findTemplateParameters = new FindTemplateParameters();
|
||||
findTemplateParameters.MinScore = parameters.MinScore;
|
||||
findTemplateParameters.TemplatePath = parameters.TemplatePath;
|
||||
findTemplateParameters.UseRoi = parameters.UseRoi;
|
||||
findTemplateParameters.RoiName = parameters.RoiName;
|
||||
|
||||
FindTemplateRequest request = new FindTemplateRequest();
|
||||
request.CameraSource = camera;
|
||||
request.CaptureOptions = CameraCaptureOptions.CreateStream(timeoutMilliseconds);
|
||||
request.Parameters = findTemplateParameters;
|
||||
request.TimeoutMilliseconds = timeoutMilliseconds;
|
||||
return request;
|
||||
}
|
||||
|
||||
private static FindEdgeCircleRequest BuildFindEdgeCircleRequest(
|
||||
CameraType camera,
|
||||
int timeoutMilliseconds,
|
||||
CenterRecognitionParameters parameters)
|
||||
{
|
||||
FindEdgeCircleParameters findEdgeCircleParameters = new FindEdgeCircleParameters();
|
||||
findEdgeCircleParameters.RoiRect = parameters.rectangle;
|
||||
|
||||
FindEdgeCircleRequest request = new FindEdgeCircleRequest();
|
||||
request.CameraSource = camera;
|
||||
request.CaptureOptions = CameraCaptureOptions.CreateStream(timeoutMilliseconds);
|
||||
request.Parameters = findEdgeCircleParameters;
|
||||
request.TimeoutMilliseconds = timeoutMilliseconds;
|
||||
return request;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,60 @@
|
||||
using MainShell.Log;
|
||||
using SemiconductorVisionAlgorithm.SemiCalib;
|
||||
using System;
|
||||
using System.Threading.Tasks;
|
||||
using System.Windows;
|
||||
|
||||
namespace MainShell.Motion
|
||||
{
|
||||
/// <summary>
|
||||
/// 坐标转换实现
|
||||
/// 此实现为基础版本，实际应用中应由标定系统提供具体实现
|
||||
/// </summary>
|
||||
public class CoordinateTransformer : ICoordinateTransformer
|
||||
{
|
||||
/// <summary>
|
||||
/// 将图像坐标转换为轴位置
|
||||
/// </summary>
|
||||
public async Task<CoordinateTransformResult> TransformAsync(
|
||||
Point ruler, Point pixel, string cameraName)
|
||||
{
|
||||
try
|
||||
{
|
||||
CoordinateTransformResult result = new CoordinateTransformResult();
|
||||
|
||||
// 模拟异步操作
|
||||
await Task.Delay(10).ConfigureAwait(false);
|
||||
|
||||
// 此处应根据机器标定数据进行坐标变换
|
||||
// 示例流程：
|
||||
// 1. 获取相机标定参数（内参、外参）
|
||||
// 2. 获取机械臂/轴的标定数据
|
||||
// 3. 将图像坐标(centerX, centerY)转换为各轴的目标位置
|
||||
|
||||
SemiconductorVisionAlgorithm.SemiParams.Point semiRulerPoint = new SemiconductorVisionAlgorithm.SemiParams.Point(ruler.X, ruler.Y);
|
||||
SemiconductorVisionAlgorithm.SemiParams.Point semiPixelPoint = new SemiconductorVisionAlgorithm.SemiParams.Point(pixel.X, pixel.Y);
|
||||
|
||||
|
||||
CalibManager.Instance.get_real_by_pixel(cameraName, 0, semiRulerPoint, semiPixelPoint
|
||||
, out SemiconductorVisionAlgorithm.SemiParams.Point point);
|
||||
|
||||
result.TargetPoint = new Point(point.X, point.Y);
|
||||
result.Succeeded = true;
|
||||
|
||||
return result;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
string.Format("Coordinate transformation failed: {0}", ex.Message).LogSysError();
|
||||
return new CoordinateTransformResult
|
||||
{
|
||||
Succeeded = false,
|
||||
Message = ex.Message,
|
||||
Exception = ex
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,301 @@
|
||||
# 逼近对位功能快速参考
|
||||
|
||||
## 1. 文档范围
|
||||
|
||||
本文档基于当前仓库 `main` 分支最新已提交代码整理，描述 [`MainShell/Motion/ApproachAlignment`](D:/ww/盘古/PanGu.DieBonderApp/MainShell/Motion/ApproachAlignment) 目录下逼近对位模块的真实实现、调用方式和接入注意事项。
|
||||
|
||||
说明：
|
||||
- 本文档以 `git` 最新已提交代码为准。
|
||||
- 若旧 README、注释或设计文档与代码不一致，以代码为准。
|
||||
- 当前模块的流程框架已具备，但坐标变换仍需业务侧补齐真实实现。
|
||||
|
||||
## 2. 文件清单
|
||||
|
||||
| 文件 | 说明 |
|
||||
|------|------|
|
||||
| `ApproachAlignment.cs` | 数据结构和接口定义 |
|
||||
| `ApproachAlignmentService.cs` | 逼近对位主流程 |
|
||||
| `CenterRecognizer.cs` | 中心识别实现 |
|
||||
| `CoordinateTransformer.cs` | 坐标转换实现 |
|
||||
|
||||
## 3. 核心类型
|
||||
|
||||
当前代码中，以下类型定义在 [`ApproachAlignment.cs`](D:/ww/盘古/PanGu.DieBonderApp/MainShell/Motion/ApproachAlignment/ApproachAlignment.cs) 中：
|
||||
|
||||
- `ApproachAlignmentAxis`
|
||||
- `ApproachAlignmentRequest`
|
||||
- `ApproachAlignmentResult`
|
||||
- `CoordinateTransformResult`
|
||||
- `ICenterRecognizer`
|
||||
- `ICoordinateTransformer`
|
||||
|
||||
### 3.1 ApproachAlignmentAxis
|
||||
|
||||
用于描述参与逼近对位的单根轴：
|
||||
|
||||
- `AxisName`：轴名
|
||||
- `ToleranceValue`：该轴允许误差
|
||||
- `Description`：可选描述，用于日志或调试
|
||||
|
||||
### 3.2 ApproachAlignmentRequest
|
||||
|
||||
用于描述一次逼近对位请求：
|
||||
|
||||
- `Axes`：参与对位的轴列表
|
||||
- `Camera`：相机类型，默认 `CameraType.UpCamera`
|
||||
- `MaxIterations`：最大迭代次数，默认 5
|
||||
- `MoveTimeoutMilliseconds`：运动超时，默认 30000
|
||||
- `RecognitionTimeoutMilliseconds`：识别超时，默认 10000
|
||||
|
||||
### 3.3 ApproachAlignmentResult
|
||||
|
||||
用于描述逼近对位结果：
|
||||
|
||||
- `Succeeded`：是否成功收敛
|
||||
- `CompletedIterations`：实际完成的迭代次数
|
||||
- `FinalErrors`：各轴最终误差
|
||||
- `FinalAxisPositions`：各轴最终位置
|
||||
- `Exception`：失败或取消时的异常
|
||||
- `Message`：结果消息
|
||||
|
||||
## 4. 当前真实入口
|
||||
|
||||
当前 `git` 最新代码中，逼近对位的真实调用入口是：
|
||||
|
||||
- [`ApproachAlignmentService.cs`](D:/ww/盘古/PanGu.DieBonderApp/MainShell/Motion/ApproachAlignment/ApproachAlignmentService.cs)
|
||||
|
||||
主要方法：
|
||||
|
||||
```csharp
|
||||
Task<ApproachAlignmentResult> ApproachAlignmentAsync(
|
||||
ApproachAlignmentRequest request,
|
||||
CancellationToken cancellationToken = default(CancellationToken))
|
||||
```
|
||||
|
||||
注意：
|
||||
- 当前 `SafeAxisMotion` 中并没有 `ApproachAlignmentAsync()` 或 `ApproachAlignment()` 这组方法。
|
||||
- 旧 README 中写到 `SafeAxisMotion.ApproachAlignmentAsync()` 的内容，和当前代码不一致。
|
||||
- 实际接入时应直接注入并调用 `ApproachAlignmentService`。
|
||||
|
||||
## 5. 工作流程
|
||||
|
||||
[`ApproachAlignmentService.cs`](D:/ww/盘古/PanGu.DieBonderApp/MainShell/Motion/ApproachAlignment/ApproachAlignmentService.cs) 当前实现的是一个迭代式流程：
|
||||
|
||||
1. 根据请求进入循环，最多执行 `MaxIterations` 次。
|
||||
2. 首次迭代调用 `ICenterRecognizer.RecognizeCenterAsync()` 识别中心。
|
||||
3. 调用 `ICoordinateTransformer.TransformAsync()` 将视觉中心转换为各轴目标位置。
|
||||
4. 把转换结果封装为 `MotionMoveRequest` 数组。
|
||||
5. 调用 `SafeAxisMotion.SafeMoveAsync()` 执行多轴移动。
|
||||
6. 再次识别中心。
|
||||
7. 再次做坐标转换。
|
||||
8. 用“当前轴位置”和“新转换目标位置”计算误差。
|
||||
9. 如果所有轴误差都在各自 `ToleranceValue` 内，则成功结束。
|
||||
10. 否则继续下一轮迭代，直到达到最大迭代次数。
|
||||
|
||||
## 6. 必须实现或理解的接口
|
||||
|
||||
### 6.1 ICenterRecognizer
|
||||
|
||||
接口定义：
|
||||
|
||||
```csharp
|
||||
Task<(double CenterX, double CenterY)?> RecognizeCenterAsync(
|
||||
CameraType camera,
|
||||
int timeoutMilliseconds,
|
||||
CancellationToken cancellationToken = default(CancellationToken));
|
||||
```
|
||||
|
||||
当前默认实现是 [`CenterRecognizer.cs`](D:/ww/盘古/PanGu.DieBonderApp/MainShell/Motion/ApproachAlignment/CenterRecognizer.cs)。
|
||||
|
||||
它的真实行为是：
|
||||
- 依赖 `IVisionAlgorithmService`
|
||||
- 构造 `VisionProcessRequest`
|
||||
- `AlgorithmType = VisionAlgorithmType.FindCenter`
|
||||
- `CaptureOptions = CameraCaptureOptions.CreateStream(timeoutMilliseconds)`
|
||||
- 从结果里读取 `OffsetX` / `OffsetY`
|
||||
- 识别失败时返回 `null`
|
||||
|
||||
如果你们已有自己的视觉流程，也可以替换成自定义的 `ICenterRecognizer` 实现。
|
||||
|
||||
### 6.2 ICoordinateTransformer
|
||||
|
||||
接口定义：
|
||||
|
||||
```csharp
|
||||
Task<CoordinateTransformResult> TransformAsync(
|
||||
double centerX,
|
||||
double centerY,
|
||||
IEnumerable<ApproachAlignmentAxis> axes);
|
||||
```
|
||||
|
||||
当前默认实现是 [`CoordinateTransformer.cs`](D:/ww/盘古/PanGu.DieBonderApp/MainShell/Motion/ApproachAlignment/CoordinateTransformer.cs)。
|
||||
|
||||
但需要特别注意：
|
||||
- 当前实现只是占位版本
|
||||
- 它会给每根轴写入 `0.0` 作为目标位置
|
||||
- 然后直接返回 `Succeeded = true`
|
||||
|
||||
这表示：
|
||||
- 逼近对位的流程框架是完整的
|
||||
- 但真实标定坐标转换还没有完成
|
||||
- 若直接用于生产逻辑，目标位置会被错误地写成固定值
|
||||
|
||||
因此，接入前必须替换为你们设备真实可用的坐标变换实现。
|
||||
|
||||
## 7. 依赖关系
|
||||
|
||||
当前 `ApproachAlignmentService` 构造函数依赖：
|
||||
|
||||
- `SafeAxisMotion`
|
||||
- `ICenterRecognizer`
|
||||
- `ICoordinateTransformer`
|
||||
- `HardwareManager`
|
||||
|
||||
依赖注入示例：
|
||||
|
||||
```csharp
|
||||
protected override void ConfigureIoC(IStyletIoCBuilder builder)
|
||||
{
|
||||
builder.Bind<SafeAxisMotion>().ToSelf().InSingletonScope();
|
||||
builder.Bind<ICenterRecognizer>().To<CenterRecognizer>().InSingletonScope();
|
||||
builder.Bind<ICoordinateTransformer>().To<CalibratedCoordinateTransformer>().InSingletonScope();
|
||||
builder.Bind<ApproachAlignmentService>().ToSelf().InSingletonScope();
|
||||
}
|
||||
```
|
||||
|
||||
说明：
|
||||
- `CenterRecognizer` 可以直接复用当前实现
|
||||
- `ICoordinateTransformer` 建议替换为你们自己的真实标定实现，不建议直接使用当前占位版 `CoordinateTransformer`
|
||||
|
||||
## 8. 推荐接入方式
|
||||
|
||||
### 8.1 典型调用流程
|
||||
|
||||
```text
|
||||
1. 创建 ApproachAlignmentAxis 列表
|
||||
2. 创建 ApproachAlignmentRequest
|
||||
3. 调用 ApproachAlignmentService.ApproachAlignmentAsync()
|
||||
4. 检查 ApproachAlignmentResult.Succeeded
|
||||
5. 如失败，查看 Exception 或 Message
|
||||
6. 如成功，读取 FinalAxisPositions 和 FinalErrors
|
||||
```
|
||||
|
||||
### 8.2 示例代码
|
||||
|
||||
```csharp
|
||||
public async Task<ApproachAlignmentResult> RunApproachAlignmentAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
var request = new ApproachAlignmentRequest(
|
||||
new[]
|
||||
{
|
||||
new ApproachAlignmentAxis("Axis_X", 0.01) { Description = "X axis" },
|
||||
new ApproachAlignmentAxis("Axis_Y", 0.01) { Description = "Y axis" }
|
||||
},
|
||||
camera: CameraType.UpCamera)
|
||||
{
|
||||
MaxIterations = 5,
|
||||
MoveTimeoutMilliseconds = 30000,
|
||||
RecognitionTimeoutMilliseconds = 10000
|
||||
};
|
||||
|
||||
var result = await _approachAlignmentService.ApproachAlignmentAsync(
|
||||
request,
|
||||
cancellationToken);
|
||||
|
||||
if (!result.Succeeded)
|
||||
{
|
||||
throw result.Exception ?? new InvalidOperationException(result.Message);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
```
|
||||
|
||||
## 9. 自定义坐标转换器示例
|
||||
|
||||
以下示例展示如何替换当前占位版 `CoordinateTransformer`：
|
||||
|
||||
```csharp
|
||||
public class CalibratedCoordinateTransformer : ICoordinateTransformer
|
||||
{
|
||||
public Task<CoordinateTransformResult> TransformAsync(
|
||||
double centerX,
|
||||
double centerY,
|
||||
IEnumerable<ApproachAlignmentAxis> axes)
|
||||
{
|
||||
var result = new CoordinateTransformResult();
|
||||
|
||||
foreach (var axis in axes)
|
||||
{
|
||||
var target = CalculateTarget(axis.AxisName, centerX, centerY);
|
||||
result.AxisPositions[axis.AxisName] = target;
|
||||
}
|
||||
|
||||
result.Succeeded = true;
|
||||
return Task.FromResult(result);
|
||||
}
|
||||
|
||||
private double CalculateTarget(string axisName, double centerX, double centerY)
|
||||
{
|
||||
// 替换为实际标定变换逻辑
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
## 10. 日志输出示例
|
||||
|
||||
当前代码里，`ApproachAlignmentService` 会输出类似这样的日志：
|
||||
|
||||
```text
|
||||
[INFO] Approach alignment iteration 1/5 started.
|
||||
[INFO] Center recognized: X=1024.5600, Y=768.3200
|
||||
[INFO] Coordinate transformation completed: 2 axes.
|
||||
[INFO] Axes moved successfully: 2 axes completed.
|
||||
[INFO] Center recognized again: X=1023.9800, Y=768.1500
|
||||
[INFO] Axis: Axis_X, Current: 100.0250, Expected: 100.0200, Error: 0.005000, Tolerance: 0.0100, WithinTolerance: True
|
||||
[INFO] Axis: Axis_Y, Current: 200.0150, Expected: 200.0100, Error: 0.005000, Tolerance: 0.0100, WithinTolerance: True
|
||||
[INFO] Approach alignment succeeded after 1 iteration(s).
|
||||
```
|
||||
|
||||
如果异常发生，也会记录失败日志，例如：
|
||||
- `Center recognition failed.`
|
||||
- `Coordinate transformation failed: ...`
|
||||
- `Failed to get current position for axis ...`
|
||||
|
||||
## 11. 常见问题
|
||||
|
||||
### Q: 如何修改最大迭代次数？
|
||||
A: 设置 `ApproachAlignmentRequest.MaxIterations`。
|
||||
|
||||
### Q: 如何设置不同轴的允许误差？
|
||||
A: 为每个 `ApproachAlignmentAxis` 设置不同的 `ToleranceValue`。
|
||||
|
||||
### Q: 能否在执行中途取消？
|
||||
A: 可以，通过 `CancellationToken` 取消。取消后结果中的 `Exception` 会记录取消异常。
|
||||
|
||||
### Q: 如何查看每个轴的最终误差？
|
||||
A: 查看 `ApproachAlignmentResult.FinalErrors`。
|
||||
|
||||
### Q: 逼近失败时如何获取原因？
|
||||
A: 查看 `ApproachAlignmentResult.Exception` 或 `ApproachAlignmentResult.Message`。
|
||||
|
||||
### Q: 当前默认实现能直接用于生产吗？
|
||||
A: 不建议。`CenterRecognizer` 基本可接真实视觉流程，但 `CoordinateTransformer` 仍是占位实现，必须先替换。
|
||||
|
||||
## 12. 当前状态总结
|
||||
|
||||
当前 `ApproachAlignment` 模块的真实状态是：
|
||||
|
||||
- 数据结构已经完整
|
||||
- 主流程已经完整
|
||||
- 视觉识别已有可接真实系统的实现
|
||||
- 运动执行复用了 `SafeAxisMotion`
|
||||
- 坐标变换仍需业务侧补齐真实算法
|
||||
- README 中旧的 `SafeAxisMotion.ApproachAlignmentAsync()` 说法已经不适用
|
||||
|
||||
接入建议：
|
||||
- 直接注入 `ApproachAlignmentService`
|
||||
- 保留当前 `CenterRecognizer` 或替换成自定义实现
|
||||
- 优先补齐真实的 `ICoordinateTransformer`
|
||||
- 在真实设备上联调前，不要使用默认 `CoordinateTransformer` 做生产动作
|
||||
@@ -0,0 +1,248 @@
|
||||
using MainShell.Common;
|
||||
using MainShell.Hardware;
|
||||
using MainShell.Log;
|
||||
using MainShell.Parameter;
|
||||
using MaxwellFramework.Core.Attributes;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using static MainShell.ParaSetting.Model.SpeedSetting;
|
||||
|
||||
namespace MainShell.Motion
|
||||
{
|
||||
public enum AxisSpeedProfile
|
||||
{
|
||||
Low,
|
||||
Medium,
|
||||
High,
|
||||
Custom
|
||||
}
|
||||
|
||||
public sealed class AxisMotionProfile
|
||||
{
|
||||
public AxisMotionProfile(double velocity, double acceleration, double deceleration, double jerk)
|
||||
{
|
||||
Velocity = velocity;
|
||||
Acceleration = acceleration;
|
||||
Deceleration = deceleration;
|
||||
Jerk = jerk;
|
||||
}
|
||||
|
||||
public double Velocity { get; }
|
||||
public double Acceleration { get; }
|
||||
public double Deceleration { get; }
|
||||
public double Jerk { get; }
|
||||
}
|
||||
|
||||
[Singleton]
|
||||
public class AxisSpeedManager
|
||||
{
|
||||
private readonly GlobalParameterContext _globalParam;
|
||||
private readonly HardwareManager _hardware;
|
||||
|
||||
public AxisSpeedManager(GlobalParameterContext globalParam, HardwareManager hardware)
|
||||
{
|
||||
_globalParam = globalParam ?? throw new ArgumentNullException(nameof(globalParam));
|
||||
_hardware = hardware ?? throw new ArgumentNullException(nameof(hardware));
|
||||
}
|
||||
|
||||
public void SetAxisSpeed(string axisName, double speed)
|
||||
{
|
||||
var speedItem = GetRequiredSpeedTypeItem(axisName);
|
||||
var profile = CreateCustomProfile(speedItem, speed);
|
||||
ApplyAxisMotionProfile(axisName, profile, AxisSpeedProfile.Custom, null);
|
||||
}
|
||||
|
||||
public void SetAxisSpeed(string axisName, AxisSpeedProfile profile)
|
||||
{
|
||||
var speedItem = GetRequiredSpeedTypeItem(axisName);
|
||||
var motionProfile = CreateProfile(speedItem, profile);
|
||||
ApplyAxisMotionProfile(axisName, motionProfile, profile, speedItem);
|
||||
}
|
||||
|
||||
public void ApplySpeedSettings()
|
||||
{
|
||||
var speedItems = _globalParam?.SpeedParaSysSetting?.SpeedTypeItemCollection?.SpeedTypeItemList;
|
||||
if (speedItems == null || speedItems.Count == 0)
|
||||
{
|
||||
"Axis speed settings skipped because no speed configuration is available.".LogInfo();
|
||||
return;
|
||||
}
|
||||
|
||||
var currentProfile = MapCurrentSpeedTypeText(_globalParam.SpeedParaSysSetting.CurrentSpeedType.ToString());
|
||||
foreach (var speedItem in speedItems)
|
||||
{
|
||||
if (speedItem == null || string.IsNullOrWhiteSpace(speedItem.AxisName))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var profile = CreateProfile(speedItem, currentProfile);
|
||||
ApplyAxisMotionProfile(speedItem.AxisName, profile, currentProfile, speedItem);
|
||||
}
|
||||
}
|
||||
|
||||
private void ApplyAxisMotionProfile(string axisName, AxisMotionProfile profile, AxisSpeedProfile profileType, SpeedTypeItem speedItem)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(axisName))
|
||||
{
|
||||
throw new ArgumentNullException(nameof(axisName));
|
||||
}
|
||||
|
||||
var axis = _hardware.GetAxisByName(axisName);
|
||||
if (axis == null)
|
||||
{
|
||||
throw new ArgumentException(string.Format("Axis with name {0} not found.", axisName), nameof(axisName));
|
||||
}
|
||||
|
||||
ValidateMotionProfile(profile, axisName);
|
||||
|
||||
axis.Param.Velocity = profile.Velocity;
|
||||
axis.Param.Accelerate = profile.Acceleration;
|
||||
axis.Param.Decelerate = profile.Deceleration;
|
||||
axis.Param.Jerk = profile.Jerk;
|
||||
|
||||
try
|
||||
{
|
||||
axis.IssueParam();
|
||||
LogProfileApplied(axisName, profile, profileType, speedItem);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
string.Format("Axis:{0} speed profile apply failed. Profile:{1} Error:{2}", axisName, profileType, ex.Message).LogSysError();
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
private SpeedTypeItem GetRequiredSpeedTypeItem(string axisName)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(axisName))
|
||||
{
|
||||
throw new ArgumentNullException(nameof(axisName));
|
||||
}
|
||||
|
||||
var speedItem = _globalParam?.SpeedParaSysSetting?.SpeedTypeItemCollection?.SpeedTypeItemList?
|
||||
.FirstOrDefault(x => x != null && string.Equals(x.AxisName, axisName, StringComparison.OrdinalIgnoreCase));
|
||||
|
||||
if (speedItem == null)
|
||||
{
|
||||
throw new InvalidOperationException(string.Format("No speed configuration found for axis {0}.", axisName));
|
||||
}
|
||||
|
||||
return speedItem;
|
||||
}
|
||||
|
||||
private static AxisMotionProfile CreateCustomProfile(SpeedTypeItem speedItem, double speed)
|
||||
{
|
||||
ValidatePositiveFinite(speed, nameof(speed), speedItem.AxisName);
|
||||
ValidatePositiveFinite(speedItem.Acc, nameof(speedItem.Acc), speedItem.AxisName);
|
||||
ValidatePositiveFinite(speedItem.Dec, nameof(speedItem.Dec), speedItem.AxisName);
|
||||
ValidatePositiveFinite(speedItem.Jerk, nameof(speedItem.Jerk), speedItem.AxisName);
|
||||
ValidatePositiveFinite(speedItem.Speed, nameof(speedItem.Speed), speedItem.AxisName);
|
||||
|
||||
var scale = speed / speedItem.Speed;
|
||||
return new AxisMotionProfile(
|
||||
speed,
|
||||
speedItem.Acc * scale,
|
||||
speedItem.Dec * scale,
|
||||
speedItem.Jerk * scale);
|
||||
}
|
||||
|
||||
private static AxisMotionProfile CreateProfile(SpeedTypeItem speedItem, AxisSpeedProfile profile)
|
||||
{
|
||||
if (speedItem == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(speedItem));
|
||||
}
|
||||
|
||||
ValidatePositiveFinite(speedItem.Speed, nameof(speedItem.Speed), speedItem.AxisName);
|
||||
ValidatePositiveFinite(speedItem.Acc, nameof(speedItem.Acc), speedItem.AxisName);
|
||||
ValidatePositiveFinite(speedItem.Dec, nameof(speedItem.Dec), speedItem.AxisName);
|
||||
ValidatePositiveFinite(speedItem.Jerk, nameof(speedItem.Jerk), speedItem.AxisName);
|
||||
|
||||
var percent = ResolveProfilePercent(speedItem, profile);
|
||||
var scale = percent / 100.0d;
|
||||
return new AxisMotionProfile(
|
||||
speedItem.Speed * scale,
|
||||
speedItem.Acc * scale,
|
||||
speedItem.Dec * scale,
|
||||
speedItem.Jerk * scale);
|
||||
}
|
||||
|
||||
private static double ResolveProfilePercent(SpeedTypeItem speedItem, AxisSpeedProfile profile)
|
||||
{
|
||||
switch (profile)
|
||||
{
|
||||
case AxisSpeedProfile.Low:
|
||||
return ValidatePercent(speedItem.LowPercent, nameof(speedItem.LowPercent), speedItem.AxisName);
|
||||
case AxisSpeedProfile.Medium:
|
||||
return ValidatePercent(speedItem.MedianPercent, nameof(speedItem.MedianPercent), speedItem.AxisName);
|
||||
case AxisSpeedProfile.High:
|
||||
return ValidatePercent(speedItem.HighPercent, nameof(speedItem.HighPercent), speedItem.AxisName);
|
||||
default:
|
||||
return 100.0d;
|
||||
}
|
||||
}
|
||||
|
||||
private static AxisSpeedProfile MapCurrentSpeedTypeText(string currentSpeedTypeText)
|
||||
{
|
||||
if (string.Equals(currentSpeedTypeText, "High", StringComparison.OrdinalIgnoreCase))
|
||||
{
|
||||
return AxisSpeedProfile.High;
|
||||
}
|
||||
|
||||
if (string.Equals(currentSpeedTypeText, "Median", StringComparison.OrdinalIgnoreCase)
|
||||
|| string.Equals(currentSpeedTypeText, "Medium", StringComparison.OrdinalIgnoreCase))
|
||||
{
|
||||
return AxisSpeedProfile.Medium;
|
||||
}
|
||||
|
||||
return AxisSpeedProfile.Low;
|
||||
}
|
||||
|
||||
private static void ValidateMotionProfile(AxisMotionProfile profile, string axisName)
|
||||
{
|
||||
if (profile == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(profile));
|
||||
}
|
||||
|
||||
ValidatePositiveFinite(profile.Velocity, nameof(profile.Velocity), axisName);
|
||||
ValidatePositiveFinite(profile.Acceleration, nameof(profile.Acceleration), axisName);
|
||||
ValidatePositiveFinite(profile.Deceleration, nameof(profile.Deceleration), axisName);
|
||||
ValidatePositiveFinite(profile.Jerk, nameof(profile.Jerk), axisName);
|
||||
}
|
||||
|
||||
private static void ValidatePositiveFinite(double value, string propertyName, string axisName)
|
||||
{
|
||||
if (double.IsNaN(value) || double.IsInfinity(value) || value <= 0d)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(propertyName, string.Format("Axis {0} has invalid motion parameter {1}: {2}.", axisName, propertyName, value));
|
||||
}
|
||||
}
|
||||
|
||||
private static double ValidatePercent(double value, string propertyName, string axisName)
|
||||
{
|
||||
if (double.IsNaN(value) || double.IsInfinity(value) || value <= 0d || value > 100d)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(propertyName, string.Format("Axis {0} has invalid speed profile percent {1}: {2}.", axisName, propertyName, value));
|
||||
}
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
private static void LogProfileApplied(string axisName, AxisMotionProfile profile, AxisSpeedProfile profileType, SpeedTypeItem speedItem)
|
||||
{
|
||||
var source = speedItem != null ? "ConfiguredProfile" : "CustomScaledProfile";
|
||||
string.Format(
|
||||
"Axis:{0} speed profile applied. Source:{1} Profile:{2} Velocity:{3:F3} Acc:{4:F3} Dec:{5:F3} Jerk:{6:F3}",
|
||||
axisName,
|
||||
source,
|
||||
profileType,
|
||||
profile.Velocity,
|
||||
profile.Acceleration,
|
||||
profile.Deceleration,
|
||||
profile.Jerk).LogInfo();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,484 @@
|
||||
using MwFramework.Device;
|
||||
using MwFramework.Device.Motion;
|
||||
using System;
|
||||
using System.Diagnostics;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace MainShell.Motion
|
||||
{
|
||||
public sealed class MotionController
|
||||
{
|
||||
public const int DefaultTimeoutMilliseconds = 30000;
|
||||
private const int PollingIntervalMilliseconds = 20;
|
||||
private const double DefaultInPositionTolerance = 0.001d;
|
||||
private readonly SemaphoreSlim _motionLock = new SemaphoreSlim(1, 1);
|
||||
|
||||
public MotionController(IAxis axis)
|
||||
{
|
||||
Axis = axis ?? throw new ArgumentNullException(nameof(axis));
|
||||
AxisFunc = axis as IAxisFunc ?? throw new ArgumentException("Axis does not support motion commands.", nameof(axis));
|
||||
}
|
||||
|
||||
private IAxis Axis { get; }
|
||||
private IAxisFunc AxisFunc { get; }
|
||||
|
||||
public string AxisName => Axis.Name;
|
||||
public double CurrentPos => Axis.State != null ? Axis.State.ActualPos : Axis.GetPositionImmediate();
|
||||
public bool IsBusy => Axis.State != null && Axis.State.Moving;
|
||||
public bool IsAlarm => Axis.State != null && (Axis.State.ALM || Axis.State.ServoFault || Axis.State.EMG);
|
||||
public bool InPos => Axis.State != null && Axis.State.Inpos;
|
||||
|
||||
public event EventHandler<MotionStartedEventArgs> MotionStarted;
|
||||
public event EventHandler<MotionFinishedEventArgs> MotionFinished;
|
||||
|
||||
public async Task<MotionResult> MoveAbsAsync(double targetPos, int timeoutMilliseconds = DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken), double? positionTolerance = null)
|
||||
{
|
||||
await _motionLock.WaitAsync(cancellationToken).ConfigureAwait(false);
|
||||
try
|
||||
{
|
||||
return await ExecuteProtectedMotionAsync(
|
||||
MotionOperation.MoveAbs,
|
||||
targetPos,
|
||||
timeoutMilliseconds,
|
||||
cancellationToken,
|
||||
prepareAction: () =>
|
||||
{
|
||||
EnsureMotionStateReady();
|
||||
// EnsureWithinSoftLimit(targetPos);
|
||||
Axis.Param.AbsPos = targetPos;
|
||||
},
|
||||
executeAsync: () =>
|
||||
{
|
||||
return Task.FromResult(Axis.AbsoluteMove());
|
||||
},
|
||||
positionTolerance: positionTolerance).ConfigureAwait(false);
|
||||
}
|
||||
finally
|
||||
{
|
||||
_motionLock.Release();
|
||||
}
|
||||
}
|
||||
|
||||
public async Task<MotionResult> MoveRelAsync(double distance, int timeoutMilliseconds = DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken), double? positionTolerance = null)
|
||||
{
|
||||
await _motionLock.WaitAsync(cancellationToken).ConfigureAwait(false);
|
||||
try
|
||||
{
|
||||
var targetPos = CurrentPos + distance;
|
||||
return await ExecuteProtectedMotionAsync(
|
||||
MotionOperation.MoveRel,
|
||||
targetPos,
|
||||
timeoutMilliseconds,
|
||||
cancellationToken,
|
||||
prepareAction: () =>
|
||||
{
|
||||
EnsureMotionStateReady();
|
||||
EnsureWithinSoftLimit(targetPos);
|
||||
Axis.Param.IncPos = distance;
|
||||
},
|
||||
executeAsync: () => AxisFunc.RelativeMoveAsync(),
|
||||
positionTolerance: positionTolerance).ConfigureAwait(false);
|
||||
}
|
||||
finally
|
||||
{
|
||||
_motionLock.Release();
|
||||
}
|
||||
}
|
||||
|
||||
public async Task<MotionResult> JogAsync(Dir direction, int durationMilliseconds, int timeoutMilliseconds = DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken))
|
||||
{
|
||||
if (durationMilliseconds <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(durationMilliseconds));
|
||||
}
|
||||
|
||||
await _motionLock.WaitAsync(cancellationToken).ConfigureAwait(false);
|
||||
try
|
||||
{
|
||||
return await ExecuteProtectedMotionAsync(
|
||||
MotionOperation.Jog,
|
||||
null,
|
||||
timeoutMilliseconds,
|
||||
cancellationToken,
|
||||
prepareAction: () =>
|
||||
{
|
||||
EnsureMotionStateReady();
|
||||
if (timeoutMilliseconds != Timeout.Infinite && durationMilliseconds > timeoutMilliseconds)
|
||||
{
|
||||
throw new TimeoutException(string.Format("Jog timeout before completion. Axis: {0}", AxisName));
|
||||
}
|
||||
},
|
||||
executeAsync: async () =>
|
||||
{
|
||||
EnsureNoError(AxisFunc.JogStart(direction, false), MotionOperation.Jog);
|
||||
await Task.Delay(durationMilliseconds, cancellationToken).ConfigureAwait(false);
|
||||
return AxisFunc.StopMove();
|
||||
}).ConfigureAwait(false);
|
||||
}
|
||||
finally
|
||||
{
|
||||
_motionLock.Release();
|
||||
}
|
||||
}
|
||||
|
||||
public async Task<MotionResult> StopAsync(int timeoutMilliseconds = 3000, CancellationToken cancellationToken = default(CancellationToken))
|
||||
{
|
||||
await _motionLock.WaitAsync(cancellationToken).ConfigureAwait(false);
|
||||
try
|
||||
{
|
||||
return await ExecuteProtectedMotionAsync(
|
||||
MotionOperation.Stop,
|
||||
null,
|
||||
timeoutMilliseconds,
|
||||
cancellationToken,
|
||||
prepareAction: null,
|
||||
executeAsync: async () =>
|
||||
{
|
||||
var stopResult = AxisFunc.StopMove();
|
||||
await Task.CompletedTask.ConfigureAwait(false);
|
||||
return stopResult;
|
||||
},
|
||||
skipCommandCompletionWait: true,
|
||||
skipInPositionValidation: true).ConfigureAwait(false);
|
||||
}
|
||||
finally
|
||||
{
|
||||
_motionLock.Release();
|
||||
}
|
||||
}
|
||||
|
||||
public async Task<MotionResult> HomeAsync(int timeoutMilliseconds = DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken))
|
||||
{
|
||||
await _motionLock.WaitAsync(cancellationToken).ConfigureAwait(false);
|
||||
try
|
||||
{
|
||||
return await ExecuteProtectedMotionAsync(
|
||||
MotionOperation.Home,
|
||||
null,
|
||||
timeoutMilliseconds,
|
||||
cancellationToken,
|
||||
prepareAction: EnsureMotionStateReady,
|
||||
executeAsync: () => Task.Run(() => AxisFunc.Home())).ConfigureAwait(false);
|
||||
}
|
||||
finally
|
||||
{
|
||||
_motionLock.Release();
|
||||
}
|
||||
}
|
||||
|
||||
private async Task<MotionResult> ExecuteProtectedMotionAsync(MotionOperation operation, double? targetPosition, int timeoutMilliseconds, CancellationToken cancellationToken, Action prepareAction, Func<Task<MotionErrorCode>> executeAsync, bool skipCommandCompletionWait = false, bool skipInPositionValidation = false, double? positionTolerance = null)
|
||||
{
|
||||
ValidateTimeout(timeoutMilliseconds);
|
||||
|
||||
var startPosition = CurrentPos;
|
||||
var startedAt = DateTime.UtcNow;
|
||||
OnMotionStarted(new MotionStartedEventArgs(AxisName, operation, targetPosition, startPosition, startedAt));
|
||||
|
||||
Exception failure = null;
|
||||
var cancelled = false;
|
||||
MotionResult motionResult = null;
|
||||
var watch = Stopwatch.StartNew();
|
||||
|
||||
using (cancellationToken.Register(TryStop))
|
||||
{
|
||||
try
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
prepareAction?.Invoke();
|
||||
var commandTask = executeAsync != null ? executeAsync() : throw new InvalidOperationException("Motion execute delegate is null.");
|
||||
if (!skipCommandCompletionWait)
|
||||
{
|
||||
var result = await WaitForCommandAsync(commandTask, watch, timeoutMilliseconds, cancellationToken).ConfigureAwait(false);
|
||||
EnsureNoError(result, operation);
|
||||
}
|
||||
else
|
||||
{
|
||||
EnsureNoError(await commandTask.ConfigureAwait(false), operation);
|
||||
}
|
||||
|
||||
await WaitForMotionCompletedAsync(operation, targetPosition, watch, timeoutMilliseconds, cancellationToken, skipInPositionValidation, positionTolerance).ConfigureAwait(false);
|
||||
}
|
||||
catch (OperationCanceledException ex)
|
||||
{
|
||||
cancelled = true;
|
||||
failure = ex;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
failure = ex;
|
||||
}
|
||||
finally
|
||||
{
|
||||
var finishedAt = DateTime.UtcNow;
|
||||
motionResult = CreateMotionResult(operation, targetPosition, startPosition, CurrentPos, failure == null && !cancelled, cancelled, failure, startedAt, finishedAt);
|
||||
OnMotionFinished(new MotionFinishedEventArgs(motionResult));
|
||||
}
|
||||
}
|
||||
|
||||
return motionResult;
|
||||
}
|
||||
|
||||
private MotionResult CreateMotionResult(MotionOperation operation, double? targetPosition, double startPosition, double endPosition, bool succeeded, bool cancelled, Exception exception, DateTime startedAtUtc, DateTime finishedAtUtc)
|
||||
{
|
||||
return new MotionResult(AxisName, operation, targetPosition, startPosition, endPosition, succeeded, cancelled, exception, exception?.Message, false, startedAtUtc, finishedAtUtc);
|
||||
}
|
||||
|
||||
private async Task<MotionErrorCode> WaitForCommandAsync(Task<MotionErrorCode> commandTask, Stopwatch watch, int timeoutMilliseconds, CancellationToken cancellationToken)
|
||||
{
|
||||
if (commandTask == null)
|
||||
{
|
||||
throw new InvalidOperationException("Motion command task is null.");
|
||||
}
|
||||
|
||||
while (!commandTask.IsCompleted)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
EnsureMotionStateHealthyDuringExecution();
|
||||
ThrowIfTimeoutExceeded(timeoutMilliseconds, watch);
|
||||
await Task.Delay(PollingIntervalMilliseconds, cancellationToken).ConfigureAwait(false);
|
||||
}
|
||||
|
||||
return await commandTask.ConfigureAwait(false);
|
||||
}
|
||||
|
||||
private async Task WaitForMotionCompletedAsync(MotionOperation operation, double? targetPosition, Stopwatch watch, int timeoutMilliseconds, CancellationToken cancellationToken, bool skipInPositionValidation, double? positionTolerance)
|
||||
{
|
||||
Task<bool> waitPositionTask = StartWaitPositionTask(watch, timeoutMilliseconds, cancellationToken);
|
||||
|
||||
while (true)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
EnsureMotionStateHealthyDuringExecution();
|
||||
ThrowIfTimeoutExceeded(timeoutMilliseconds, watch);
|
||||
|
||||
if (waitPositionTask.IsCompleted)
|
||||
{
|
||||
bool waitCompleted = await waitPositionTask.ConfigureAwait(false);
|
||||
if (!waitCompleted)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
throw new TimeoutException(string.Format("WaitPosition failed. Axis: {0}", AxisName));
|
||||
}
|
||||
|
||||
if (IsTargetReached(operation, targetPosition, skipInPositionValidation, positionTolerance))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
throw new InvalidOperationException(string.Format("Axis wait completed but target position was not reached. Axis: {0}", AxisName));
|
||||
}
|
||||
|
||||
await Task.Delay(PollingIntervalMilliseconds, cancellationToken).ConfigureAwait(false);
|
||||
}
|
||||
}
|
||||
|
||||
private Task<bool> StartWaitPositionTask(Stopwatch watch, int timeoutMilliseconds, CancellationToken cancellationToken)
|
||||
{
|
||||
int remainingTimeoutMilliseconds = GetRemainingTimeoutMilliseconds(watch, timeoutMilliseconds);
|
||||
return Task.Run(() => AxisFunc.WaitPosition(remainingTimeoutMilliseconds, cancellationToken), cancellationToken);
|
||||
}
|
||||
|
||||
private int GetRemainingTimeoutMilliseconds(Stopwatch watch, int timeoutMilliseconds)
|
||||
{
|
||||
if (timeoutMilliseconds == Timeout.Infinite)
|
||||
{
|
||||
return Timeout.Infinite;
|
||||
}
|
||||
|
||||
long remainingMilliseconds = timeoutMilliseconds - watch.ElapsedMilliseconds;
|
||||
if (remainingMilliseconds <= 0)
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (remainingMilliseconds > int.MaxValue)
|
||||
{
|
||||
return int.MaxValue;
|
||||
}
|
||||
|
||||
return (int)remainingMilliseconds;
|
||||
}
|
||||
|
||||
private bool IsTargetReached(MotionOperation operation, double? targetPosition, bool skipInPositionValidation, double? positionTolerance)
|
||||
{
|
||||
if (operation == MotionOperation.Stop)
|
||||
{
|
||||
return !IsBusy;
|
||||
}
|
||||
|
||||
if(positionTolerance == null)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
if (skipInPositionValidation || !targetPosition.HasValue)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
if (InPos)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
var tolerance = positionTolerance.HasValue && positionTolerance.Value > 0d ? positionTolerance.Value : DefaultInPositionTolerance;
|
||||
return Math.Abs(CurrentPos - targetPosition.Value) <= tolerance;
|
||||
}
|
||||
|
||||
private void EnsureMotionStateReady()
|
||||
{
|
||||
var state = Axis.State;
|
||||
if (state == null)
|
||||
{
|
||||
throw new InvalidOperationException(string.Format("Axis state is unavailable. Axis: {0}", AxisName));
|
||||
}
|
||||
|
||||
if (!state.Servo)
|
||||
{
|
||||
throw new InvalidOperationException(string.Format("Axis servo is off. Axis: {0}", AxisName));
|
||||
}
|
||||
|
||||
if (state.ALM || state.ServoFault || state.EMG)
|
||||
{
|
||||
throw new InvalidOperationException(string.Format("Axis is in alarm state. Axis: {0}", AxisName));
|
||||
}
|
||||
|
||||
//if (!state.CanMove)
|
||||
//{
|
||||
// throw new InvalidOperationException(string.Format("Axis cannot move. Axis: {0}", AxisName));
|
||||
//}
|
||||
}
|
||||
|
||||
private void EnsureMotionStateHealthyDuringExecution()
|
||||
{
|
||||
var state = Axis.State;
|
||||
if (state == null)
|
||||
{
|
||||
throw new InvalidOperationException(string.Format("Axis state is unavailable during motion. Axis: {0}", AxisName));
|
||||
}
|
||||
|
||||
if (state.ALM || state.ServoFault || state.EMG)
|
||||
{
|
||||
throw new InvalidOperationException(string.Format("Axis entered alarm state during motion. Axis: {0}", AxisName));
|
||||
}
|
||||
|
||||
if (!state.Servo)
|
||||
{
|
||||
throw new InvalidOperationException(string.Format("Axis servo turned off during motion. Axis: {0}", AxisName));
|
||||
}
|
||||
|
||||
//if (!Axis.IsMotionDone())
|
||||
//{
|
||||
// throw new InvalidOperationException(string.Format("Axis cannot continue motion. Axis: {0}", AxisName));
|
||||
//}
|
||||
}
|
||||
|
||||
private void EnsureWithinSoftLimit(double targetPos)
|
||||
{
|
||||
double softMel = 0d;
|
||||
if (AxisFunc.GetSoftMel(ref softMel) == MotionErrorCode.NoError && targetPos < softMel)
|
||||
{
|
||||
throw new InvalidOperationException(string.Format("Target position {0} is below the negative soft limit {1}. Axis: {2}", targetPos, softMel, AxisName));
|
||||
}
|
||||
|
||||
double softPel = 0d;
|
||||
if (AxisFunc.GetSoftPel(ref softPel) == MotionErrorCode.NoError && targetPos > softPel)
|
||||
{
|
||||
throw new InvalidOperationException(string.Format("Target position {0} is above the positive soft limit {1}. Axis: {2}", targetPos, softPel, AxisName));
|
||||
}
|
||||
}
|
||||
|
||||
private static void ValidateTimeout(int timeoutMilliseconds)
|
||||
{
|
||||
if (timeoutMilliseconds <= 0 && timeoutMilliseconds != Timeout.Infinite)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(timeoutMilliseconds));
|
||||
}
|
||||
}
|
||||
|
||||
private void ThrowIfTimeoutExceeded(int timeoutMilliseconds, Stopwatch watch)
|
||||
{
|
||||
if (timeoutMilliseconds != Timeout.Infinite && watch.ElapsedMilliseconds > timeoutMilliseconds)
|
||||
{
|
||||
TryStop();
|
||||
throw new TimeoutException(string.Format("Motion timeout. Axis: {0}", AxisName));
|
||||
}
|
||||
}
|
||||
|
||||
private void EnsureNoError(MotionErrorCode errorCode, MotionOperation operation)
|
||||
{
|
||||
if (errorCode != MotionErrorCode.NoError)
|
||||
{
|
||||
throw new InvalidOperationException(string.Format("{0} failed on axis {1}. Error: {2}", operation, AxisName, errorCode));
|
||||
}
|
||||
}
|
||||
|
||||
private void TryStop()
|
||||
{
|
||||
try
|
||||
{
|
||||
AxisFunc.StopMove();
|
||||
}
|
||||
catch
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
private void OnMotionStarted(MotionStartedEventArgs args)
|
||||
{
|
||||
MotionStarted?.Invoke(this, args);
|
||||
}
|
||||
|
||||
private void OnMotionFinished(MotionFinishedEventArgs args)
|
||||
{
|
||||
MotionFinished?.Invoke(this, args);
|
||||
}
|
||||
}
|
||||
|
||||
public enum MotionOperation
|
||||
{
|
||||
MoveAbs,
|
||||
MoveRel,
|
||||
Jog,
|
||||
Stop,
|
||||
Home
|
||||
}
|
||||
|
||||
public sealed class MotionStartedEventArgs : EventArgs
|
||||
{
|
||||
public MotionStartedEventArgs(string axisName, MotionOperation operation, double? targetPosition, double startPosition, DateTime startedAtUtc)
|
||||
{
|
||||
AxisName = axisName;
|
||||
Operation = operation;
|
||||
TargetPosition = targetPosition;
|
||||
StartPosition = startPosition;
|
||||
StartedAtUtc = startedAtUtc;
|
||||
}
|
||||
|
||||
public string AxisName { get; }
|
||||
public MotionOperation Operation { get; }
|
||||
public double? TargetPosition { get; }
|
||||
public double StartPosition { get; }
|
||||
public DateTime StartedAtUtc { get; }
|
||||
}
|
||||
|
||||
public sealed class MotionFinishedEventArgs : EventArgs
|
||||
{
|
||||
public MotionFinishedEventArgs(MotionResult result)
|
||||
{
|
||||
Result = result ?? throw new ArgumentNullException(nameof(result));
|
||||
}
|
||||
|
||||
public MotionResult Result { get; }
|
||||
public string AxisName => Result.AxisName;
|
||||
public MotionOperation Operation => Result.Operation;
|
||||
public double? TargetPosition => Result.TargetPosition;
|
||||
public double EndPosition => Result.EndPosition;
|
||||
public bool Succeeded => Result.Succeeded;
|
||||
public bool Cancelled => Result.Cancelled;
|
||||
public Exception Exception => Result.Exception;
|
||||
public DateTime StartedAtUtc => Result.StartedAtUtc;
|
||||
public DateTime FinishedAtUtc => Result.FinishedAtUtc;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,207 @@
|
||||
using MaxwellFramework.Core.Attributes;
|
||||
using MainShell.Alarm;
|
||||
using MainShell.Hardware;
|
||||
using MainShell.Log;
|
||||
using MainShell.Motion.Safety;
|
||||
using MwFramework.Device;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace MainShell.Motion
|
||||
{
|
||||
[Singleton]
|
||||
public sealed class MotionAlarmReporter
|
||||
{
|
||||
private readonly AlarmOperate _alarmOperate;
|
||||
|
||||
public MotionAlarmReporter(AlarmOperate alarmOperate)
|
||||
{
|
||||
_alarmOperate = alarmOperate ?? throw new ArgumentNullException(nameof(alarmOperate));
|
||||
}
|
||||
|
||||
public async Task<bool> ReportAlarmAsync(int? alarmId)
|
||||
{
|
||||
if (!alarmId.HasValue)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
await _alarmOperate.AlertAsync(alarmId.Value).ConfigureAwait(false);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
[Singleton]
|
||||
public sealed class StagePlatformSafetyOptionsProvider
|
||||
{
|
||||
private const double DefaultMaxPlaneSpread = 5.0d;
|
||||
private const double DefaultMaxTravelPerStep = 10.0d;
|
||||
|
||||
public StagePlatformSafetyOptions GetOptions()
|
||||
{
|
||||
return new StagePlatformSafetyOptions(DefaultMaxPlaneSpread, DefaultMaxTravelPerStep);
|
||||
}
|
||||
}
|
||||
|
||||
[Singleton]
|
||||
public sealed class MotionPrecheckService
|
||||
{
|
||||
private readonly MotionAlarmReporter _motionAlarmReporter;
|
||||
private readonly IReadOnlyList<IMotionSafetyCheck> _motionSafetyChecks;
|
||||
|
||||
public MotionPrecheckService(HardwareManager hardware, MotionSafetyStateProvider motionSafetyStateProvider, MotionAlarmReporter motionAlarmReporter, StagePlatformSafetyOptionsProvider stagePlatformSafetyOptionsProvider)
|
||||
{
|
||||
if (hardware == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(hardware));
|
||||
}
|
||||
|
||||
if (motionSafetyStateProvider == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(motionSafetyStateProvider));
|
||||
}
|
||||
|
||||
_motionAlarmReporter = motionAlarmReporter ?? throw new ArgumentNullException(nameof(motionAlarmReporter));
|
||||
var stagePlatformSafetyOptions = (stagePlatformSafetyOptionsProvider ?? throw new ArgumentNullException(nameof(stagePlatformSafetyOptionsProvider))).GetOptions();
|
||||
_motionSafetyChecks = new IMotionSafetyCheck[]
|
||||
{
|
||||
new MotionRequestDuplicateAxisCheck(),
|
||||
new MotionBatchSameSourceCheck(),
|
||||
new EmergencyStopReleasedCheck(motionSafetyStateProvider),
|
||||
new SafetyDoorClosedCheck(motionSafetyStateProvider),
|
||||
new AutoFlowModeCheck(motionSafetyStateProvider),
|
||||
new StageVacuumReadyCheck(motionSafetyStateProvider),
|
||||
new BondHeadSafePositionCheck(motionSafetyStateProvider),
|
||||
new StagePlatformPlaneMoveSafetyCheck(hardware, stagePlatformSafetyOptions)
|
||||
};
|
||||
}
|
||||
|
||||
public async Task ValidateAsync(MotionSafetyContext context, CancellationToken cancellationToken, int? fallbackAlarmId)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
if (context == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(context));
|
||||
}
|
||||
|
||||
foreach (var check in _motionSafetyChecks)
|
||||
{
|
||||
var result = check.Check(context);
|
||||
if (result == null || result.IsPassed)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var failureMessage = string.Format("Motion precheck failed. Rule:{0} Kind:{1} Source:{2} Message:{3}", result.RuleName, context.RequestKind, context.Source ?? "N/A", result.Message);
|
||||
failureMessage.LogSysError();
|
||||
var alarmId = result.AlarmId ?? fallbackAlarmId;
|
||||
if (alarmId.HasValue)
|
||||
{
|
||||
await _motionAlarmReporter.ReportAlarmAsync(alarmId).ConfigureAwait(false);
|
||||
}
|
||||
|
||||
throw new InvalidOperationException(failureMessage);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
[Singleton]
|
||||
public sealed class MotionExecutionLogger
|
||||
{
|
||||
public void HandleMotionStarted(object sender, MotionStartedEventArgs e)
|
||||
{
|
||||
try
|
||||
{
|
||||
string.Format("Axis:{0} Motion:{1} Started StartPos:{2:F3} Target:{3}", e.AxisName, e.Operation, e.StartPosition, e.TargetPosition.HasValue ? e.TargetPosition.Value.ToString("F3") : "N/A").LogInfo();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
string.Format("Axis:{0} motion start log failed. Error:{1}", e.AxisName, ex.Message).LogSysError();
|
||||
}
|
||||
}
|
||||
|
||||
public void HandleMotionFinished(object sender, MotionFinishedEventArgs e)
|
||||
{
|
||||
try
|
||||
{
|
||||
var result = e.Result;
|
||||
var message = string.Format("Axis:{0} Motion:{1} Finished Success:{2} Cancelled:{3} EndPos:{4:F3} Duration:{5}ms FailureStage:{6}", result.AxisName, result.Operation, result.Succeeded, result.Cancelled, result.EndPosition, result.DurationMilliseconds, result.FailureStage ?? "N/A");
|
||||
if (result.Succeeded)
|
||||
{
|
||||
message.LogInfo();
|
||||
return;
|
||||
}
|
||||
|
||||
var error = string.Format("{0} Error:{1}", message, result.Exception);
|
||||
error.LogSysError();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
string.Format("Axis:{0} motion finish log failed. Error:{1}", e.AxisName, ex.Message).LogSysError();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
[Singleton]
|
||||
public sealed class MotionControllerRegistry
|
||||
{
|
||||
private readonly Dictionary<string, ControllerEntry> _controllers = new Dictionary<string, ControllerEntry>(StringComparer.OrdinalIgnoreCase);
|
||||
private readonly object _syncRoot = new object();
|
||||
private readonly MotionExecutionLogger _motionExecutionLogger;
|
||||
|
||||
private sealed class ControllerEntry
|
||||
{
|
||||
public ControllerEntry(IAxis axis, MotionController controller)
|
||||
{
|
||||
Axis = axis ?? throw new ArgumentNullException(nameof(axis));
|
||||
Controller = controller ?? throw new ArgumentNullException(nameof(controller));
|
||||
}
|
||||
|
||||
public IAxis Axis { get; }
|
||||
public MotionController Controller { get; }
|
||||
}
|
||||
|
||||
public MotionControllerRegistry(MotionExecutionLogger motionExecutionLogger)
|
||||
{
|
||||
_motionExecutionLogger = motionExecutionLogger ?? throw new ArgumentNullException(nameof(motionExecutionLogger));
|
||||
}
|
||||
|
||||
public MotionController GetController(IAxis axis)
|
||||
{
|
||||
if (axis == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(axis));
|
||||
}
|
||||
|
||||
var axisName = axis.Name;
|
||||
if (string.IsNullOrWhiteSpace(axisName))
|
||||
{
|
||||
return CreateController(axis);
|
||||
}
|
||||
|
||||
lock (_syncRoot)
|
||||
{
|
||||
ControllerEntry entry;
|
||||
if (_controllers.TryGetValue(axisName, out entry) && ReferenceEquals(entry.Axis, axis))
|
||||
{
|
||||
return entry.Controller;
|
||||
}
|
||||
|
||||
var controller = CreateController(axis);
|
||||
_controllers[axisName] = new ControllerEntry(axis, controller);
|
||||
return controller;
|
||||
}
|
||||
}
|
||||
|
||||
private MotionController CreateController(IAxis axis)
|
||||
{
|
||||
var controller = new MotionController(axis);
|
||||
controller.MotionStarted += _motionExecutionLogger.HandleMotionStarted;
|
||||
controller.MotionFinished += _motionExecutionLogger.HandleMotionFinished;
|
||||
return controller;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,117 @@
|
||||
using MwFramework.Device;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
|
||||
namespace MainShell.Motion
|
||||
{
|
||||
public enum MotionRequestKind
|
||||
{
|
||||
MoveAbs,
|
||||
MoveRel,
|
||||
Home,
|
||||
Jog
|
||||
}
|
||||
|
||||
public sealed class MotionMoveRequest
|
||||
{
|
||||
private MotionMoveRequest(string axisName, IAxis axis, double targetPosition, int timeoutMilliseconds, int? alarmId, MotionRequestKind requestKind, string source, IReadOnlyCollection<string> tags, string correlationId, string batchId, double? positionTolerance, bool stopOnFailure)
|
||||
{
|
||||
AxisName = axisName;
|
||||
Axis = axis;
|
||||
TargetPosition = targetPosition;
|
||||
TimeoutMilliseconds = timeoutMilliseconds;
|
||||
AlarmId = alarmId;
|
||||
RequestKind = requestKind;
|
||||
Source = source;
|
||||
Tags = tags ?? Array.Empty<string>();
|
||||
CorrelationId = correlationId;
|
||||
BatchId = batchId;
|
||||
PositionTolerance = positionTolerance;
|
||||
StopOnFailure = stopOnFailure;
|
||||
}
|
||||
|
||||
public string AxisName { get; }
|
||||
public IAxis Axis { get; }
|
||||
public double TargetPosition { get; }
|
||||
public int TimeoutMilliseconds { get; }
|
||||
public int? AlarmId { get; }
|
||||
public MotionRequestKind RequestKind { get; }
|
||||
public string Source { get; }
|
||||
public IReadOnlyCollection<string> Tags { get; }
|
||||
public string CorrelationId { get; }
|
||||
public string BatchId { get; }
|
||||
public double? PositionTolerance { get; }
|
||||
public bool StopOnFailure { get; }
|
||||
|
||||
public MotionMoveRequest WithMetadata(string source = null, IEnumerable<string> tags = null, string correlationId = null, string batchId = null, double? positionTolerance = null, bool? stopOnFailure = null)
|
||||
{
|
||||
return new MotionMoveRequest(
|
||||
AxisName,
|
||||
Axis,
|
||||
TargetPosition,
|
||||
TimeoutMilliseconds,
|
||||
AlarmId,
|
||||
RequestKind,
|
||||
string.IsNullOrWhiteSpace(source) ? Source : source,
|
||||
MergeTags(Tags, tags),
|
||||
string.IsNullOrWhiteSpace(correlationId) ? CorrelationId : correlationId,
|
||||
string.IsNullOrWhiteSpace(batchId) ? BatchId : batchId,
|
||||
positionTolerance ?? PositionTolerance,
|
||||
stopOnFailure ?? StopOnFailure);
|
||||
}
|
||||
|
||||
public static MotionMoveRequest ForAxisName(string axisName, double targetPosition, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, int? alarmId = null, string source = null, IEnumerable<string> tags = null, string correlationId = null, string batchId = null, double? positionTolerance = null, bool stopOnFailure = true)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(axisName))
|
||||
{
|
||||
throw new ArgumentNullException(nameof(axisName));
|
||||
}
|
||||
|
||||
return new MotionMoveRequest(axisName, null, targetPosition, timeoutMilliseconds, alarmId, MotionRequestKind.MoveAbs, source, NormalizeTags(tags), correlationId, batchId, positionTolerance, stopOnFailure);
|
||||
}
|
||||
|
||||
public static MotionMoveRequest ForAxis(IAxis axis, double targetPosition, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, int? alarmId = null, string source = null, IEnumerable<string> tags = null, string correlationId = null, string batchId = null, double? positionTolerance = null, bool stopOnFailure = true)
|
||||
{
|
||||
if (axis == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(axis));
|
||||
}
|
||||
|
||||
return new MotionMoveRequest(axis.Name, axis, targetPosition, timeoutMilliseconds, alarmId, MotionRequestKind.MoveAbs, source, NormalizeTags(tags), correlationId, batchId, positionTolerance, stopOnFailure);
|
||||
}
|
||||
|
||||
public static MotionMoveRequest ForRelativeAxisName(string axisName, double distance, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, int? alarmId = null, string source = null, IEnumerable<string> tags = null, string correlationId = null, string batchId = null, double? positionTolerance = null, bool stopOnFailure = true)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(axisName))
|
||||
{
|
||||
throw new ArgumentNullException(nameof(axisName));
|
||||
}
|
||||
|
||||
return new MotionMoveRequest(axisName, null, distance, timeoutMilliseconds, alarmId, MotionRequestKind.MoveRel, source, NormalizeTags(tags), correlationId, batchId, positionTolerance, stopOnFailure);
|
||||
}
|
||||
|
||||
public static MotionMoveRequest ForRelativeAxis(IAxis axis, double distance, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, int? alarmId = null, string source = null, IEnumerable<string> tags = null, string correlationId = null, string batchId = null, double? positionTolerance = null, bool stopOnFailure = true)
|
||||
{
|
||||
if (axis == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(axis));
|
||||
}
|
||||
|
||||
return new MotionMoveRequest(axis.Name, axis, distance, timeoutMilliseconds, alarmId, MotionRequestKind.MoveRel, source, NormalizeTags(tags), correlationId, batchId, positionTolerance, stopOnFailure);
|
||||
}
|
||||
|
||||
private static IReadOnlyCollection<string> NormalizeTags(IEnumerable<string> tags)
|
||||
{
|
||||
return (tags ?? Enumerable.Empty<string>())
|
||||
.Where(x => !string.IsNullOrWhiteSpace(x))
|
||||
.Distinct(StringComparer.OrdinalIgnoreCase)
|
||||
.ToArray();
|
||||
}
|
||||
|
||||
private static IReadOnlyCollection<string> MergeTags(IEnumerable<string> currentTags, IEnumerable<string> newTags)
|
||||
{
|
||||
return NormalizeTags((currentTags ?? Enumerable.Empty<string>()).Concat(newTags ?? Enumerable.Empty<string>()));
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,118 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
|
||||
namespace MainShell.Motion
|
||||
{
|
||||
public sealed class MotionResult
|
||||
{
|
||||
public MotionResult(string axisName, MotionOperation operation, double? targetPosition, double startPosition, double endPosition, bool succeeded, bool cancelled, Exception exception, string message, bool alarmReported, DateTime startedAtUtc, DateTime finishedAtUtc, string correlationId = null, string batchId = null, string failureStage = null, bool timedOut = false, bool stoppedByCoordinator = false)
|
||||
{
|
||||
AxisName = axisName;
|
||||
Operation = operation;
|
||||
TargetPosition = targetPosition;
|
||||
StartPosition = startPosition;
|
||||
EndPosition = endPosition;
|
||||
Succeeded = succeeded;
|
||||
Cancelled = cancelled;
|
||||
Exception = exception;
|
||||
Message = message;
|
||||
AlarmReported = alarmReported;
|
||||
StartedAtUtc = startedAtUtc;
|
||||
FinishedAtUtc = finishedAtUtc;
|
||||
CorrelationId = correlationId;
|
||||
BatchId = batchId;
|
||||
FailureStage = failureStage;
|
||||
TimedOut = timedOut;
|
||||
StoppedByCoordinator = stoppedByCoordinator;
|
||||
}
|
||||
|
||||
public string AxisName { get; }
|
||||
public MotionOperation Operation { get; }
|
||||
public double? TargetPosition { get; }
|
||||
public double StartPosition { get; }
|
||||
public double EndPosition { get; }
|
||||
public bool Succeeded { get; }
|
||||
public bool Cancelled { get; }
|
||||
public Exception Exception { get; }
|
||||
public string Message { get; }
|
||||
public bool AlarmReported { get; }
|
||||
public DateTime StartedAtUtc { get; }
|
||||
public DateTime FinishedAtUtc { get; }
|
||||
public string CorrelationId { get; }
|
||||
public string BatchId { get; }
|
||||
public string FailureStage { get; }
|
||||
public bool TimedOut { get; }
|
||||
public bool StoppedByCoordinator { get; }
|
||||
public long DurationMilliseconds => (long)(FinishedAtUtc - StartedAtUtc).TotalMilliseconds;
|
||||
|
||||
public MotionResult WithAlarmReported(bool alarmReported)
|
||||
{
|
||||
if (AlarmReported == alarmReported)
|
||||
{
|
||||
return this;
|
||||
}
|
||||
|
||||
return new MotionResult(AxisName, Operation, TargetPosition, StartPosition, EndPosition, Succeeded, Cancelled, Exception, Message, alarmReported, StartedAtUtc, FinishedAtUtc, CorrelationId, BatchId, FailureStage, TimedOut, StoppedByCoordinator);
|
||||
}
|
||||
|
||||
public MotionResult WithFailureDetails(string failureStage, bool timedOut = false, bool stoppedByCoordinator = false, string correlationId = null, string batchId = null)
|
||||
{
|
||||
return new MotionResult(AxisName, Operation, TargetPosition, StartPosition, EndPosition, Succeeded, Cancelled, Exception, Message, AlarmReported, StartedAtUtc, FinishedAtUtc, correlationId ?? CorrelationId, batchId ?? BatchId, failureStage ?? FailureStage, timedOut, stoppedByCoordinator);
|
||||
}
|
||||
|
||||
public void EnsureSuccess()
|
||||
{
|
||||
if (Succeeded)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (Exception != null)
|
||||
{
|
||||
throw Exception;
|
||||
}
|
||||
|
||||
throw new InvalidOperationException(Message ?? string.Format("Motion failed. Axis: {0}", AxisName));
|
||||
}
|
||||
}
|
||||
|
||||
public sealed class MotionBatchResult
|
||||
{
|
||||
public MotionBatchResult(IEnumerable<MotionResult> results)
|
||||
{
|
||||
Results = (results ?? Enumerable.Empty<MotionResult>()).ToList().AsReadOnly();
|
||||
}
|
||||
|
||||
public IReadOnlyList<MotionResult> Results { get; private set; }
|
||||
public bool Succeeded => Results.All(x => x != null && x.Succeeded);
|
||||
public bool Cancelled => Results.Any(x => x != null && x.Cancelled);
|
||||
|
||||
public void EnsureSuccess()
|
||||
{
|
||||
var failedResults = Results.Where(x => x != null && !x.Succeeded).ToList();
|
||||
if (failedResults.Count == 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (failedResults.Count == 1)
|
||||
{
|
||||
failedResults[0].EnsureSuccess();
|
||||
return;
|
||||
}
|
||||
|
||||
throw new AggregateException(failedResults.Select(CreateException));
|
||||
}
|
||||
|
||||
private static Exception CreateException(MotionResult result)
|
||||
{
|
||||
if (result == null)
|
||||
{
|
||||
return new InvalidOperationException("Motion result is null.");
|
||||
}
|
||||
|
||||
return result.Exception ?? new InvalidOperationException(result.Message ?? string.Format("Motion failed. Axis: {0}", result.AxisName));
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,117 @@
|
||||
using MainShell.Common;
|
||||
using MainShell.Hardware;
|
||||
using MainShell.Process;
|
||||
using MainShell.ProcessResult;
|
||||
using MaxwellFramework.Core.Attributes;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MainShell.Motion
|
||||
{
|
||||
[Singleton]
|
||||
public sealed class MotionSafetyStateProvider
|
||||
{
|
||||
private readonly IDeviceIoMonitorService _deviceIoMonitorService;
|
||||
private readonly ProcessResultManager _processResultManager;
|
||||
private readonly MachineState _machineState;
|
||||
|
||||
public MotionSafetyStateProvider(IDeviceIoMonitorService deviceIoMonitorService, ProcessResultManager processResultManager, MachineState machineState = null)
|
||||
{
|
||||
_deviceIoMonitorService = deviceIoMonitorService ?? throw new ArgumentNullException(nameof(deviceIoMonitorService));
|
||||
_processResultManager = processResultManager ?? throw new ArgumentNullException(nameof(processResultManager));
|
||||
_machineState = machineState;
|
||||
}
|
||||
|
||||
public MachineMode CurrentMachineMode => _machineState != null ? _machineState.CurrentMode : MachineMode.Manual;
|
||||
|
||||
public bool IsEmergencyStopReleased()
|
||||
{
|
||||
if (!_deviceIoMonitorService.IsOnline)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
return !_deviceIoMonitorService.IsPointOn(DeviceIoNames.WZ.EmergencyStopButton)
|
||||
&& !IsSafetyCircuitHealthy();
|
||||
}
|
||||
|
||||
public bool AreSafetyDoorsClosed()
|
||||
{
|
||||
if (!_deviceIoMonitorService.IsOnline)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (_deviceIoMonitorService.IsPointOn(DeviceIoNames.WZ.SecurityDoorShield))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
return _deviceIoMonitorService.IsPointOn(DeviceIoNames.WZ.SafetyDoor1Closed)
|
||||
&& _deviceIoMonitorService.IsPointOn(DeviceIoNames.WZ.SafetyDoor2Closed)
|
||||
&& _deviceIoMonitorService.IsPointOn(DeviceIoNames.WZ.SafetyDoor3Closed)
|
||||
&& _deviceIoMonitorService.IsPointOn(DeviceIoNames.WZ.SafetyDoor4Closed);
|
||||
}
|
||||
|
||||
public bool IsFlowRunning()
|
||||
{
|
||||
var status = _processResultManager.FlowState.Status;
|
||||
return string.Equals(status, ProcessExecutionStatus.Running.ToString(), StringComparison.OrdinalIgnoreCase);
|
||||
}
|
||||
|
||||
public string CurrentFlowName => _processResultManager.FlowState.WorkflowName;
|
||||
|
||||
public bool AreStageVacuumsReady()
|
||||
{
|
||||
if (!_deviceIoMonitorService.IsOnline)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
return _deviceIoMonitorService.IsPointOn(DeviceIoNames.Stage.Vacuum1)
|
||||
&& _deviceIoMonitorService.IsPointOn(DeviceIoNames.Stage.Vacuum2)
|
||||
&& _deviceIoMonitorService.IsPointOn(DeviceIoNames.Stage.Vacuum3)
|
||||
&& _deviceIoMonitorService.IsPointOn(DeviceIoNames.Stage.Vacuum4);
|
||||
}
|
||||
|
||||
public IReadOnlyList<string> GetUnsafeBondHeadSignals()
|
||||
{
|
||||
if (!_deviceIoMonitorService.IsOnline)
|
||||
{
|
||||
return new[] { "DeviceIoOffline" };
|
||||
}
|
||||
|
||||
var unsafeSignals = new List<string>();
|
||||
AddIfOff(unsafeSignals, DeviceIoNames.WS.BondHead1InPlace);
|
||||
AddIfOff(unsafeSignals, DeviceIoNames.WS.BondHead2InPlace);
|
||||
AddIfOff(unsafeSignals, DeviceIoNames.WS.BondHead3InPlace);
|
||||
AddIfOff(unsafeSignals, DeviceIoNames.WS.BondHead4InPlace);
|
||||
return unsafeSignals;
|
||||
}
|
||||
|
||||
public bool IsAutoWorkflowSource(string source)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(source))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
return string.Equals(source, ProcessFlowName.AutoProduction, StringComparison.OrdinalIgnoreCase)
|
||||
|| source.IndexOf("Auto", StringComparison.OrdinalIgnoreCase) >= 0;
|
||||
}
|
||||
|
||||
private bool IsSafetyCircuitHealthy()
|
||||
{
|
||||
return _deviceIoMonitorService.IsPointOn(DeviceIoNames.General.SafetyCircuitStatus1)
|
||||
&& _deviceIoMonitorService.IsPointOn(DeviceIoNames.General.SafetyCircuitStatus2);
|
||||
}
|
||||
|
||||
private void AddIfOff(ICollection<string> unsafeSignals, string signalName)
|
||||
{
|
||||
if (!_deviceIoMonitorService.IsPointOn(signalName))
|
||||
{
|
||||
unsafeSignals.Add(signalName);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,829 @@
|
||||
# Motion 运动框架使用说明
|
||||
|
||||
## 1. 文档范围
|
||||
|
||||
本文档基于当前代码实际实现整理，描述 [`MainShell/Motion`](MainShell/Motion) 目录下已经存在的运动控制能力、调用入口、约束行为与示例代码。
|
||||
|
||||
说明：
|
||||
- 文档内容以当前源码实现为准。
|
||||
- 若旧文档、注释或设计稿与代码不一致，以代码为准。
|
||||
- 本文档仅描述已经实现的能力，不对未来规划功能做扩展解释。
|
||||
|
||||
适用对象：
|
||||
- 业务流程开发人员
|
||||
- 手动功能开发人员
|
||||
- 运动控制维护人员
|
||||
- 需要接入平台联动或视觉逼近对位功能的开发人员
|
||||
|
||||
## 2. 模块总览
|
||||
|
||||
当前 [`MainShell/Motion`](MainShell/Motion) 目录中的能力主要包括：
|
||||
|
||||
1. 单轴运动控制
|
||||
2. 批量安全运动入口
|
||||
3. 运动前安全检查与报警上报
|
||||
4. 平台三轴联动封装
|
||||
5. 速度参数下发管理
|
||||
6. 逼近对位流程框架
|
||||
|
||||
主要文件如下：
|
||||
|
||||
- [`MotionController.cs`](MainShell/Motion/MotionController.cs)：单轴底层执行器
|
||||
- [`MotionInfrastructure.cs`](MainShell/Motion/MotionInfrastructure.cs)：运动报警、预检查、日志、控制器注册表
|
||||
- [`MotionMoveRequest.cs`](MainShell/Motion/MotionMoveRequest.cs)：运动请求模型
|
||||
- [`MotionResult.cs`](MainShell/Motion/MotionResult.cs)：单轴结果与批量结果模型
|
||||
- [`SafeAxisMotion.cs`](MainShell/Motion/SafeAxisMotion.cs)：业务层统一运动入口
|
||||
- [`StagePlatformMotionService.cs`](MainShell/Motion/StagePlatformMotionService.cs)：平台三轴联动封装
|
||||
- [`AxisSpeedManager.cs`](MainShell/Motion/AxisSpeedManager.cs)：轴速度曲线管理
|
||||
- [`MotionSafetyStateProvider.cs`](MainShell/Motion/MotionSafetyStateProvider.cs)：运动安全状态提供器
|
||||
- [`ApproachAlignment/ApproachAlignment.cs`](MainShell/Motion/ApproachAlignment/ApproachAlignment.cs)：逼近对位的数据结构和接口定义
|
||||
- [`ApproachAlignment/ApproachAlignmentService.cs`](MainShell/Motion/ApproachAlignment/ApproachAlignmentService.cs)：逼近对位主流程
|
||||
- [`ApproachAlignment/CenterRecognizer.cs`](MainShell/Motion/ApproachAlignment/CenterRecognizer.cs)：视觉中心识别实现
|
||||
- [`ApproachAlignment/CoordinateTransformer.cs`](MainShell/Motion/ApproachAlignment/CoordinateTransformer.cs)：坐标转换实现
|
||||
|
||||
## 3. 分层理解
|
||||
|
||||
### 3.1 单轴执行层
|
||||
|
||||
[`MotionController`](MainShell/Motion/MotionController.cs:10) 直接面向 `IAxis` / `IAxisFunc`，负责：
|
||||
|
||||
- 绝对运动 [`MoveAbsAsync()`](MainShell/Motion/MotionController.cs:35)
|
||||
- 相对运动 [`MoveRelAsync()`](MainShell/Motion/MotionController.cs:60)
|
||||
- Jog [`JogAsync()`](MainShell/Motion/MotionController.cs:86)
|
||||
- 停止 [`StopAsync()`](MainShell/Motion/MotionController.cs:122)
|
||||
- 回零 [`HomeAsync()`](MainShell/Motion/MotionController.cs:148)
|
||||
- 运动状态校验、软限位检查、超时与取消
|
||||
- 运动开始/结束事件通知
|
||||
|
||||
### 3.2 基础设施层
|
||||
|
||||
[`MotionInfrastructure.cs`](MainShell/Motion/MotionInfrastructure.cs) 中提供了几类基础设施：
|
||||
|
||||
- [`MotionAlarmReporter`](MainShell/Motion/MotionInfrastructure.cs:16)：统一报警上报
|
||||
- [`MotionPrecheckService`](MainShell/Motion/MotionInfrastructure.cs:50)：运动前安全检查聚合入口
|
||||
- [`MotionExecutionLogger`](MainShell/Motion/MotionInfrastructure.cs:112)：统一记录运动开始/结束日志
|
||||
- [`MotionControllerRegistry`](MainShell/Motion/MotionInfrastructure.cs:149)：缓存并复用每根轴对应的 [`MotionController`](MainShell/Motion/MotionController.cs:10)
|
||||
|
||||
### 3.3 请求与结果模型层
|
||||
|
||||
[`MotionMoveRequest`](MainShell/Motion/MotionMoveRequest.cs:16) 与 [`MotionResult`](MainShell/Motion/MotionResult.cs:7) / [`MotionBatchResult`](MainShell/Motion/MotionResult.cs:80) 用于统一描述：
|
||||
|
||||
- 运动目标
|
||||
- 超时时间
|
||||
- 报警号
|
||||
- 请求来源与标签
|
||||
- 批次号与关联号
|
||||
- 单轴结果
|
||||
- 批量结果
|
||||
|
||||
### 3.4 业务统一入口层
|
||||
|
||||
[`SafeAxisMotion`](MainShell/Motion/SafeAxisMotion.cs:14) 是当前推荐的业务调用入口，负责：
|
||||
|
||||
- 通过轴名解析实际轴对象
|
||||
- 复用 [`MotionController`](MainShell/Motion/MotionController.cs:10)
|
||||
- 单轴绝对/相对运动
|
||||
- 多轴绝对联动
|
||||
- 单轴/多轴回零
|
||||
- 调用 [`MotionPrecheckService.ValidateAsync()`](MainShell/Motion/MotionInfrastructure.cs:82) 做运动前安全校验
|
||||
- 失败时联动取消并停止相关轴
|
||||
- 失败后按请求配置上报报警
|
||||
- 为结果补充 `CorrelationId` / `BatchId` / `FailureStage`
|
||||
|
||||
### 3.5 业务场景服务层
|
||||
|
||||
[`StagePlatformMotionService`](MainShell/Motion/StagePlatformMotionService.cs:11) 封装平台三轴动作：
|
||||
|
||||
- 等高移动 [`MoveFlatAsync()`](MainShell/Motion/StagePlatformMotionService.cs:29)
|
||||
- 平面移动 [`MovePlaneAsync()`](MainShell/Motion/StagePlatformMotionService.cs:39)
|
||||
|
||||
### 3.6 视觉逼近对位层
|
||||
|
||||
[`ApproachAlignmentService`](MainShell/Motion/ApproachAlignment/ApproachAlignmentService.cs:17) 封装迭代式逼近对位流程：
|
||||
|
||||
1. 识别中心
|
||||
2. 将视觉中心转换为各轴目标位置
|
||||
3. 驱动多轴移动
|
||||
4. 再次识别中心
|
||||
5. 重新计算目标位置与当前误差
|
||||
6. 判断是否收敛
|
||||
|
||||
## 4. 当前代码中的真实能力
|
||||
|
||||
结合当前代码，以下能力已经明确存在：
|
||||
|
||||
- 单轴绝对移动
|
||||
- 单轴相对移动
|
||||
- 单轴 Jog
|
||||
- 单轴停止
|
||||
- 单轴回零
|
||||
- 多轴绝对联动移动
|
||||
- 多轴批量回零
|
||||
- 运动前安全检查
|
||||
- 失败报警上报
|
||||
- 平台三轴等高/平面联动
|
||||
- 基于视觉中心识别的迭代式逼近对位框架
|
||||
- 轴速度配置与速度档位下发
|
||||
|
||||
以下内容需要特别注意：
|
||||
|
||||
- 批量移动仅支持绝对运动，不支持批量相对运动
|
||||
- 逼近对位中的 [`CoordinateTransformer`](MainShell/Motion/ApproachAlignment/CoordinateTransformer.cs:14) 仍是占位实现
|
||||
- 文档中若提到通过 [`SafeAxisMotion`](MainShell/Motion/SafeAxisMotion.cs:14) 直接调用逼近对位，不符合当前代码真实入口
|
||||
|
||||
## 5. MotionController 说明
|
||||
|
||||
[`MotionController`](MainShell/Motion/MotionController.cs:10) 是单轴动作的最终执行器。
|
||||
|
||||
### 5.1 关键属性
|
||||
|
||||
- [`AxisName`](MainShell/Motion/MotionController.cs:26)
|
||||
- [`CurrentPos`](MainShell/Motion/MotionController.cs:27)
|
||||
- [`IsBusy`](MainShell/Motion/MotionController.cs:28)
|
||||
- [`IsAlarm`](MainShell/Motion/MotionController.cs:29)
|
||||
- [`InPos`](MainShell/Motion/MotionController.cs:30)
|
||||
|
||||
### 5.2 关键事件
|
||||
|
||||
- [`MotionStarted`](MainShell/Motion/MotionController.cs:32)
|
||||
- [`MotionFinished`](MainShell/Motion/MotionController.cs:33)
|
||||
|
||||
### 5.3 关键方法
|
||||
|
||||
- [`MoveAbsAsync()`](MainShell/Motion/MotionController.cs:35)
|
||||
- [`MoveRelAsync()`](MainShell/Motion/MotionController.cs:60)
|
||||
- [`JogAsync()`](MainShell/Motion/MotionController.cs:86)
|
||||
- [`StopAsync()`](MainShell/Motion/MotionController.cs:122)
|
||||
- [`HomeAsync()`](MainShell/Motion/MotionController.cs:148)
|
||||
|
||||
### 5.4 内部保障机制
|
||||
|
||||
[`MotionController`](MainShell/Motion/MotionController.cs:10) 内部具备以下保障：
|
||||
|
||||
- 使用 `SemaphoreSlim` 保证同一根轴同一时刻只执行一个动作，见 [`_motionLock`](MainShell/Motion/MotionController.cs:15)
|
||||
- 每次运动前调用 [`EnsureMotionStateReady()`](MainShell/Motion/MotionController.cs:280) 检查轴状态
|
||||
- 运动执行过程中调用 [`EnsureMotionStateHealthyDuringExecution()`](MainShell/Motion/MotionController.cs:304) 检查状态是否恶化
|
||||
- 绝对与相对移动执行前都会调用 [`EnsureWithinSoftLimit()`](MainShell/Motion/MotionController.cs:328) 做软限位检查
|
||||
- 取消时通过 [`TryStop()`](MainShell/Motion/MotionController.cs:368) 自动尝试停轴
|
||||
- 超时时通过 [`ThrowIfTimeoutExceeded()`](MainShell/Motion/MotionController.cs:351) 自动尝试停轴并抛出超时异常
|
||||
- 完成后统一生成 [`MotionResult`](MainShell/Motion/MotionResult.cs:7)
|
||||
|
||||
### 5.5 失败返回行为
|
||||
|
||||
[`ExecuteProtectedMotionAsync()`](MainShell/Motion/MotionController.cs:167) 会捕获执行异常，并在最终返回的 [`MotionResult`](MainShell/Motion/MotionResult.cs:7) 中体现成功、取消和异常信息，而不是始终直接向外抛出异常。
|
||||
|
||||
因此调用后建议统一执行：
|
||||
|
||||
```csharp
|
||||
var result = await controller.MoveAbsAsync(100.0, 10000, cancellationToken);
|
||||
result.EnsureSuccess();
|
||||
```
|
||||
|
||||
## 6. MotionInfrastructure 说明
|
||||
|
||||
[`MotionInfrastructure.cs`](MainShell/Motion/MotionInfrastructure.cs) 提供了运动框架的关键支撑能力。
|
||||
|
||||
### 6.1 MotionAlarmReporter
|
||||
|
||||
[`MotionAlarmReporter`](MainShell/Motion/MotionInfrastructure.cs:16) 对 [`AlarmOperate`](MainShell/Alarm/AlarmOperate.cs) 做了一层封装：
|
||||
|
||||
- 传入 `null` 报警号时直接返回 `false`
|
||||
- 传入有效报警号时调用 [`ReportAlarmAsync()`](MainShell/Motion/MotionInfrastructure.cs:25) 间接触发报警
|
||||
|
||||
### 6.2 MotionPrecheckService
|
||||
|
||||
[`MotionPrecheckService`](MainShell/Motion/MotionInfrastructure.cs:50) 在构造函数中聚合了以下安全检查：
|
||||
|
||||
- `MotionRequestDuplicateAxisCheck`
|
||||
- `MotionBatchSameSourceCheck`
|
||||
- `EmergencyStopReleasedCheck`
|
||||
- `SafetyDoorClosedCheck`
|
||||
- `AutoFlowModeCheck`
|
||||
- `StageVacuumReadyCheck`
|
||||
- `BondHeadSafePositionCheck`
|
||||
- `StagePlatformPlaneMoveSafetyCheck`
|
||||
|
||||
调用 [`ValidateAsync()`](MainShell/Motion/MotionInfrastructure.cs:82) 时，会：
|
||||
|
||||
1. 逐条执行安全规则
|
||||
2. 对失败规则记录系统错误日志
|
||||
3. 使用规则上的 `AlarmId` 或传入的 `fallbackAlarmId` 上报报警
|
||||
4. 最终抛出 `InvalidOperationException`
|
||||
|
||||
这意味着：
|
||||
- 当前代码中已经存在统一运动前预检查机制
|
||||
- [`SafeAxisMotion`](MainShell/Motion/SafeAxisMotion.cs:14) 的各类入口会先走预检查，再执行实际运动
|
||||
|
||||
### 6.3 MotionExecutionLogger
|
||||
|
||||
[`MotionExecutionLogger`](MainShell/Motion/MotionInfrastructure.cs:112) 负责处理：
|
||||
|
||||
- [`HandleMotionStarted()`](MainShell/Motion/MotionInfrastructure.cs:114)
|
||||
- [`HandleMotionFinished()`](MainShell/Motion/MotionInfrastructure.cs:126)
|
||||
|
||||
它会输出开始位置、目标位置、结束位置、耗时、是否取消、失败阶段等日志信息。
|
||||
|
||||
### 6.4 MotionControllerRegistry
|
||||
|
||||
[`MotionControllerRegistry`](MainShell/Motion/MotionInfrastructure.cs:149) 负责：
|
||||
|
||||
- 按轴名缓存控制器实例
|
||||
- 对同名且同引用的轴复用已有控制器
|
||||
- 在创建控制器时自动挂接 [`MotionExecutionLogger`](MainShell/Motion/MotionInfrastructure.cs:112) 事件
|
||||
|
||||
业务侧一般不需要自己维护 [`MotionController`](MainShell/Motion/MotionController.cs:10) 生命周期，直接通过 [`SafeAxisMotion`](MainShell/Motion/SafeAxisMotion.cs:14) 使用即可。
|
||||
|
||||
## 7. MotionMoveRequest 说明
|
||||
|
||||
[`MotionMoveRequest`](MainShell/Motion/MotionMoveRequest.cs:16) 当前并不轻量，除基础字段外还已经支持业务元数据。
|
||||
|
||||
### 7.1 主要属性
|
||||
|
||||
- [`AxisName`](MainShell/Motion/MotionMoveRequest.cs:34)
|
||||
- [`Axis`](MainShell/Motion/MotionMoveRequest.cs:35)
|
||||
- [`TargetPosition`](MainShell/Motion/MotionMoveRequest.cs:36)
|
||||
- [`TimeoutMilliseconds`](MainShell/Motion/MotionMoveRequest.cs:37)
|
||||
- [`AlarmId`](MainShell/Motion/MotionMoveRequest.cs:38)
|
||||
- [`RequestKind`](MainShell/Motion/MotionMoveRequest.cs:39)
|
||||
- [`Source`](MainShell/Motion/MotionMoveRequest.cs:40)
|
||||
- [`Tags`](MainShell/Motion/MotionMoveRequest.cs:41)
|
||||
- [`CorrelationId`](MainShell/Motion/MotionMoveRequest.cs:42)
|
||||
- [`BatchId`](MainShell/Motion/MotionMoveRequest.cs:43)
|
||||
- [`PositionTolerance`](MainShell/Motion/MotionMoveRequest.cs:44)
|
||||
- [`StopOnFailure`](MainShell/Motion/MotionMoveRequest.cs:45)
|
||||
|
||||
### 7.2 创建方式
|
||||
|
||||
绝对运动：
|
||||
- [`ForAxisName()`](MainShell/Motion/MotionMoveRequest.cs:64)
|
||||
- [`ForAxis()`](MainShell/Motion/MotionMoveRequest.cs:74)
|
||||
|
||||
相对运动：
|
||||
- [`ForRelativeAxisName()`](MainShell/Motion/MotionMoveRequest.cs:84)
|
||||
- [`ForRelativeAxis()`](MainShell/Motion/MotionMoveRequest.cs:94)
|
||||
|
||||
补充元数据：
|
||||
- [`WithMetadata()`](MainShell/Motion/MotionMoveRequest.cs:47)
|
||||
|
||||
### 7.3 行为说明
|
||||
|
||||
- `TargetPosition` 对绝对运动表示目标位置，对相对运动表示位移量
|
||||
- `Tags` 会通过 [`NormalizeTags()`](MainShell/Motion/MotionMoveRequest.cs:104) 去空、去重
|
||||
- `WithMetadata()` 会保留原请求主体，仅覆盖指定元数据
|
||||
- 批量绝对移动时会由 [`SafeAxisMotion`](MainShell/Motion/SafeAxisMotion.cs:14) 自动补充 `CorrelationId` 与 `BatchId`
|
||||
|
||||
## 8. MotionResult 与 MotionBatchResult 说明
|
||||
|
||||
[`MotionResult`](MainShell/Motion/MotionResult.cs:7) 包含：
|
||||
|
||||
- [`AxisName`](MainShell/Motion/MotionResult.cs:30)
|
||||
- [`Operation`](MainShell/Motion/MotionResult.cs:31)
|
||||
- [`TargetPosition`](MainShell/Motion/MotionResult.cs:32)
|
||||
- [`StartPosition`](MainShell/Motion/MotionResult.cs:33)
|
||||
- [`EndPosition`](MainShell/Motion/MotionResult.cs:34)
|
||||
- [`Succeeded`](MainShell/Motion/MotionResult.cs:35)
|
||||
- [`Cancelled`](MainShell/Motion/MotionResult.cs:36)
|
||||
- [`Exception`](MainShell/Motion/MotionResult.cs:37)
|
||||
- [`Message`](MainShell/Motion/MotionResult.cs:38)
|
||||
- [`AlarmReported`](MainShell/Motion/MotionResult.cs:39)
|
||||
- [`StartedAtUtc`](MainShell/Motion/MotionResult.cs:40)
|
||||
- [`FinishedAtUtc`](MainShell/Motion/MotionResult.cs:41)
|
||||
- [`CorrelationId`](MainShell/Motion/MotionResult.cs:42)
|
||||
- [`BatchId`](MainShell/Motion/MotionResult.cs:43)
|
||||
- [`FailureStage`](MainShell/Motion/MotionResult.cs:44)
|
||||
- [`TimedOut`](MainShell/Motion/MotionResult.cs:45)
|
||||
- [`StoppedByCoordinator`](MainShell/Motion/MotionResult.cs:46)
|
||||
- [`DurationMilliseconds`](MainShell/Motion/MotionResult.cs:47)
|
||||
|
||||
常用方法：
|
||||
- [`WithAlarmReported()`](MainShell/Motion/MotionResult.cs:49)
|
||||
- [`WithFailureDetails()`](MainShell/Motion/MotionResult.cs:59)
|
||||
- [`EnsureSuccess()`](MainShell/Motion/MotionResult.cs:64)
|
||||
|
||||
[`MotionBatchResult`](MainShell/Motion/MotionResult.cs:80) 包含：
|
||||
|
||||
- [`Results`](MainShell/Motion/MotionResult.cs:87)
|
||||
- [`Succeeded`](MainShell/Motion/MotionResult.cs:88)
|
||||
- [`Cancelled`](MainShell/Motion/MotionResult.cs:89)
|
||||
- [`EnsureSuccess()`](MainShell/Motion/MotionResult.cs:91)
|
||||
|
||||
如果批量结果中只有一根轴失败，[`EnsureSuccess()`](MainShell/Motion/MotionResult.cs:91) 会直接抛该轴异常；如果多根轴失败，会抛 `AggregateException`。
|
||||
|
||||
## 9. SafeAxisMotion 说明
|
||||
|
||||
[`SafeAxisMotion`](MainShell/Motion/SafeAxisMotion.cs:14) 是当前业务最推荐使用的统一入口。
|
||||
|
||||
### 9.1 主要能力
|
||||
|
||||
- 单轴绝对移动
|
||||
- 单轴相对移动
|
||||
- 多轴绝对联动移动
|
||||
- 单轴回零
|
||||
- 多轴批量回零
|
||||
- 轴名解析
|
||||
- 控制器缓存与日志挂接
|
||||
- 运动前安全预检查
|
||||
- 失败联动停轴
|
||||
- 失败报警
|
||||
- 批次与关联信息补充
|
||||
|
||||
### 9.2 主要异步方法
|
||||
|
||||
单轴移动：
|
||||
- [`MoveAbsAsync()`](MainShell/Motion/SafeAxisMotion.cs:156)
|
||||
- [`MoveAbsAsync()`](MainShell/Motion/SafeAxisMotion.cs:163)
|
||||
- [`MoveRelAsync()`](MainShell/Motion/SafeAxisMotion.cs:170)
|
||||
- [`MoveRelAsync()`](MainShell/Motion/SafeAxisMotion.cs:177)
|
||||
|
||||
批量绝对移动：
|
||||
- [`SafeMoveAsync()`](MainShell/Motion/SafeAxisMotion.cs:57)
|
||||
|
||||
回零：
|
||||
- [`HomeAsync()`](MainShell/Motion/SafeAxisMotion.cs:226)
|
||||
- [`HomeAsync()`](MainShell/Motion/SafeAxisMotion.cs:234)
|
||||
- [`SafeHomeAsync()`](MainShell/Motion/SafeAxisMotion.cs:194)
|
||||
- [`SafeHomeAsync()`](MainShell/Motion/SafeAxisMotion.cs:204)
|
||||
|
||||
同步封装：
|
||||
- [`SafeMove()`](MainShell/Motion/SafeAxisMotion.cs:42)
|
||||
- [`MoveAbs()`](MainShell/Motion/SafeAxisMotion.cs:76)
|
||||
- [`MoveRel()`](MainShell/Motion/SafeAxisMotion.cs:96)
|
||||
- [`Home()`](MainShell/Motion/SafeAxisMotion.cs:136)
|
||||
- [`SafeHome()`](MainShell/Motion/SafeAxisMotion.cs:116)
|
||||
|
||||
### 9.3 真实执行流程
|
||||
|
||||
以 [`MoveAbsAsync()`](MainShell/Motion/SafeAxisMotion.cs:156) 为例，执行顺序为：
|
||||
|
||||
1. 构造 [`MotionMoveRequest`](MainShell/Motion/MotionMoveRequest.cs:16)
|
||||
2. 调用 [`EnsureRequestBatchMetadata()`](MainShell/Motion/SafeAxisMotion.cs:385) 补齐关联元数据
|
||||
3. 解析轴对象
|
||||
4. 调用 [`MotionPrecheckService.ValidateAsync()`](MainShell/Motion/MotionInfrastructure.cs:82) 执行预检查
|
||||
5. 调用内部 [`MoveAbsInternalAsync()`](MainShell/Motion/SafeAxisMotion.cs:244)
|
||||
6. 通过 [`MotionControllerRegistry.GetController()`](MainShell/Motion/MotionInfrastructure.cs:172) 获取控制器并执行运动
|
||||
7. 按请求补充结果元数据与失败阶段
|
||||
8. 必要时上报报警
|
||||
|
||||
### 9.4 批量运动行为说明
|
||||
|
||||
[`SafeMoveAsync()`](MainShell/Motion/SafeAxisMotion.cs:57) 当前执行的是多轴绝对联动：
|
||||
|
||||
- 请求数组为空时，返回空的 [`MotionBatchResult`](MainShell/Motion/MotionResult.cs:80)
|
||||
- 会先对整批请求生成 `batchId`
|
||||
- 会统一做安全检查
|
||||
- 会并发启动各轴绝对运动任务
|
||||
- 任一轴失败时，若请求 `StopOnFailure = true`，会调用 [`CancelAndStopRelatedAxes()`](MainShell/Motion/SafeAxisMotion.cs:438)
|
||||
- [`CancelAndStopRelatedAxes()`](MainShell/Motion/SafeAxisMotion.cs:438) 会取消协调令牌，并逐轴调用 [`StopAsync()`](MainShell/Motion/MotionController.cs:122)
|
||||
|
||||
### 9.5 超时与失败细节
|
||||
|
||||
- [`NormalizeTimeout()`](MainShell/Motion/SafeAxisMotion.cs:423) 会把小于等于 0 的超时值恢复为默认值
|
||||
- 失败结果会通过 [`WithFailureDetails()`](MainShell/Motion/MotionResult.cs:59) 写入 `FailureStage`、`TimedOut`、`StoppedByCoordinator`、`CorrelationId`、`BatchId`
|
||||
- 当 `alarmId` 为空时，失败不会触发报警
|
||||
- 当 `stopOnFailure = false` 时，失败不会触发联动停轴
|
||||
|
||||
## 10. MotionSafetyStateProvider 说明
|
||||
|
||||
[`MotionSafetyStateProvider`](MainShell/Motion/MotionSafetyStateProvider.cs:13) 为预检查规则提供外部状态。
|
||||
|
||||
### 10.1 主要能力
|
||||
|
||||
- 读取当前机器模式 [`CurrentMachineMode`](MainShell/Motion/MotionSafetyStateProvider.cs:26)
|
||||
- 判断急停是否释放 [`IsEmergencyStopReleased()`](MainShell/Motion/MotionSafetyStateProvider.cs:28)
|
||||
- 判断安全门是否关闭 [`AreSafetyDoorsClosed()`](MainShell/Motion/MotionSafetyStateProvider.cs:39)
|
||||
- 判断流程是否在运行 [`IsFlowRunning()`](MainShell/Motion/MotionSafetyStateProvider.cs:57)
|
||||
- 获取当前流程名 [`CurrentFlowName`](MainShell/Motion/MotionSafetyStateProvider.cs:63)
|
||||
- 判断平台真空是否就绪 [`AreStageVacuumsReady()`](MainShell/Motion/MotionSafetyStateProvider.cs:65)
|
||||
- 获取吸嘴/打头相关不安全信号 [`GetUnsafeBondHeadSignals()`](MainShell/Motion/MotionSafetyStateProvider.cs:78)
|
||||
- 判断来源是否属于自动流程 [`IsAutoWorkflowSource()`](MainShell/Motion/MotionSafetyStateProvider.cs:93)
|
||||
|
||||
### 10.2 行为说明
|
||||
|
||||
- 当 `DeviceIo` 离线时，多数安全状态会直接返回不可用或不安全结果
|
||||
- 安全门支持 `SecurityDoorShield` 旁路逻辑
|
||||
- 自动流程判断不是只看精确名称，也支持来源字符串包含 `Auto`
|
||||
|
||||
## 11. StagePlatformMotionService 说明
|
||||
|
||||
[`StagePlatformMotionService`](MainShell/Motion/StagePlatformMotionService.cs:11) 封装平台三轴 [`Axis_Stage_Z7`](MainShell/Motion/StagePlatformMotionService.cs:45)、[`Axis_Stage_Z8`](MainShell/Motion/StagePlatformMotionService.cs:46)、[`Axis_Stage_Z9`](MainShell/Motion/StagePlatformMotionService.cs:47) 的联动动作。
|
||||
|
||||
### 11.1 主要方法
|
||||
|
||||
- [`MoveFlatAsync()`](MainShell/Motion/StagePlatformMotionService.cs:29)
|
||||
- [`MovePlaneAsync()`](MainShell/Motion/StagePlatformMotionService.cs:39)
|
||||
- [`MoveFlat()`](MainShell/Motion/StagePlatformMotionService.cs:24)
|
||||
- [`MovePlane()`](MainShell/Motion/StagePlatformMotionService.cs:34)
|
||||
|
||||
### 11.2 内部实现方式
|
||||
|
||||
- [`MoveFlatAsync()`](MainShell/Motion/StagePlatformMotionService.cs:29) 实际调用 [`MovePlaneAsync()`](MainShell/Motion/StagePlatformMotionService.cs:39)
|
||||
- [`MovePlaneAsync()`](MainShell/Motion/StagePlatformMotionService.cs:39) 会先调用 [`EnsurePlaneTargetsOrdered()`](MainShell/Motion/StagePlatformMotionService.cs:53)
|
||||
- 允许的三轴目标最大高差为 `5.0`
|
||||
- 然后将三根轴封装为带 `Source` 和 `Tags` 的 [`MotionMoveRequest`](MainShell/Motion/MotionMoveRequest.cs:16)
|
||||
- 再统一调用 [`SafeAxisMotion.SafeMoveAsync()`](MainShell/Motion/SafeAxisMotion.cs:57)
|
||||
- 方法内部已经执行 [`EnsureSuccess()`](MainShell/Motion/MotionResult.cs:91)
|
||||
|
||||
### 11.3 使用建议
|
||||
|
||||
只要是平台三轴动作，建议统一走 [`StagePlatformMotionService`](MainShell/Motion/StagePlatformMotionService.cs:11)，不要在业务代码里手工拼三轴请求。
|
||||
|
||||
## 12. AxisSpeedManager 说明
|
||||
|
||||
[`AxisSpeedManager`](MainShell/Motion/AxisSpeedManager.cs:38) 用于从系统参数生成速度曲线并下发到轴参数。
|
||||
|
||||
### 12.1 相关类型
|
||||
|
||||
- [`AxisSpeedProfile`](MainShell/Motion/AxisSpeedManager.cs:13)：`Low` / `Medium` / `High` / `Custom`
|
||||
- [`AxisMotionProfile`](MainShell/Motion/AxisSpeedManager.cs:21)：封装 `Velocity` / `Acceleration` / `Deceleration` / `Jerk`
|
||||
|
||||
### 12.2 主要方法
|
||||
|
||||
- [`SetAxisSpeed(string axisName, double speed)`](MainShell/Motion/AxisSpeedManager.cs:49)
|
||||
- [`SetAxisSpeed(string axisName, AxisSpeedProfile profile)`](MainShell/Motion/AxisSpeedManager.cs:56)
|
||||
- [`ApplySpeedSettings()`](MainShell/Motion/AxisSpeedManager.cs:63)
|
||||
|
||||
### 12.3 真实行为说明
|
||||
|
||||
[`SetAxisSpeed(string axisName, double speed)`](MainShell/Motion/AxisSpeedManager.cs:49) 的行为不是固定倍数计算，而是：
|
||||
|
||||
1. 从速度参数表中找到该轴对应的 `SpeedTypeItem`
|
||||
2. 以配置基准速度 `speedItem.Speed` 为基数
|
||||
3. 用 `speed / speedItem.Speed` 计算缩放比例
|
||||
4. 按比例缩放 `Acc` / `Dec` / `Jerk`
|
||||
5. 最终通过 [`ApplyAxisMotionProfile()`](MainShell/Motion/AxisSpeedManager.cs:85) 下发到轴参数
|
||||
|
||||
[`SetAxisSpeed(string axisName, AxisSpeedProfile profile)`](MainShell/Motion/AxisSpeedManager.cs:56) 的行为是：
|
||||
|
||||
- 读取当前轴配置中的 `LowPercent` / `MedianPercent` / `HighPercent`
|
||||
- 调用 [`CreateProfile()`](MainShell/Motion/AxisSpeedManager.cs:151) 生成缩放后的运动参数
|
||||
- 调用 [`ApplyAxisMotionProfile()`](MainShell/Motion/AxisSpeedManager.cs:85) 下发
|
||||
|
||||
[`ApplySpeedSettings()`](MainShell/Motion/AxisSpeedManager.cs:63) 的行为是：
|
||||
|
||||
- 从 `GlobalParam.SpeedParaSysSetting.SpeedTypeItemCollection.SpeedTypeItemList` 读取所有速度配置
|
||||
- 根据当前 `CurrentSpeedType` 通过 [`MapCurrentSpeedTypeText()`](MainShell/Motion/AxisSpeedManager.cs:187) 映射到 `Low` / `Medium` / `High`
|
||||
- 对每根轴生成对应档位的速度参数并下发
|
||||
|
||||
### 12.4 校验与异常
|
||||
|
||||
[`AxisSpeedManager`](MainShell/Motion/AxisSpeedManager.cs:38) 会校验：
|
||||
|
||||
- 轴名不能为空
|
||||
- 轴必须存在
|
||||
- 轴必须有对应速度配置
|
||||
- 速度、加速度、减速度、Jerk 必须为正且有限值，见 [`ValidatePositiveFinite()`](MainShell/Motion/AxisSpeedManager.cs:216)
|
||||
- 百分比必须在 `(0, 100]` 范围内，见 [`ValidatePercent()`](MainShell/Motion/AxisSpeedManager.cs:224)
|
||||
|
||||
## 13. 逼近对位模块说明
|
||||
|
||||
### 13.1 组成文件
|
||||
|
||||
- [`ApproachAlignment/ApproachAlignment.cs`](MainShell/Motion/ApproachAlignment/ApproachAlignment.cs)
|
||||
- [`ApproachAlignment/ApproachAlignmentService.cs`](MainShell/Motion/ApproachAlignment/ApproachAlignmentService.cs)
|
||||
- [`ApproachAlignment/CenterRecognizer.cs`](MainShell/Motion/ApproachAlignment/CenterRecognizer.cs)
|
||||
- [`ApproachAlignment/CoordinateTransformer.cs`](MainShell/Motion/ApproachAlignment/CoordinateTransformer.cs)
|
||||
|
||||
### 13.2 核心类型
|
||||
|
||||
[`ApproachAlignment.cs`](MainShell/Motion/ApproachAlignment/ApproachAlignment.cs:15) 中定义了：
|
||||
|
||||
- [`ApproachAlignmentAxis`](MainShell/Motion/ApproachAlignment/ApproachAlignment.cs:15)
|
||||
- [`ApproachAlignmentRequest`](MainShell/Motion/ApproachAlignment/ApproachAlignment.cs:52)
|
||||
- [`ApproachAlignmentResult`](MainShell/Motion/ApproachAlignment/ApproachAlignment.cs:109)
|
||||
- [`CoordinateTransformResult`](MainShell/Motion/ApproachAlignment/ApproachAlignment.cs:151)
|
||||
- [`ICenterRecognizer`](MainShell/Motion/ApproachAlignment/ApproachAlignment.cs:186)
|
||||
- [`ICoordinateTransformer`](MainShell/Motion/ApproachAlignment/ApproachAlignment.cs:204)
|
||||
|
||||
### 13.3 ApproachAlignmentRequest 说明
|
||||
|
||||
[`ApproachAlignmentRequest`](MainShell/Motion/ApproachAlignment/ApproachAlignment.cs:52) 构造函数要求至少传入一根轴，并提供以下默认值：
|
||||
|
||||
- `Camera = CameraType.UpCamera`
|
||||
- `MaxIterations = 5`
|
||||
- `MoveTimeoutMilliseconds = 30000`
|
||||
- `RecognitionTimeoutMilliseconds = 10000`
|
||||
|
||||
### 13.4 ApproachAlignmentService 流程
|
||||
|
||||
[`ApproachAlignmentService.ApproachAlignmentAsync()`](MainShell/Motion/ApproachAlignment/ApproachAlignmentService.cs:42) 当前流程如下：
|
||||
|
||||
1. 最多执行 `MaxIterations` 次迭代
|
||||
2. 首次迭代调用 [`RecognizeCenterAsync()`](MainShell/Motion/ApproachAlignment/ApproachAlignmentService.cs:182) 识别中心
|
||||
3. 调用 [`ICoordinateTransformer.TransformAsync()`](MainShell/Motion/ApproachAlignment/ApproachAlignment.cs:213) 计算各轴目标位置
|
||||
4. 将目标位置转成 [`MotionMoveRequest.ForAxisName()`](MainShell/Motion/MotionMoveRequest.cs:64)
|
||||
5. 调用 [`SafeAxisMotion.SafeMoveAsync()`](MainShell/Motion/SafeAxisMotion.cs:57) 执行多轴移动
|
||||
6. 再次识别中心
|
||||
7. 再次转换，得到理论目标位置
|
||||
8. 读取各轴当前位置并计算误差
|
||||
9. 若全部轴误差都小于等于容差，则成功结束
|
||||
10. 否则进入下一轮迭代
|
||||
|
||||
### 13.5 CenterRecognizer 现状
|
||||
|
||||
[`CenterRecognizer`](MainShell/Motion/ApproachAlignment/CenterRecognizer.cs:14) 已接入视觉算法服务：
|
||||
|
||||
- 依赖 `IVisionAlgorithmService`
|
||||
- 使用 `VisionProcessRequest`
|
||||
- `AlgorithmType = VisionAlgorithmType.FindCenter`
|
||||
- `CaptureOptions = CameraCaptureOptions.CreateStream(timeoutMilliseconds)`
|
||||
- 从结果中读取 `OffsetX` / `OffsetY`
|
||||
- 识别失败或超时返回 `null`
|
||||
|
||||
### 13.6 CoordinateTransformer 现状
|
||||
|
||||
[`CoordinateTransformer`](MainShell/Motion/ApproachAlignment/CoordinateTransformer.cs:14) 当前仍是占位实现。
|
||||
|
||||
实际行为：
|
||||
- 若轴列表为空，则返回 `Succeeded = false`
|
||||
- 否则延时约 10ms 后，为每根轴写入 `0.0` 作为目标位置
|
||||
- 返回 `Succeeded = true`
|
||||
|
||||
这表示：
|
||||
- 逼近对位的流程框架已经存在
|
||||
- 但真实标定坐标变换尚未完成
|
||||
- 若直接用于生产逻辑，会把各轴目标位置算成固定 `0.0`
|
||||
|
||||
### 13.7 当前真实入口
|
||||
|
||||
当前应直接注入并调用 [`ApproachAlignmentService.ApproachAlignmentAsync()`](MainShell/Motion/ApproachAlignment/ApproachAlignmentService.cs:42)，而不是通过 [`SafeAxisMotion`](MainShell/Motion/SafeAxisMotion.cs:14) 调用逼近对位。
|
||||
|
||||
## 14. 推荐调用方式
|
||||
|
||||
### 14.1 普通业务运动
|
||||
|
||||
优先调用 [`SafeAxisMotion`](MainShell/Motion/SafeAxisMotion.cs:14)：
|
||||
|
||||
- 单轴绝对移动用 [`MoveAbsAsync()`](MainShell/Motion/SafeAxisMotion.cs:156)
|
||||
- 单轴相对移动用 [`MoveRelAsync()`](MainShell/Motion/SafeAxisMotion.cs:170)
|
||||
- 多轴绝对联动用 [`SafeMoveAsync()`](MainShell/Motion/SafeAxisMotion.cs:57)
|
||||
- 单轴回零用 [`HomeAsync()`](MainShell/Motion/SafeAxisMotion.cs:226)
|
||||
- 多轴回零用 [`SafeHomeAsync()`](MainShell/Motion/SafeAxisMotion.cs:194)
|
||||
|
||||
### 14.2 平台三轴业务
|
||||
|
||||
统一调用 [`StagePlatformMotionService`](MainShell/Motion/StagePlatformMotionService.cs:11)：
|
||||
|
||||
- 等高移动用 [`MoveFlatAsync()`](MainShell/Motion/StagePlatformMotionService.cs:29)
|
||||
- 平面移动用 [`MovePlaneAsync()`](MainShell/Motion/StagePlatformMotionService.cs:39)
|
||||
|
||||
### 14.3 单轴调试或事件监听
|
||||
|
||||
可以直接使用 [`MotionController`](MainShell/Motion/MotionController.cs:10)：
|
||||
|
||||
- 适合底层调试
|
||||
- 适合观察 [`MotionStarted`](MainShell/Motion/MotionController.cs:32) / [`MotionFinished`](MainShell/Motion/MotionController.cs:33)
|
||||
- 不建议作为一般业务入口
|
||||
|
||||
### 14.4 逼近对位业务
|
||||
|
||||
当前应直接调用 [`ApproachAlignmentService`](MainShell/Motion/ApproachAlignment/ApproachAlignmentService.cs:17)，前提是先替换 [`CoordinateTransformer`](MainShell/Motion/ApproachAlignment/CoordinateTransformer.cs:14) 为真实可用的标定变换实现。
|
||||
|
||||
## 15. 示例代码
|
||||
|
||||
### 15.1 单轴绝对移动
|
||||
|
||||
```csharp
|
||||
public async Task MoveAxisXAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
var result = await _safeAxisMotion.MoveAbsAsync(
|
||||
"Axis_X",
|
||||
100.0,
|
||||
timeoutMilliseconds: 10000,
|
||||
cancellationToken: cancellationToken,
|
||||
alarmId: 5001);
|
||||
|
||||
result.EnsureSuccess();
|
||||
}
|
||||
```
|
||||
|
||||
### 15.2 单轴相对移动
|
||||
|
||||
```csharp
|
||||
public async Task NudgeAxisYAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
var result = await _safeAxisMotion.MoveRelAsync(
|
||||
"Axis_Y",
|
||||
-2.0,
|
||||
timeoutMilliseconds: 8000,
|
||||
cancellationToken: cancellationToken,
|
||||
alarmId: 5002);
|
||||
|
||||
result.EnsureSuccess();
|
||||
}
|
||||
```
|
||||
|
||||
### 15.3 带元数据的多轴绝对联动
|
||||
|
||||
```csharp
|
||||
public async Task MoveXYAsync(double x, double y, CancellationToken cancellationToken)
|
||||
{
|
||||
var batch = await _safeAxisMotion.SafeMoveAsync(
|
||||
cancellationToken,
|
||||
MotionMoveRequest.ForAxisName(
|
||||
"Axis_X",
|
||||
x,
|
||||
timeoutMilliseconds: 12000,
|
||||
alarmId: 6001,
|
||||
source: "Manual.Align",
|
||||
tags: new[] { "Manual", "Align" },
|
||||
correlationId: "ALIGN-001",
|
||||
batchId: "BATCH-ALIGN-001",
|
||||
positionTolerance: 0.005,
|
||||
stopOnFailure: true),
|
||||
MotionMoveRequest.ForAxisName(
|
||||
"Axis_Y",
|
||||
y,
|
||||
timeoutMilliseconds: 12000,
|
||||
alarmId: 6002,
|
||||
source: "Manual.Align",
|
||||
tags: new[] { "Manual", "Align" },
|
||||
correlationId: "ALIGN-002",
|
||||
batchId: "BATCH-ALIGN-001",
|
||||
positionTolerance: 0.005,
|
||||
stopOnFailure: true));
|
||||
|
||||
batch.EnsureSuccess();
|
||||
}
|
||||
```
|
||||
|
||||
### 15.4 单轴回零
|
||||
|
||||
```csharp
|
||||
public async Task HomeAxisZAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
var result = await _safeAxisMotion.HomeAsync(
|
||||
"Axis_Z",
|
||||
timeoutMilliseconds: 30000,
|
||||
cancellationToken: cancellationToken,
|
||||
alarmId: 7001);
|
||||
|
||||
result.EnsureSuccess();
|
||||
}
|
||||
```
|
||||
|
||||
### 15.5 多轴批量回零
|
||||
|
||||
```csharp
|
||||
public async Task HomeAllAxesAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
var batch = await _safeAxisMotion.SafeHomeAsync(
|
||||
cancellationToken,
|
||||
"Axis_X",
|
||||
"Axis_Y",
|
||||
"Axis_Z");
|
||||
|
||||
batch.EnsureSuccess();
|
||||
}
|
||||
```
|
||||
|
||||
### 15.6 平台三轴等高移动
|
||||
|
||||
```csharp
|
||||
public async Task MoveStageFlatAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
var batch = await _stagePlatformMotionService.MoveFlatAsync(
|
||||
20.0,
|
||||
timeoutMilliseconds: 20000,
|
||||
cancellationToken: cancellationToken);
|
||||
|
||||
batch.EnsureSuccess();
|
||||
}
|
||||
```
|
||||
|
||||
### 15.7 平台三轴平面移动
|
||||
|
||||
```csharp
|
||||
public async Task MoveStagePlaneAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
var batch = await _stagePlatformMotionService.MovePlaneAsync(
|
||||
z7Target: 20.000,
|
||||
z8Target: 20.150,
|
||||
z9Target: 19.950,
|
||||
timeoutMilliseconds: 25000,
|
||||
cancellationToken: cancellationToken);
|
||||
|
||||
batch.EnsureSuccess();
|
||||
}
|
||||
```
|
||||
|
||||
### 15.8 直接使用 MotionController 监听事件
|
||||
|
||||
```csharp
|
||||
var controller = new MotionController(axis);
|
||||
|
||||
controller.MotionStarted += (sender, e) =>
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"Start Axis={e.AxisName}, Op={e.Operation}, Start={e.StartPosition}, Target={e.TargetPosition}");
|
||||
};
|
||||
|
||||
controller.MotionFinished += (sender, e) =>
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"Finish Axis={e.AxisName}, Success={e.Succeeded}, Cancelled={e.Cancelled}, End={e.EndPosition}, Error={e.Exception}");
|
||||
};
|
||||
|
||||
var result = await controller.MoveAbsAsync(50.0, 10000, cancellationToken);
|
||||
result.EnsureSuccess();
|
||||
```
|
||||
|
||||
### 15.9 使用 AxisSpeedManager 切换速度档位
|
||||
|
||||
```csharp
|
||||
public void ApplyMediumSpeedToStageX()
|
||||
{
|
||||
_axisSpeedManager.SetAxisSpeed("Axis_Stage_X", AxisSpeedProfile.Medium);
|
||||
}
|
||||
```
|
||||
|
||||
### 15.10 使用 AxisSpeedManager 设置自定义速度
|
||||
|
||||
```csharp
|
||||
public void ApplyCustomSpeedToStageX()
|
||||
{
|
||||
_axisSpeedManager.SetAxisSpeed("Axis_Stage_X", 150.0);
|
||||
}
|
||||
```
|
||||
|
||||
### 15.11 逼近对位调用示例
|
||||
|
||||
```csharp
|
||||
public async Task<ApproachAlignmentResult> RunApproachAlignmentAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
var request = new ApproachAlignmentRequest(
|
||||
new[]
|
||||
{
|
||||
new ApproachAlignmentAxis("Axis_X", 0.01) { Description = "X axis" },
|
||||
new ApproachAlignmentAxis("Axis_Y", 0.01) { Description = "Y axis" }
|
||||
},
|
||||
camera: CameraType.UpCamera)
|
||||
{
|
||||
MaxIterations = 5,
|
||||
MoveTimeoutMilliseconds = 30000,
|
||||
RecognitionTimeoutMilliseconds = 10000
|
||||
};
|
||||
|
||||
var result = await _approachAlignmentService.ApproachAlignmentAsync(
|
||||
request,
|
||||
cancellationToken);
|
||||
|
||||
if (!result.Succeeded)
|
||||
{
|
||||
throw result.Exception ?? new InvalidOperationException(result.Message);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
```
|
||||
|
||||
### 15.12 自定义真实坐标转换器示例
|
||||
|
||||
```csharp
|
||||
public class CalibratedCoordinateTransformer : ICoordinateTransformer
|
||||
{
|
||||
public Task<CoordinateTransformResult> TransformAsync(
|
||||
double centerX,
|
||||
double centerY,
|
||||
IEnumerable<ApproachAlignmentAxis> axes)
|
||||
{
|
||||
var result = new CoordinateTransformResult();
|
||||
|
||||
foreach (var axis in axes)
|
||||
{
|
||||
var target = CalculateTarget(axis.AxisName, centerX, centerY);
|
||||
result.AxisPositions[axis.AxisName] = target;
|
||||
}
|
||||
|
||||
result.Succeeded = true;
|
||||
return Task.FromResult(result);
|
||||
}
|
||||
|
||||
private double CalculateTarget(string axisName, double centerX, double centerY)
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
## 16. 注意事项
|
||||
|
||||
- [`SafeAxisMotion.SafeMoveAsync()`](MainShell/Motion/SafeAxisMotion.cs:57) 当前仅用于批量绝对移动
|
||||
- 单轴相对运动支持 [`MoveRelAsync()`](MainShell/Motion/SafeAxisMotion.cs:170)，但没有批量相对移动入口
|
||||
- 运动前安全检查已经存在，不应再按旧文档理解为“未实现”
|
||||
- [`MotionMoveRequest`](MainShell/Motion/MotionMoveRequest.cs:16) 已支持 `Source`、`Tags`、`CorrelationId`、`BatchId`、`PositionTolerance`、`StopOnFailure`
|
||||
- [`AxisSpeedManager`](MainShell/Motion/AxisSpeedManager.cs:38) 的自定义速度是基于配置比例缩放，不是固定倍数算法
|
||||
- [`CoordinateTransformer`](MainShell/Motion/ApproachAlignment/CoordinateTransformer.cs:14) 当前仍是占位实现，接入前必须替换
|
||||
- [`ApproachAlignmentService`](MainShell/Motion/ApproachAlignment/ApproachAlignmentService.cs:17) 是逼近对位真实入口
|
||||
|
||||
## 17. 总结
|
||||
|
||||
当前 [`MainShell/Motion`](MainShell/Motion) 模块已经具备一套较完整的运动控制框架：
|
||||
|
||||
- [`MotionController`](MainShell/Motion/MotionController.cs:10) 负责可靠单轴执行
|
||||
- [`MotionPrecheckService`](MainShell/Motion/MotionInfrastructure.cs:50) 负责运动前安全校验
|
||||
- [`SafeAxisMotion`](MainShell/Motion/SafeAxisMotion.cs:14) 负责业务层统一调用、批量联动与失败联停
|
||||
- [`StagePlatformMotionService`](MainShell/Motion/StagePlatformMotionService.cs:11) 负责平台三轴动作封装
|
||||
- [`AxisSpeedManager`](MainShell/Motion/AxisSpeedManager.cs:38) 负责轴速度参数下发与档位管理
|
||||
- [`ApproachAlignmentService`](MainShell/Motion/ApproachAlignment/ApproachAlignmentService.cs:17) 提供逼近对位流程框架
|
||||
|
||||
其中基础运动控制、预检查、安全联停和平台联动已经可以作为业务入口直接使用；逼近对位部分仍需补齐真实坐标变换，才能形成可用于生产的完整闭环。
|
||||
@@ -0,0 +1,584 @@
|
||||
using MaxwellFramework.Core.Attributes;
|
||||
using MainShell.Hardware;
|
||||
using MainShell.Log;
|
||||
using MainShell.Motion.Safety;
|
||||
using MwFramework.Device;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace MainShell.Motion
|
||||
{
|
||||
public class SafeAxisMotion
|
||||
{
|
||||
private sealed class ResolvedMotionMoveRequest
|
||||
{
|
||||
public ResolvedMotionMoveRequest(MotionMoveRequest request, IAxis axis)
|
||||
{
|
||||
Request = request ?? throw new ArgumentNullException(nameof(request));
|
||||
Axis = axis ?? throw new ArgumentNullException(nameof(axis));
|
||||
}
|
||||
|
||||
public MotionMoveRequest Request { get; private set; }
|
||||
public IAxis Axis { get; private set; }
|
||||
}
|
||||
|
||||
private readonly HardwareManager _hardware;
|
||||
private readonly MotionControllerRegistry _controllerRegistry;
|
||||
private readonly MotionPrecheckService _motionPrecheckService;
|
||||
private readonly MotionAlarmReporter _motionAlarmReporter;
|
||||
|
||||
public SafeAxisMotion(HardwareManager hardware, MotionControllerRegistry controllerRegistry, MotionPrecheckService motionPrecheckService, MotionAlarmReporter motionAlarmReporter)
|
||||
{
|
||||
_hardware = hardware ?? throw new ArgumentNullException(nameof(hardware));
|
||||
_controllerRegistry = controllerRegistry ?? throw new ArgumentNullException(nameof(controllerRegistry));
|
||||
_motionPrecheckService = motionPrecheckService ?? throw new ArgumentNullException(nameof(motionPrecheckService));
|
||||
_motionAlarmReporter = motionAlarmReporter ?? throw new ArgumentNullException(nameof(motionAlarmReporter));
|
||||
}
|
||||
|
||||
#region ??????
|
||||
public void SafeMove(params MotionMoveRequest[] requests)
|
||||
{
|
||||
SafeMove(CancellationToken.None, requests);
|
||||
}
|
||||
|
||||
public void SafeMove(CancellationToken cancellationToken, params MotionMoveRequest[] requests)
|
||||
{
|
||||
RunSync(() => SafeMoveAsync(cancellationToken, requests)).EnsureSuccess();
|
||||
}
|
||||
|
||||
public Task<MotionBatchResult> SafeMoveAsync(params MotionMoveRequest[] requests)
|
||||
{
|
||||
return SafeMoveAsync(CancellationToken.None, requests);
|
||||
}
|
||||
|
||||
public async Task<MotionBatchResult> SafeMoveAsync(CancellationToken cancellationToken, params MotionMoveRequest[] requests)
|
||||
{
|
||||
if (requests == null || requests.Length == 0)
|
||||
{
|
||||
return new MotionBatchResult(Array.Empty<MotionResult>());
|
||||
}
|
||||
|
||||
var batchId = CreateBatchId(requests);
|
||||
var resolvedRequests = requests.Select(x => ResolveRequest(EnsureRequestBatchMetadata(x, batchId))).ToArray();
|
||||
var axes = resolvedRequests.Select(x => x.Axis).ToArray();
|
||||
await _motionPrecheckService.ValidateAsync(CreateSafetyContext(MotionRequestKind.MoveAbs, resolvedRequests), cancellationToken, requests.Select(x => x.AlarmId).FirstOrDefault(x => x.HasValue)).ConfigureAwait(false);
|
||||
using (var linkedCancellationTokenSource = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken))
|
||||
{
|
||||
var tasks = resolvedRequests.Select(x => MoveAbsInternalAsync(x, linkedCancellationTokenSource.Token, linkedCancellationTokenSource, axes)).ToArray();
|
||||
var results = await Task.WhenAll(tasks).ConfigureAwait(false);
|
||||
return new MotionBatchResult(results);
|
||||
}
|
||||
}
|
||||
|
||||
public void MoveAbs(string axisName, double targetPos)
|
||||
{
|
||||
SafeMove(MotionMoveRequest.ForAxisName(axisName, targetPos));
|
||||
}
|
||||
|
||||
public void MoveAbs(string axisName, double targetPos, CancellationToken cancellationToken)
|
||||
{
|
||||
RunSync(() => MoveAbsAsync(axisName, targetPos, MotionController.DefaultTimeoutMilliseconds, cancellationToken)).EnsureSuccess();
|
||||
}
|
||||
|
||||
public void MoveAbs(IAxis axis, double targetPos)
|
||||
{
|
||||
SafeMove(MotionMoveRequest.ForAxis(axis, targetPos));
|
||||
}
|
||||
|
||||
public void MoveAbs(IAxis axis, double targetPos, CancellationToken cancellationToken)
|
||||
{
|
||||
RunSync(() => MoveAbsAsync(axis, targetPos, MotionController.DefaultTimeoutMilliseconds, cancellationToken)).EnsureSuccess();
|
||||
}
|
||||
|
||||
public void MoveRel(string axisName, double distance)
|
||||
{
|
||||
MoveRel(axisName, distance, CancellationToken.None);
|
||||
}
|
||||
|
||||
public void MoveRel(string axisName, double distance, CancellationToken cancellationToken)
|
||||
{
|
||||
RunSync(() => MoveRelAsync(axisName, distance, MotionController.DefaultTimeoutMilliseconds, cancellationToken)).EnsureSuccess();
|
||||
}
|
||||
|
||||
public void MoveRel(IAxis axis, double distance)
|
||||
{
|
||||
MoveRel(axis, distance, CancellationToken.None);
|
||||
}
|
||||
|
||||
public void MoveRel(IAxis axis, double distance, CancellationToken cancellationToken)
|
||||
{
|
||||
RunSync(() => MoveRelAsync(axis, distance, MotionController.DefaultTimeoutMilliseconds, cancellationToken)).EnsureSuccess();
|
||||
}
|
||||
|
||||
public void SafeHome(params string[] axisNames)
|
||||
{
|
||||
SafeHome(CancellationToken.None, axisNames);
|
||||
}
|
||||
|
||||
public void SafeHome(CancellationToken cancellationToken, params string[] axisNames)
|
||||
{
|
||||
RunSync(() => SafeHomeAsync(cancellationToken, axisNames)).EnsureSuccess();
|
||||
}
|
||||
|
||||
public void SafeHome(params IAxis[] axes)
|
||||
{
|
||||
SafeHome(CancellationToken.None, axes);
|
||||
}
|
||||
|
||||
public void SafeHome(CancellationToken cancellationToken, params IAxis[] axes)
|
||||
{
|
||||
RunSync(() => SafeHomeAsync(cancellationToken, axes)).EnsureSuccess();
|
||||
}
|
||||
|
||||
public void Home(string axisName)
|
||||
{
|
||||
Home(axisName, CancellationToken.None);
|
||||
}
|
||||
|
||||
public void Home(string axisName, CancellationToken cancellationToken)
|
||||
{
|
||||
RunSync(() => HomeAsync(axisName, MotionController.DefaultTimeoutMilliseconds, cancellationToken)).EnsureSuccess();
|
||||
}
|
||||
|
||||
public void Home(IAxis axis)
|
||||
{
|
||||
Home(axis, CancellationToken.None);
|
||||
}
|
||||
|
||||
public void Home(IAxis axis, CancellationToken cancellationToken)
|
||||
{
|
||||
RunSync(() => HomeAsync(axis, MotionController.DefaultTimeoutMilliseconds, cancellationToken)).EnsureSuccess();
|
||||
}
|
||||
|
||||
public void Stop(string axisName)
|
||||
{
|
||||
Stop(axisName, CancellationToken.None);
|
||||
}
|
||||
|
||||
public void Stop(string axisName, CancellationToken cancellationToken)
|
||||
{
|
||||
RunSync(() => StopAsync(axisName, 3000, cancellationToken)).EnsureSuccess();
|
||||
}
|
||||
|
||||
public void Stop(IAxis axis)
|
||||
{
|
||||
Stop(axis, CancellationToken.None);
|
||||
}
|
||||
|
||||
public void Stop(IAxis axis, CancellationToken cancellationToken)
|
||||
{
|
||||
RunSync(() => StopAsync(axis, 3000, cancellationToken)).EnsureSuccess();
|
||||
}
|
||||
|
||||
public void Stop(params string[] axisNames)
|
||||
{
|
||||
Stop(CancellationToken.None, axisNames);
|
||||
}
|
||||
|
||||
public void Stop(CancellationToken cancellationToken, params string[] axisNames)
|
||||
{
|
||||
RunSync(() => StopAsync(3000, cancellationToken, axisNames)).EnsureSuccess();
|
||||
}
|
||||
|
||||
public void Stop(params IAxis[] axes)
|
||||
{
|
||||
Stop(CancellationToken.None, axes);
|
||||
}
|
||||
|
||||
public void Stop(CancellationToken cancellationToken, params IAxis[] axes)
|
||||
{
|
||||
RunSync(() => StopAsync(3000, cancellationToken, axes)).EnsureSuccess();
|
||||
}
|
||||
|
||||
public async Task<MotionResult> MoveAbsAsync(string axisName, double targetPos, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken), int? alarmId = null)
|
||||
{
|
||||
var request = ResolveRequest(EnsureRequestBatchMetadata(MotionMoveRequest.ForAxisName(axisName, targetPos, timeoutMilliseconds, alarmId), null));
|
||||
await _motionPrecheckService.ValidateAsync(CreateSafetyContext(MotionRequestKind.MoveAbs, request), cancellationToken, alarmId).ConfigureAwait(false);
|
||||
return await MoveAbsInternalAsync(request, cancellationToken).ConfigureAwait(false);
|
||||
}
|
||||
|
||||
public async Task<MotionResult> MoveAbsAsync(IAxis axis, double targetPos, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken), int? alarmId = null)
|
||||
{
|
||||
var request = ResolveRequest(EnsureRequestBatchMetadata(MotionMoveRequest.ForAxis(axis, targetPos, timeoutMilliseconds, alarmId), null));
|
||||
await _motionPrecheckService.ValidateAsync(CreateSafetyContext(MotionRequestKind.MoveAbs, request), cancellationToken, alarmId).ConfigureAwait(false);
|
||||
return await MoveAbsInternalAsync(request, cancellationToken).ConfigureAwait(false);
|
||||
}
|
||||
|
||||
public async Task<MotionResult> MoveRelAsync(string axisName, double distance, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken), int? alarmId = null)
|
||||
{
|
||||
var request = ResolveRequest(EnsureRequestBatchMetadata(MotionMoveRequest.ForRelativeAxisName(axisName, distance, timeoutMilliseconds, alarmId), null));
|
||||
await _motionPrecheckService.ValidateAsync(CreateSafetyContext(MotionRequestKind.MoveRel, request), cancellationToken, alarmId).ConfigureAwait(false);
|
||||
return await MoveRelInternalAsync(request, cancellationToken).ConfigureAwait(false);
|
||||
}
|
||||
|
||||
public async Task<MotionResult> MoveRelAsync(IAxis axis, double distance, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken), int? alarmId = null)
|
||||
{
|
||||
var request = ResolveRequest(EnsureRequestBatchMetadata(MotionMoveRequest.ForRelativeAxis(axis, distance, timeoutMilliseconds, alarmId), null));
|
||||
await _motionPrecheckService.ValidateAsync(CreateSafetyContext(MotionRequestKind.MoveRel, request), cancellationToken, alarmId).ConfigureAwait(false);
|
||||
return await MoveRelInternalAsync(request, cancellationToken).ConfigureAwait(false);
|
||||
}
|
||||
|
||||
public Task<MotionBatchResult> SafeHomeAsync(params string[] axisNames)
|
||||
{
|
||||
return SafeHomeAsync(CancellationToken.None, axisNames);
|
||||
}
|
||||
|
||||
public Task<MotionBatchResult> SafeHomeAsync(params IAxis[] axes)
|
||||
{
|
||||
return SafeHomeAsync(CancellationToken.None, axes);
|
||||
}
|
||||
|
||||
public async Task<MotionBatchResult> SafeHomeAsync(CancellationToken cancellationToken, params string[] axisNames)
|
||||
{
|
||||
if (axisNames == null || axisNames.Length == 0)
|
||||
{
|
||||
return new MotionBatchResult(Array.Empty<MotionResult>());
|
||||
}
|
||||
|
||||
return await SafeHomeAsync(cancellationToken, axisNames.Select(ResolveAxis).ToArray()).ConfigureAwait(false);
|
||||
}
|
||||
|
||||
public async Task<MotionBatchResult> SafeHomeAsync(CancellationToken cancellationToken, params IAxis[] axes)
|
||||
{
|
||||
if (axes == null || axes.Length == 0)
|
||||
{
|
||||
return new MotionBatchResult(Array.Empty<MotionResult>());
|
||||
}
|
||||
|
||||
var batchId = CreateBatchId(axes.Select(x => x != null ? x.Name : null));
|
||||
var resolvedAxes = axes.Select(EnsureAxis).ToArray();
|
||||
await _motionPrecheckService.ValidateAsync(CreateHomeSafetyContext(resolvedAxes, timeoutMilliseconds: null, batchId: batchId), cancellationToken, null).ConfigureAwait(false);
|
||||
using (var linkedCancellationTokenSource = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken))
|
||||
{
|
||||
var tasks = resolvedAxes.Select(x =>
|
||||
{
|
||||
var homeRequest = BuildHomeRequest(x, MotionController.DefaultTimeoutMilliseconds, null, batchId);
|
||||
return HomeInternalAsync(homeRequest, linkedCancellationTokenSource.Token, linkedCancellationTokenSource, resolvedAxes);
|
||||
}).ToArray();
|
||||
var results = await Task.WhenAll(tasks).ConfigureAwait(false);
|
||||
return new MotionBatchResult(results);
|
||||
}
|
||||
}
|
||||
|
||||
public async Task<MotionResult> HomeAsync(string axisName, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken), int? alarmId = null)
|
||||
{
|
||||
var axis = ResolveAxis(axisName);
|
||||
var request = BuildHomeRequest(axis, timeoutMilliseconds, alarmId, null);
|
||||
await _motionPrecheckService.ValidateAsync(CreateHomeSafetyContext(new[] { axis }, timeoutMilliseconds, request.BatchId), cancellationToken, alarmId).ConfigureAwait(false);
|
||||
return await HomeInternalAsync(request, cancellationToken, null, null).ConfigureAwait(false);
|
||||
}
|
||||
|
||||
public async Task<MotionResult> HomeAsync(IAxis axis, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken), int? alarmId = null)
|
||||
{
|
||||
var resolvedAxis = EnsureAxis(axis);
|
||||
var request = BuildHomeRequest(resolvedAxis, timeoutMilliseconds, alarmId, null);
|
||||
await _motionPrecheckService.ValidateAsync(CreateHomeSafetyContext(new[] { resolvedAxis }, timeoutMilliseconds, request.BatchId), cancellationToken, alarmId).ConfigureAwait(false);
|
||||
return await HomeInternalAsync(request, cancellationToken, null, null).ConfigureAwait(false);
|
||||
}
|
||||
|
||||
public Task<MotionResult> StopAsync(string axisName, int timeoutMilliseconds = 3000, CancellationToken cancellationToken = default(CancellationToken))
|
||||
{
|
||||
return StopAsync(ResolveAxis(axisName), timeoutMilliseconds, cancellationToken);
|
||||
}
|
||||
|
||||
public async Task<MotionResult> StopAsync(IAxis axis, int timeoutMilliseconds = 3000, CancellationToken cancellationToken = default(CancellationToken))
|
||||
{
|
||||
var resolvedAxis = EnsureAxis(axis);
|
||||
var controller = _controllerRegistry.GetController(resolvedAxis);
|
||||
var normalizedTimeout = NormalizeStopTimeout(timeoutMilliseconds);
|
||||
var result = await controller.StopAsync(normalizedTimeout, cancellationToken).ConfigureAwait(false);
|
||||
return result.WithFailureDetails(result.FailureStage, result.TimedOut, result.StoppedByCoordinator, Guid.NewGuid().ToString("N"), null);
|
||||
}
|
||||
|
||||
public Task<MotionBatchResult> StopAsync(params string[] axisNames)
|
||||
{
|
||||
return StopAsync(3000, CancellationToken.None, axisNames);
|
||||
}
|
||||
|
||||
public Task<MotionBatchResult> StopAsync(params IAxis[] axes)
|
||||
{
|
||||
return StopAsync(3000, CancellationToken.None, axes);
|
||||
}
|
||||
|
||||
public async Task<MotionBatchResult> StopAsync(int timeoutMilliseconds, CancellationToken cancellationToken, params string[] axisNames)
|
||||
{
|
||||
if (axisNames == null || axisNames.Length == 0)
|
||||
{
|
||||
return new MotionBatchResult(Array.Empty<MotionResult>());
|
||||
}
|
||||
|
||||
return await StopAsync(timeoutMilliseconds, cancellationToken, axisNames.Select(ResolveAxis).ToArray()).ConfigureAwait(false);
|
||||
}
|
||||
|
||||
public async Task<MotionBatchResult> StopAsync(int timeoutMilliseconds, CancellationToken cancellationToken, params IAxis[] axes)
|
||||
{
|
||||
if (axes == null || axes.Length == 0)
|
||||
{
|
||||
return new MotionBatchResult(Array.Empty<MotionResult>());
|
||||
}
|
||||
|
||||
var resolvedAxes = axes.Select(EnsureAxis).ToArray();
|
||||
var tasks = resolvedAxes.Select(x => StopAsync(x, timeoutMilliseconds, cancellationToken)).ToArray();
|
||||
var results = await Task.WhenAll(tasks).ConfigureAwait(false);
|
||||
return new MotionBatchResult(results);
|
||||
}
|
||||
#endregion
|
||||
|
||||
#region ??????
|
||||
private async Task<MotionResult> MoveAbsInternalAsync(ResolvedMotionMoveRequest resolvedRequest, CancellationToken cancellationToken, CancellationTokenSource coordinatedCancellationTokenSource = null, IReadOnlyCollection<IAxis> relatedAxes = null)
|
||||
{
|
||||
if (resolvedRequest == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(resolvedRequest));
|
||||
}
|
||||
|
||||
var request = resolvedRequest.Request;
|
||||
var axis = resolvedRequest.Axis;
|
||||
var controller = _controllerRegistry.GetController(axis);
|
||||
var timeoutMilliseconds = NormalizeTimeout(request.TimeoutMilliseconds);
|
||||
|
||||
var result = await controller.MoveAbsAsync(request.TargetPosition, timeoutMilliseconds, cancellationToken, request.PositionTolerance).ConfigureAwait(false);
|
||||
result = ApplyRequestMetadata(result, request);
|
||||
if (result.Succeeded || result.Cancelled)
|
||||
{
|
||||
return result;
|
||||
}
|
||||
|
||||
CancelAndStopRelatedAxes(coordinatedCancellationTokenSource, relatedAxes, axis.Name, result.Exception ?? new InvalidOperationException(result.Message), request.StopOnFailure);
|
||||
if (!request.AlarmId.HasValue)
|
||||
{
|
||||
return result.WithFailureDetails("Execute", result.TimedOut, coordinatedCancellationTokenSource != null && coordinatedCancellationTokenSource.IsCancellationRequested, request.CorrelationId, request.BatchId);
|
||||
}
|
||||
|
||||
return result.WithAlarmReported(await _motionAlarmReporter.ReportAlarmAsync(request.AlarmId).ConfigureAwait(false))
|
||||
.WithFailureDetails("Execute", result.TimedOut, coordinatedCancellationTokenSource != null && coordinatedCancellationTokenSource.IsCancellationRequested, request.CorrelationId, request.BatchId);
|
||||
}
|
||||
|
||||
private async Task<MotionResult> MoveRelInternalAsync(ResolvedMotionMoveRequest resolvedRequest, CancellationToken cancellationToken, CancellationTokenSource coordinatedCancellationTokenSource = null, IReadOnlyCollection<IAxis> relatedAxes = null)
|
||||
{
|
||||
if (resolvedRequest == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(resolvedRequest));
|
||||
}
|
||||
|
||||
var request = resolvedRequest.Request;
|
||||
var axis = resolvedRequest.Axis;
|
||||
var controller = _controllerRegistry.GetController(axis);
|
||||
var timeoutMilliseconds = NormalizeTimeout(request.TimeoutMilliseconds);
|
||||
|
||||
var result = await controller.MoveRelAsync(request.TargetPosition, timeoutMilliseconds, cancellationToken, request.PositionTolerance).ConfigureAwait(false);
|
||||
result = ApplyRequestMetadata(result, request);
|
||||
if (result.Succeeded || result.Cancelled)
|
||||
{
|
||||
return result;
|
||||
}
|
||||
|
||||
CancelAndStopRelatedAxes(coordinatedCancellationTokenSource, relatedAxes, axis.Name, result.Exception ?? new InvalidOperationException(result.Message), request.StopOnFailure);
|
||||
if (!request.AlarmId.HasValue)
|
||||
{
|
||||
return result.WithFailureDetails("Execute", result.TimedOut, coordinatedCancellationTokenSource != null && coordinatedCancellationTokenSource.IsCancellationRequested, request.CorrelationId, request.BatchId);
|
||||
}
|
||||
|
||||
return result.WithAlarmReported(await _motionAlarmReporter.ReportAlarmAsync(request.AlarmId).ConfigureAwait(false))
|
||||
.WithFailureDetails("Execute", result.TimedOut, coordinatedCancellationTokenSource != null && coordinatedCancellationTokenSource.IsCancellationRequested, request.CorrelationId, request.BatchId);
|
||||
}
|
||||
|
||||
private async Task<MotionResult> HomeInternalAsync(MotionMoveRequest request, CancellationToken cancellationToken, CancellationTokenSource coordinatedCancellationTokenSource = null, IReadOnlyCollection<IAxis> relatedAxes = null)
|
||||
{
|
||||
if (request == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(request));
|
||||
}
|
||||
|
||||
var axis = EnsureAxis(request.Axis ?? ResolveAxis(request.AxisName));
|
||||
var controller = _controllerRegistry.GetController(axis);
|
||||
var result = await controller.HomeAsync(NormalizeTimeout(request.TimeoutMilliseconds), cancellationToken).ConfigureAwait(false);
|
||||
result = ApplyRequestMetadata(result, request);
|
||||
if (result.Succeeded || result.Cancelled)
|
||||
{
|
||||
return result;
|
||||
}
|
||||
|
||||
CancelAndStopRelatedAxes(coordinatedCancellationTokenSource, relatedAxes, axis.Name, result.Exception ?? new InvalidOperationException(result.Message), request.StopOnFailure);
|
||||
if (!request.AlarmId.HasValue)
|
||||
{
|
||||
return result.WithFailureDetails("Execute", result.TimedOut, coordinatedCancellationTokenSource != null && coordinatedCancellationTokenSource.IsCancellationRequested, request.CorrelationId, request.BatchId);
|
||||
}
|
||||
|
||||
return result.WithAlarmReported(await _motionAlarmReporter.ReportAlarmAsync(request.AlarmId).ConfigureAwait(false))
|
||||
.WithFailureDetails("Execute", result.TimedOut, coordinatedCancellationTokenSource != null && coordinatedCancellationTokenSource.IsCancellationRequested, request.CorrelationId, request.BatchId);
|
||||
}
|
||||
|
||||
private ResolvedMotionMoveRequest ResolveRequest(MotionMoveRequest request)
|
||||
{
|
||||
if (request == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(request));
|
||||
}
|
||||
|
||||
var axis = request.Axis ?? ResolveAxis(request.AxisName);
|
||||
return new ResolvedMotionMoveRequest(request, axis);
|
||||
}
|
||||
|
||||
private static IAxis EnsureAxis(IAxis axis)
|
||||
{
|
||||
if (axis == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(axis));
|
||||
}
|
||||
|
||||
return axis;
|
||||
}
|
||||
|
||||
private MotionSafetyContext CreateSafetyContext(MotionRequestKind requestKind, params ResolvedMotionMoveRequest[] resolvedRequests)
|
||||
{
|
||||
var requests = (resolvedRequests ?? Array.Empty<ResolvedMotionMoveRequest>()).Select(x => x.Request).ToArray();
|
||||
var axes = (resolvedRequests ?? Array.Empty<ResolvedMotionMoveRequest>()).Select(x => x.Axis).ToArray();
|
||||
var timeout = requests.Length == 0 ? (int?)null : requests.Max(x => x.TimeoutMilliseconds);
|
||||
var source = requests.Select(x => x.Source).FirstOrDefault(x => !string.IsNullOrWhiteSpace(x));
|
||||
return new MotionSafetyContext(requestKind, axes, requests, source, timeout);
|
||||
}
|
||||
|
||||
private MotionSafetyContext CreateHomeSafetyContext(IReadOnlyCollection<IAxis> axes, int? timeoutMilliseconds = null, string batchId = null)
|
||||
{
|
||||
var homeRequests = (axes ?? Array.Empty<IAxis>())
|
||||
.Select(x => BuildHomeRequest(x, timeoutMilliseconds ?? MotionController.DefaultTimeoutMilliseconds, null, batchId))
|
||||
.ToArray();
|
||||
|
||||
return new MotionSafetyContext(MotionRequestKind.Home, axes ?? Array.Empty<IAxis>(), homeRequests, "SafeHome", timeoutMilliseconds);
|
||||
}
|
||||
|
||||
private MotionMoveRequest BuildHomeRequest(IAxis axis, int timeoutMilliseconds, int? alarmId, string batchId)
|
||||
{
|
||||
var resolvedAxis = EnsureAxis(axis);
|
||||
var homeRequest = MotionMoveRequest.ForAxis(
|
||||
resolvedAxis,
|
||||
resolvedAxis.State != null ? resolvedAxis.State.ActualPos : resolvedAxis.GetPositionImmediate(),
|
||||
timeoutMilliseconds,
|
||||
alarmId,
|
||||
"SafeHome",
|
||||
new[] { "Home" },
|
||||
null,
|
||||
batchId,
|
||||
null,
|
||||
true);
|
||||
|
||||
return EnsureRequestBatchMetadata(homeRequest, batchId);
|
||||
}
|
||||
|
||||
private static MotionMoveRequest EnsureRequestBatchMetadata(MotionMoveRequest request, string batchId)
|
||||
{
|
||||
if (request == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(request));
|
||||
}
|
||||
|
||||
var ensuredCorrelationId = string.IsNullOrWhiteSpace(request.CorrelationId) ? Guid.NewGuid().ToString("N") : request.CorrelationId;
|
||||
var ensuredBatchId = string.IsNullOrWhiteSpace(request.BatchId) ? batchId : request.BatchId;
|
||||
return request.WithMetadata(correlationId: ensuredCorrelationId, batchId: ensuredBatchId);
|
||||
}
|
||||
|
||||
private static MotionResult ApplyRequestMetadata(MotionResult result, MotionMoveRequest request)
|
||||
{
|
||||
if (result == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(result));
|
||||
}
|
||||
|
||||
if (request == null)
|
||||
{
|
||||
return result;
|
||||
}
|
||||
|
||||
return result.WithFailureDetails(result.FailureStage, result.TimedOut, result.StoppedByCoordinator, request.CorrelationId, request.BatchId);
|
||||
}
|
||||
|
||||
private static string CreateBatchId(IEnumerable<string> seedValues)
|
||||
{
|
||||
var normalized = (seedValues ?? Enumerable.Empty<string>()).Where(x => !string.IsNullOrWhiteSpace(x)).ToArray();
|
||||
return normalized.Length == 0 ? Guid.NewGuid().ToString("N") : string.Format("{0}-{1}", DateTime.UtcNow.ToString("yyyyMMddHHmmssfff"), Math.Abs(string.Join("|", normalized).GetHashCode()));
|
||||
}
|
||||
|
||||
private static string CreateBatchId(IEnumerable<MotionMoveRequest> requests)
|
||||
{
|
||||
return CreateBatchId((requests ?? Enumerable.Empty<MotionMoveRequest>()).Select(x => x != null ? x.AxisName : null));
|
||||
}
|
||||
|
||||
private static int NormalizeTimeout(int timeoutMilliseconds)
|
||||
{
|
||||
return timeoutMilliseconds > 0 ? timeoutMilliseconds : MotionController.DefaultTimeoutMilliseconds;
|
||||
}
|
||||
|
||||
private static int NormalizeStopTimeout(int timeoutMilliseconds)
|
||||
{
|
||||
return timeoutMilliseconds > 0 ? timeoutMilliseconds : 3000;
|
||||
}
|
||||
|
||||
private static TResult RunSync<TResult>(Func<Task<TResult>> action)
|
||||
{
|
||||
if (action == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(action));
|
||||
}
|
||||
|
||||
return Task.Run(action).GetAwaiter().GetResult();
|
||||
}
|
||||
|
||||
private void CancelAndStopRelatedAxes(CancellationTokenSource coordinatedCancellationTokenSource, IReadOnlyCollection<IAxis> relatedAxes, string failedAxisName, Exception exception, bool stopOnFailure)
|
||||
{
|
||||
if (!stopOnFailure)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (coordinatedCancellationTokenSource != null && !coordinatedCancellationTokenSource.IsCancellationRequested)
|
||||
{
|
||||
coordinatedCancellationTokenSource.Cancel();
|
||||
}
|
||||
|
||||
if (relatedAxes == null || relatedAxes.Count <= 1)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
string.Format("Axis:{0} move failed, stopping related axes. Error:{1}", failedAxisName, exception.Message).LogSysError();
|
||||
|
||||
foreach (var axis in relatedAxes)
|
||||
{
|
||||
if (axis == null)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
_controllerRegistry.GetController(axis).StopAsync(3000, CancellationToken.None).GetAwaiter().GetResult();
|
||||
}
|
||||
catch (Exception stopException)
|
||||
{
|
||||
string.Format("Axis:{0} stop failed during coordinated stop. Error:{1}", axis.Name, stopException.Message).LogSysError();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private IAxis ResolveAxis(string axisName)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(axisName))
|
||||
{
|
||||
throw new ArgumentNullException(nameof(axisName));
|
||||
}
|
||||
|
||||
var axis = _hardware.GetAxisByName(axisName);
|
||||
if (axis == null)
|
||||
{
|
||||
throw new ArgumentException(string.Format("Axis with name {0} not found.", axisName), nameof(axisName));
|
||||
}
|
||||
|
||||
return axis;
|
||||
}
|
||||
#endregion
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,439 @@
|
||||
using MainShell.Hardware;
|
||||
using MainShell.Parameter;
|
||||
using MainShell.Common;
|
||||
using MwFramework.Device;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
|
||||
namespace MainShell.Motion.Safety
|
||||
{
|
||||
public sealed class MotionSafetyContext
|
||||
{
|
||||
public MotionSafetyContext(MotionRequestKind requestKind, IReadOnlyCollection<IAxis> axes, IReadOnlyCollection<MotionMoveRequest> requests, string source, int? timeoutMilliseconds)
|
||||
{
|
||||
RequestKind = requestKind;
|
||||
Axes = axes ?? Array.Empty<IAxis>();
|
||||
Requests = requests ?? Array.Empty<MotionMoveRequest>();
|
||||
Source = source;
|
||||
TimeoutMilliseconds = timeoutMilliseconds;
|
||||
}
|
||||
|
||||
public MotionRequestKind RequestKind { get; }
|
||||
public IReadOnlyCollection<IAxis> Axes { get; }
|
||||
public IReadOnlyCollection<MotionMoveRequest> Requests { get; }
|
||||
public string Source { get; }
|
||||
public int? TimeoutMilliseconds { get; }
|
||||
public bool IsBatch => Requests.Count > 1;
|
||||
}
|
||||
|
||||
public sealed class MotionSafetyCheckResult
|
||||
{
|
||||
public MotionSafetyCheckResult(bool isPassed, string ruleName, string message, string severity = "Block", int? alarmId = null, string recoveryHint = null, IEnumerable<string> affectedAxes = null, MessageKey messageKey = MessageKey.None, object[] messageArgs = null)
|
||||
{
|
||||
IsPassed = isPassed;
|
||||
RuleName = ruleName;
|
||||
Message = message;
|
||||
Severity = severity;
|
||||
AlarmId = alarmId;
|
||||
RecoveryHint = recoveryHint;
|
||||
AffectedAxes = (affectedAxes ?? Enumerable.Empty<string>()).Where(x => !string.IsNullOrWhiteSpace(x)).Distinct(StringComparer.OrdinalIgnoreCase).ToArray();
|
||||
MessageKey = messageKey;
|
||||
MessageArgs = messageArgs ?? Array.Empty<object>();
|
||||
}
|
||||
|
||||
public bool IsPassed { get; }
|
||||
public string RuleName { get; }
|
||||
public string Message { get; }
|
||||
public string Severity { get; }
|
||||
public int? AlarmId { get; }
|
||||
public string RecoveryHint { get; }
|
||||
public IReadOnlyCollection<string> AffectedAxes { get; }
|
||||
public MessageKey MessageKey { get; }
|
||||
public object[] MessageArgs { get; }
|
||||
|
||||
public static MotionSafetyCheckResult Passed(string ruleName)
|
||||
{
|
||||
return new MotionSafetyCheckResult(true, ruleName, null);
|
||||
}
|
||||
|
||||
public static MotionSafetyCheckResult Blocked(string ruleName, string message, int? alarmId = null, string recoveryHint = null, IEnumerable<string> affectedAxes = null, MessageKey messageKey = MessageKey.None, params object[] messageArgs)
|
||||
{
|
||||
return new MotionSafetyCheckResult(false, ruleName, message, "Block", alarmId, recoveryHint, affectedAxes, messageKey, messageArgs);
|
||||
}
|
||||
}
|
||||
|
||||
public interface IMotionSafetyCheck
|
||||
{
|
||||
MotionSafetyCheckResult Check(MotionSafetyContext context);
|
||||
}
|
||||
|
||||
public sealed class StagePlatformSafetyOptions
|
||||
{
|
||||
public StagePlatformSafetyOptions(double maxPlaneSpread, double maxTravelPerStep)
|
||||
{
|
||||
MaxPlaneSpread = maxPlaneSpread;
|
||||
MaxTravelPerStep = maxTravelPerStep;
|
||||
}
|
||||
|
||||
public double MaxPlaneSpread { get; }
|
||||
public double MaxTravelPerStep { get; }
|
||||
}
|
||||
|
||||
internal sealed class MotionRequestDuplicateAxisCheck : IMotionSafetyCheck
|
||||
{
|
||||
public MotionSafetyCheckResult Check(MotionSafetyContext context)
|
||||
{
|
||||
if (context == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(context));
|
||||
}
|
||||
|
||||
var duplicateAxisNames = context.Requests
|
||||
.Select(x => x.AxisName)
|
||||
.Where(x => !string.IsNullOrWhiteSpace(x))
|
||||
.GroupBy(x => x, StringComparer.OrdinalIgnoreCase)
|
||||
.Where(x => x.Count() > 1)
|
||||
.Select(x => x.Key)
|
||||
.ToArray();
|
||||
|
||||
if (duplicateAxisNames.Length == 0)
|
||||
{
|
||||
return MotionSafetyCheckResult.Passed(nameof(MotionRequestDuplicateAxisCheck));
|
||||
}
|
||||
|
||||
return MotionSafetyCheckResult.Blocked(
|
||||
nameof(MotionRequestDuplicateAxisCheck),
|
||||
string.Format("Duplicate axis requests detected: {0}.", string.Join(", ", duplicateAxisNames)),
|
||||
recoveryHint: "<22><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ<EFBFBD><C8B7>ͬһ<CDAC><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD>ֻ<EFBFBD><D6BB><EFBFBD><EFBFBD>һ<EFBFBD>Ρ<EFBFBD>",
|
||||
affectedAxes: duplicateAxisNames,
|
||||
messageKey: MessageKey.ProcessFailedWithReason,
|
||||
messageArgs: new object[] { string.Format("<22>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: {0}", string.Join(", ", duplicateAxisNames)) });
|
||||
}
|
||||
}
|
||||
|
||||
internal sealed class MotionBatchSameSourceCheck : IMotionSafetyCheck
|
||||
{
|
||||
public MotionSafetyCheckResult Check(MotionSafetyContext context)
|
||||
{
|
||||
if (context == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(context));
|
||||
}
|
||||
|
||||
var nonEmptySources = context.Requests
|
||||
.Select(x => x.Source)
|
||||
.Where(x => !string.IsNullOrWhiteSpace(x))
|
||||
.Distinct(StringComparer.OrdinalIgnoreCase)
|
||||
.ToArray();
|
||||
|
||||
if (nonEmptySources.Length <= 1)
|
||||
{
|
||||
return MotionSafetyCheckResult.Passed(nameof(MotionBatchSameSourceCheck));
|
||||
}
|
||||
|
||||
return MotionSafetyCheckResult.Blocked(
|
||||
nameof(MotionBatchSameSourceCheck),
|
||||
string.Format("Batch requests must share the same source. Sources: {0}.", string.Join(", ", nonEmptySources)),
|
||||
recoveryHint: "<22><>ȷ<EFBFBD><C8B7>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD>ͬһ<CDAC><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͳһ<CDB3><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>",
|
||||
messageKey: MessageKey.ProcessFailedWithReason,
|
||||
messageArgs: new object[] { string.Format("<22><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Դ<EFBFBD><D4B4>һ<EFBFBD><D2BB>: {0}", string.Join(", ", nonEmptySources)) });
|
||||
}
|
||||
}
|
||||
|
||||
internal sealed class EmergencyStopReleasedCheck : IMotionSafetyCheck
|
||||
{
|
||||
private readonly MotionSafetyStateProvider _stateProvider;
|
||||
|
||||
public EmergencyStopReleasedCheck(MotionSafetyStateProvider stateProvider)
|
||||
{
|
||||
_stateProvider = stateProvider ?? throw new ArgumentNullException(nameof(stateProvider));
|
||||
}
|
||||
|
||||
public MotionSafetyCheckResult Check(MotionSafetyContext context)
|
||||
{
|
||||
if (_stateProvider.IsEmergencyStopReleased())
|
||||
{
|
||||
return MotionSafetyCheckResult.Passed(nameof(EmergencyStopReleasedCheck));
|
||||
}
|
||||
|
||||
return MotionSafetyCheckResult.Blocked(
|
||||
nameof(EmergencyStopReleasedCheck),
|
||||
"Emergency stop is active or safety circuit is not ready.",
|
||||
recoveryHint: "<22><><EFBFBD>ͷż<CDB7>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD>鰲ȫ<E9B0B2><C8AB>·״̬<D7B4><CCAC><EFBFBD><EFBFBD>ִ<EFBFBD><D6B4><EFBFBD>˶<EFBFBD><CBB6><EFBFBD>",
|
||||
affectedAxes: context != null ? context.Axes.Select(x => x != null ? x.Name : null) : null,
|
||||
messageKey: MessageKey.ProcessFailedWithReason,
|
||||
messageArgs: new object[] { "<22><>ͣδ<CDA3>ͷŻ<CDB7><C5BB><EFBFBD>ȫ<EFBFBD><C8AB>·δ<C2B7><CEB4><EFBFBD><EFBFBD>" });
|
||||
}
|
||||
}
|
||||
|
||||
internal sealed class SafetyDoorClosedCheck : IMotionSafetyCheck
|
||||
{
|
||||
private readonly MotionSafetyStateProvider _stateProvider;
|
||||
|
||||
public SafetyDoorClosedCheck(MotionSafetyStateProvider stateProvider)
|
||||
{
|
||||
_stateProvider = stateProvider ?? throw new ArgumentNullException(nameof(stateProvider));
|
||||
}
|
||||
|
||||
public MotionSafetyCheckResult Check(MotionSafetyContext context)
|
||||
{
|
||||
if (_stateProvider.CurrentMachineMode != MachineMode.Auto)
|
||||
{
|
||||
return MotionSafetyCheckResult.Passed(nameof(SafetyDoorClosedCheck));
|
||||
}
|
||||
if (_stateProvider.AreSafetyDoorsClosed())
|
||||
{
|
||||
return MotionSafetyCheckResult.Passed(nameof(SafetyDoorClosedCheck));
|
||||
}
|
||||
|
||||
return MotionSafetyCheckResult.Blocked(
|
||||
nameof(SafetyDoorClosedCheck),
|
||||
"Safety door is open and shield is not active.",
|
||||
recoveryHint: "<22><><EFBFBD>رհ<D8B1>ȫ<EFBFBD>ţ<EFBFBD><C5A3><EFBFBD>ȷ<EFBFBD><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬<D7B4>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD>ϵ<EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ģʽ<C4A3><CABD>",
|
||||
affectedAxes: context != null ? context.Axes.Select(x => x != null ? x.Name : null) : null,
|
||||
messageKey: MessageKey.ProcessFailedWithReason,
|
||||
messageArgs: new object[] { "<22><>ȫ<EFBFBD><C8AB>δ<EFBFBD>ر<EFBFBD>" });
|
||||
}
|
||||
}
|
||||
|
||||
internal sealed class AutoFlowModeCheck : IMotionSafetyCheck
|
||||
{
|
||||
private readonly MotionSafetyStateProvider _stateProvider;
|
||||
|
||||
public AutoFlowModeCheck(MotionSafetyStateProvider stateProvider)
|
||||
{
|
||||
_stateProvider = stateProvider ?? throw new ArgumentNullException(nameof(stateProvider));
|
||||
}
|
||||
|
||||
public MotionSafetyCheckResult Check(MotionSafetyContext context)
|
||||
{
|
||||
if (context == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(context));
|
||||
}
|
||||
|
||||
if (!_stateProvider.IsAutoWorkflowSource(context.Source))
|
||||
{
|
||||
return MotionSafetyCheckResult.Passed(nameof(AutoFlowModeCheck));
|
||||
}
|
||||
|
||||
if (_stateProvider.CurrentMachineMode == MainShell.Common.MachineMode.Auto)
|
||||
{
|
||||
return MotionSafetyCheckResult.Passed(nameof(AutoFlowModeCheck));
|
||||
}
|
||||
|
||||
return MotionSafetyCheckResult.Blocked(
|
||||
nameof(AutoFlowModeCheck),
|
||||
string.Format("Motion source '{0}' requires Auto mode. Current mode: {1}.", context.Source, _stateProvider.CurrentMachineMode),
|
||||
recoveryHint: "<22><><EFBFBD>л<EFBFBD><D0BB><EFBFBD><EFBFBD>Զ<EFBFBD>ģʽ<C4A3><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֶ<EFBFBD><D6B6><EFBFBD>ȫ<EFBFBD><C8AB><EFBFBD><EFBFBD>ִ<EFBFBD>иö<D0B8><C3B6><EFBFBD><EFBFBD><EFBFBD>",
|
||||
affectedAxes: context.Axes.Select(x => x != null ? x.Name : null),
|
||||
messageKey: MessageKey.ProcessFailedWithReason,
|
||||
messageArgs: new object[] { string.Format("<22><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Դ {0} <20><>Ҫ<EFBFBD>Զ<EFBFBD>ģʽ", context.Source) });
|
||||
}
|
||||
}
|
||||
|
||||
internal sealed class StageVacuumReadyCheck : IMotionSafetyCheck
|
||||
{
|
||||
private readonly MotionSafetyStateProvider _stateProvider;
|
||||
private const string StagePlaneTag = "StagePlaneMove";
|
||||
|
||||
public StageVacuumReadyCheck(MotionSafetyStateProvider stateProvider)
|
||||
{
|
||||
_stateProvider = stateProvider ?? throw new ArgumentNullException(nameof(stateProvider));
|
||||
}
|
||||
|
||||
public MotionSafetyCheckResult Check(MotionSafetyContext context)
|
||||
{
|
||||
if (context == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(context));
|
||||
}
|
||||
|
||||
if (!context.Requests.Any(ContainsStagePlaneTag))
|
||||
{
|
||||
return MotionSafetyCheckResult.Passed(nameof(StageVacuumReadyCheck));
|
||||
}
|
||||
|
||||
if (_stateProvider.AreStageVacuumsReady())
|
||||
{
|
||||
return MotionSafetyCheckResult.Passed(nameof(StageVacuumReadyCheck));
|
||||
}
|
||||
|
||||
return MotionSafetyCheckResult.Blocked(
|
||||
nameof(StageVacuumReadyCheck),
|
||||
"Stage vacuum signals are not ready for plane movement.",
|
||||
recoveryHint: "<22><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ̨<C6BD><CCA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬<D7B4><CCAC>ȷ<EFBFBD>ϻ<EFBFBD><CFBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ⱥ<EFBFBD><C8BA><EFBFBD><EFBFBD>ƶ<EFBFBD>ƽ̨<C6BD><CCA8>",
|
||||
affectedAxes: new[] { AxisName.Axis_Stage_Z7, AxisName.Axis_Stage_Z8, AxisName.Axis_Stage_Z9 },
|
||||
messageKey: MessageKey.ProcessFailedWithReason,
|
||||
messageArgs: new object[] { "ƽ̨<C6BD><CCA8><EFBFBD><EFBFBD>δ<EFBFBD><CEB4><EFBFBD><EFBFBD>" });
|
||||
}
|
||||
|
||||
private static bool ContainsStagePlaneTag(MotionMoveRequest request)
|
||||
{
|
||||
return request != null && request.Tags != null && request.Tags.Contains(StagePlaneTag, StringComparer.OrdinalIgnoreCase);
|
||||
}
|
||||
}
|
||||
|
||||
internal sealed class BondHeadSafePositionCheck : IMotionSafetyCheck
|
||||
{
|
||||
private readonly MotionSafetyStateProvider _stateProvider;
|
||||
private const string StagePlaneTag = "StagePlaneMove";
|
||||
|
||||
public BondHeadSafePositionCheck(MotionSafetyStateProvider stateProvider)
|
||||
{
|
||||
_stateProvider = stateProvider ?? throw new ArgumentNullException(nameof(stateProvider));
|
||||
}
|
||||
|
||||
public MotionSafetyCheckResult Check(MotionSafetyContext context)
|
||||
{
|
||||
if (context == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(context));
|
||||
}
|
||||
|
||||
if (!context.Requests.Any(ContainsStagePlaneTag))
|
||||
{
|
||||
return MotionSafetyCheckResult.Passed(nameof(BondHeadSafePositionCheck));
|
||||
}
|
||||
|
||||
var unsafeSignals = _stateProvider.GetUnsafeBondHeadSignals();
|
||||
if (unsafeSignals == null || unsafeSignals.Count == 0)
|
||||
{
|
||||
return MotionSafetyCheckResult.Passed(nameof(BondHeadSafePositionCheck));
|
||||
}
|
||||
|
||||
return MotionSafetyCheckResult.Blocked(
|
||||
nameof(BondHeadSafePositionCheck),
|
||||
string.Format("Bond head safe position signals are not ready: {0}.", string.Join(", ", unsafeSignals)),
|
||||
recoveryHint: "<22><>ȷ<EFBFBD><C8B7> BondHead <20>ѻص<D1BB><D8B5><EFBFBD>ȫλ<C8AB><CEBB><EFBFBD><EFBFBD>ִ<EFBFBD><D6B4>ƽ̨<C6BD><CCA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>",
|
||||
affectedAxes: new[] { AxisName.Axis_Stage_Z7, AxisName.Axis_Stage_Z8, AxisName.Axis_Stage_Z9 },
|
||||
messageKey: MessageKey.ProcessFailedWithReason,
|
||||
messageArgs: new object[] { string.Format("BondHead <20><>ȫλ<C8AB>ź<EFBFBD>δ<EFBFBD><CEB4><EFBFBD><EFBFBD>: {0}", string.Join(", ", unsafeSignals)) });
|
||||
}
|
||||
|
||||
private static bool ContainsStagePlaneTag(MotionMoveRequest request)
|
||||
{
|
||||
return request != null && request.Tags != null && request.Tags.Contains(StagePlaneTag, StringComparer.OrdinalIgnoreCase);
|
||||
}
|
||||
}
|
||||
|
||||
internal sealed class StagePlatformPlaneMoveSafetyCheck : IMotionSafetyCheck
|
||||
{
|
||||
private const string StagePlaneTag = "StagePlaneMove";
|
||||
private readonly HardwareManager _hardware;
|
||||
private readonly StagePlatformSafetyOptions _options;
|
||||
|
||||
public StagePlatformPlaneMoveSafetyCheck(HardwareManager hardware, StagePlatformSafetyOptions options)
|
||||
{
|
||||
_hardware = hardware ?? throw new ArgumentNullException(nameof(hardware));
|
||||
_options = options ?? throw new ArgumentNullException(nameof(options));
|
||||
}
|
||||
|
||||
public MotionSafetyCheckResult Check(MotionSafetyContext context)
|
||||
{
|
||||
if (context == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(context));
|
||||
}
|
||||
|
||||
var requests = context.Requests.Where(ContainsStagePlaneTag).ToArray();
|
||||
if (requests.Length == 0)
|
||||
{
|
||||
return MotionSafetyCheckResult.Passed(nameof(StagePlatformPlaneMoveSafetyCheck));
|
||||
}
|
||||
|
||||
if (requests.Length != 3)
|
||||
{
|
||||
return MotionSafetyCheckResult.Blocked(
|
||||
nameof(StagePlatformPlaneMoveSafetyCheck),
|
||||
"Stage plane move requires exactly 3 axis requests.",
|
||||
recoveryHint: "<22><>ȷ<EFBFBD><C8B7>ƽ̨<C6BD><CCA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> Z7<5A><37>Z8<5A><38>Z9 <20><><EFBFBD><EFBFBD><EFBFBD>ᡣ",
|
||||
affectedAxes: new[] { AxisName.Axis_Stage_Z7, AxisName.Axis_Stage_Z8, AxisName.Axis_Stage_Z9 },
|
||||
messageKey: MessageKey.ProcessFailedWithReason,
|
||||
messageArgs: new object[] { "ƽ̨<C6BD><CCA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ" });
|
||||
}
|
||||
|
||||
var expectedAxes = new[]
|
||||
{
|
||||
AxisName.Axis_Stage_Z7,
|
||||
AxisName.Axis_Stage_Z8,
|
||||
AxisName.Axis_Stage_Z9
|
||||
};
|
||||
|
||||
var requestMap = requests.ToDictionary(x => x.AxisName, x => x, StringComparer.OrdinalIgnoreCase);
|
||||
if (expectedAxes.Any(x => !requestMap.ContainsKey(x)))
|
||||
{
|
||||
return MotionSafetyCheckResult.Blocked(
|
||||
nameof(StagePlatformPlaneMoveSafetyCheck),
|
||||
"Stage plane move must include Stage-Z7, Stage-Z8 and Stage-Z9.",
|
||||
recoveryHint: "<22><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ̨<C6BD><CCA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><DFBC><EFBFBD>",
|
||||
affectedAxes: expectedAxes,
|
||||
messageKey: MessageKey.ProcessFailedWithReason,
|
||||
messageArgs: new object[] { "ƽ̨<C6BD><CCA8><EFBFBD><EFBFBD>ȱ<EFBFBD>ٱ<EFBFBD>Ҫ<EFBFBD><D2AA>" });
|
||||
}
|
||||
|
||||
var currentPositions = new Dictionary<string, double>(StringComparer.OrdinalIgnoreCase)
|
||||
{
|
||||
{ AxisName.Axis_Stage_Z7, GetAxisPosition(_hardware.Axis_Stage_Z7, nameof(_hardware.Axis_Stage_Z7)) },
|
||||
{ AxisName.Axis_Stage_Z8, GetAxisPosition(_hardware.Axis_Stage_Z8, nameof(_hardware.Axis_Stage_Z8)) },
|
||||
{ AxisName.Axis_Stage_Z9, GetAxisPosition(_hardware.Axis_Stage_Z9, nameof(_hardware.Axis_Stage_Z9)) }
|
||||
};
|
||||
|
||||
var targetPositions = expectedAxes.Select(x => requestMap[x].TargetPosition).ToArray();
|
||||
var currentSpread = currentPositions.Values.Max() - currentPositions.Values.Min();
|
||||
var targetSpread = targetPositions.Max() - targetPositions.Min();
|
||||
var maxTravelDelta = expectedAxes.Max(x => Math.Abs(requestMap[x].TargetPosition - currentPositions[x]));
|
||||
|
||||
if (targetSpread > _options.MaxPlaneSpread)
|
||||
{
|
||||
return MotionSafetyCheckResult.Blocked(
|
||||
nameof(StagePlatformPlaneMoveSafetyCheck),
|
||||
string.Format("Stage plane target spread {0:F3} exceeds limit {1:F3}.", targetSpread, _options.MaxPlaneSpread),
|
||||
recoveryHint: "<22><><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>߶Ȳ<C8B2><EEA3AC><EFBFBD><EFBFBD>ִ<EFBFBD><D6B4><EFBFBD>м<EFBFBD><D0BC><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>",
|
||||
affectedAxes: expectedAxes,
|
||||
messageKey: MessageKey.ProcessFailedWithReason,
|
||||
messageArgs: new object[] { string.Format("ƽ̨Ŀ<CCA8><C4BF><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD><EEB3AC>: {0:F3}", targetSpread) });
|
||||
}
|
||||
|
||||
if (currentSpread > _options.MaxPlaneSpread)
|
||||
{
|
||||
return MotionSafetyCheckResult.Blocked(
|
||||
nameof(StagePlatformPlaneMoveSafetyCheck),
|
||||
string.Format("Stage plane current spread {0:F3} exceeds limit {1:F3}.", currentSpread, _options.MaxPlaneSpread),
|
||||
recoveryHint: "<22><><EFBFBD><EFBFBD>ִ<EFBFBD><D6B4>ƽ̨<C6BD><CCA8>ƽ<EFBFBD><C6BD><EFBFBD>ذ<EFBFBD>ȫƽ<C8AB>涯<EFBFBD><E6B6AF><EFBFBD><EFBFBD>",
|
||||
affectedAxes: expectedAxes,
|
||||
messageKey: MessageKey.ProcessFailedWithReason,
|
||||
messageArgs: new object[] { string.Format("ƽ̨<C6BD><CCA8>ǰ<EFBFBD><C7B0>̬<EFBFBD><EFBFBD><EEB3AC>: {0:F3}", currentSpread) });
|
||||
}
|
||||
|
||||
if (maxTravelDelta > _options.MaxTravelPerStep)
|
||||
{
|
||||
return MotionSafetyCheckResult.Blocked(
|
||||
nameof(StagePlatformPlaneMoveSafetyCheck),
|
||||
string.Format("Stage plane travel delta {0:F3} exceeds single-step limit {1:F3}. Consider a safe intermediate move.", maxTravelDelta, _options.MaxTravelPerStep),
|
||||
recoveryHint: "<22><><EFBFBD><EFBFBD>Ϊ<EFBFBD>ֲ<EFBFBD><D6B2><EFBFBD>λ<EFBFBD><CEBB><EFBFBD>ȵ<EFBFBD><C8B5><EFBFBD>ȫ<EFBFBD><C8AB>תλ<D7AA>ٵ<EFBFBD>Ŀ<EFBFBD><C4BF>λ<EFBFBD><CEBB>",
|
||||
affectedAxes: expectedAxes,
|
||||
messageKey: MessageKey.ProcessFailedWithReason,
|
||||
messageArgs: new object[] { string.Format("ƽ̨<C6BD><CCA8><EFBFBD><EFBFBD><EFBFBD>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: {0:F3}", maxTravelDelta) });
|
||||
}
|
||||
|
||||
return MotionSafetyCheckResult.Passed(nameof(StagePlatformPlaneMoveSafetyCheck));
|
||||
}
|
||||
|
||||
private static bool ContainsStagePlaneTag(MotionMoveRequest request)
|
||||
{
|
||||
return request != null && request.Tags != null && request.Tags.Contains(StagePlaneTag, StringComparer.OrdinalIgnoreCase);
|
||||
}
|
||||
|
||||
private static double GetAxisPosition(IAxis axis, string axisPropertyName)
|
||||
{
|
||||
if (axis == null)
|
||||
{
|
||||
throw new InvalidOperationException(string.Format("Axis '{0}' is not available.", axisPropertyName));
|
||||
}
|
||||
|
||||
return axis.State != null ? axis.State.ActualPos : axis.GetPositionImmediate();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,83 @@
|
||||
using MainShell.Hardware;
|
||||
using MaxwellFramework.Core.Attributes;
|
||||
using MwFramework.Device;
|
||||
using System;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace MainShell.Motion
|
||||
{
|
||||
[Singleton]
|
||||
public class StagePlatformMotionService
|
||||
{
|
||||
private readonly HardwareManager _hardware;
|
||||
private readonly SafeAxisMotion _safeAxisMotion;
|
||||
private const string StagePlaneMoveSource = "StagePlatform.MovePlane";
|
||||
private static readonly string[] StagePlaneMoveTags = { "StagePlaneMove", "StagePlatform" };
|
||||
|
||||
public StagePlatformMotionService(HardwareManager hardware, SafeAxisMotion safeAxisMotion)
|
||||
{
|
||||
_hardware = hardware ?? throw new ArgumentNullException(nameof(hardware));
|
||||
_safeAxisMotion = safeAxisMotion ?? throw new ArgumentNullException(nameof(safeAxisMotion));
|
||||
}
|
||||
|
||||
public void MoveFlat(double targetHeight, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken))
|
||||
{
|
||||
RunSync(() => MoveFlatAsync(targetHeight, timeoutMilliseconds, cancellationToken));
|
||||
}
|
||||
|
||||
public Task<MotionBatchResult> MoveFlatAsync(double targetHeight, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken))
|
||||
{
|
||||
return MovePlaneAsync(targetHeight, targetHeight, targetHeight, timeoutMilliseconds, cancellationToken);
|
||||
}
|
||||
|
||||
public void MovePlane(double z7Target, double z8Target, double z9Target, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken))
|
||||
{
|
||||
RunSync(() => MovePlaneAsync(z7Target, z8Target, z9Target, timeoutMilliseconds, cancellationToken)).EnsureSuccess();
|
||||
}
|
||||
|
||||
public async Task<MotionBatchResult> MovePlaneAsync(double z7Target, double z8Target, double z9Target, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken))
|
||||
{
|
||||
EnsurePlaneTargetsOrdered(z7Target, z8Target, z9Target);
|
||||
|
||||
var result = await _safeAxisMotion.SafeMoveAsync(
|
||||
cancellationToken,
|
||||
MotionMoveRequest.ForAxis(GetRequiredAxis(_hardware.Axis_Stage_Z7, nameof(_hardware.Axis_Stage_Z7)), z7Target, timeoutMilliseconds, null, StagePlaneMoveSource, StagePlaneMoveTags),
|
||||
MotionMoveRequest.ForAxis(GetRequiredAxis(_hardware.Axis_Stage_Z8, nameof(_hardware.Axis_Stage_Z8)), z8Target, timeoutMilliseconds, null, StagePlaneMoveSource, StagePlaneMoveTags),
|
||||
MotionMoveRequest.ForAxis(GetRequiredAxis(_hardware.Axis_Stage_Z9, nameof(_hardware.Axis_Stage_Z9)), z9Target, timeoutMilliseconds, null, StagePlaneMoveSource, StagePlaneMoveTags)).ConfigureAwait(false);
|
||||
|
||||
result.EnsureSuccess();
|
||||
return result;
|
||||
}
|
||||
|
||||
private static void EnsurePlaneTargetsOrdered(double z7Target, double z8Target, double z9Target)
|
||||
{
|
||||
var spread = Math.Max(z7Target, Math.Max(z8Target, z9Target)) - Math.Min(z7Target, Math.Min(z8Target, z9Target));
|
||||
const double maxTargetSpread = 5.0d;
|
||||
if (spread > maxTargetSpread)
|
||||
{
|
||||
throw new InvalidOperationException(string.Format("Stage plane target spread {0:F3} exceeds limit {1:F3}.", spread, maxTargetSpread));
|
||||
}
|
||||
}
|
||||
|
||||
private static IAxis GetRequiredAxis(IAxis axis, string axisPropertyName)
|
||||
{
|
||||
if (axis == null)
|
||||
{
|
||||
throw new InvalidOperationException(string.Format("Axis '{0}' is not available.", axisPropertyName));
|
||||
}
|
||||
|
||||
return axis;
|
||||
}
|
||||
|
||||
private static TResult RunSync<TResult>(Func<Task<TResult>> action)
|
||||
{
|
||||
if (action == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(action));
|
||||
}
|
||||
|
||||
return Task.Run(action).GetAwaiter().GetResult();
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user