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