Files
test_demo/MX-PD-盘古 - new/PanGu.DieBonderApp/MainShell/Motion/Safety/MotionSafetyChecks.cs

440 lines
19 KiB
C#
Raw Normal View History

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();
}
}
}