585 lines
28 KiB
C#
585 lines
28 KiB
C#
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
|
|
}
|
|
}
|