310 lines
12 KiB
C#
310 lines
12 KiB
C#
using MainShell.EventArgsFolder;
|
|
using MainShell.Log;
|
|
using MainShell.Motion;
|
|
using MaxwellFramework.Core.Attributes;
|
|
using MwFramework.Device;
|
|
using MwFramework.Device.Delta;
|
|
using Stylet;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Threading;
|
|
using System.Threading.Tasks;
|
|
|
|
namespace MainShell.Hardware
|
|
{
|
|
[Singleton]
|
|
public sealed class AxisHomeService : IHandle<OriginCalibDataChangedEventArgs>
|
|
{
|
|
private readonly HardwareManager _hardwareManager;
|
|
private readonly SafeAxisMotion _safeAxisMotion;
|
|
private readonly IDeltaAxisExtend _deltaAxisExtend;
|
|
private readonly Dictionary<string, double> _latestOriginCalibOffsets = new Dictionary<string, double>(StringComparer.Ordinal);
|
|
|
|
public IReadOnlyDictionary<string, double> LatestOriginCalibOffsets
|
|
{
|
|
get
|
|
{
|
|
return _latestOriginCalibOffsets;
|
|
}
|
|
}
|
|
|
|
public AxisHomeService(HardwareManager hardwareManager, SafeAxisMotion safeAxisMotion, IEventAggregator eventAggregator)
|
|
{
|
|
_hardwareManager = hardwareManager ?? throw new ArgumentNullException(nameof(hardwareManager));
|
|
_safeAxisMotion = safeAxisMotion ?? throw new ArgumentNullException(nameof(safeAxisMotion));
|
|
if (eventAggregator == null)
|
|
{
|
|
throw new ArgumentNullException(nameof(eventAggregator));
|
|
}
|
|
|
|
eventAggregator.Subscribe(this);
|
|
_deltaAxisExtend=_hardwareManager.TdCard as IDeltaAxisExtend;
|
|
SetHomeDoneEvent(AxisName.Axis_PHS_Z1);
|
|
SetHomeDoneEvent(AxisName.Axis_PHS_Z3);
|
|
SetHomeDoneEvent(AxisName.Axis_PHS_Z4);
|
|
SetHomeDoneEvent(AxisName.Axis_PHS_Z5);
|
|
SetHomeDoneEvent(AxisName.Axis_PHS_Z6);
|
|
}
|
|
|
|
private void SetHomeDoneEvent(string axisName)
|
|
{
|
|
var axis = ResolveAxis(axisName);
|
|
if (axis != null && axis is IDeltaAxisEvent deltaAxis)
|
|
{
|
|
deltaAxis.HomeFinished -= DeltaAxis_HomeFinished;
|
|
deltaAxis.HomeFinished += DeltaAxis_HomeFinished;
|
|
}
|
|
}
|
|
|
|
private void DeltaAxis_HomeFinished(IAxis arg1, bool arg2)
|
|
{
|
|
if(arg2)
|
|
{
|
|
|
|
}
|
|
}
|
|
|
|
private void SetHomeOffset(IAxis axis, double offset)
|
|
{
|
|
if (_deltaAxisExtend != null)
|
|
{
|
|
_deltaAxisExtend.SetAxisCurrentPosition(axis, offset);
|
|
}
|
|
}
|
|
|
|
public void Home(string axisName)
|
|
{
|
|
Home(axisName, CancellationToken.None);
|
|
}
|
|
|
|
public void Home(string axisName, CancellationToken cancellationToken)
|
|
{
|
|
HomeAsync(axisName, MotionController.DefaultTimeoutMilliseconds, cancellationToken).GetAwaiter().GetResult().EnsureSuccess();
|
|
}
|
|
|
|
public void Home(IAxis axis)
|
|
{
|
|
Home(axis, CancellationToken.None);
|
|
}
|
|
|
|
public void Home(IAxis axis, CancellationToken cancellationToken)
|
|
{
|
|
HomeAsync(axis, MotionController.DefaultTimeoutMilliseconds, cancellationToken).GetAwaiter().GetResult().EnsureSuccess();
|
|
}
|
|
|
|
public void Home(params string[] axisNames)
|
|
{
|
|
Home(CancellationToken.None, axisNames);
|
|
}
|
|
|
|
public void Home(CancellationToken cancellationToken, params string[] axisNames)
|
|
{
|
|
HomeAsync(MotionController.DefaultTimeoutMilliseconds, cancellationToken, axisNames).GetAwaiter().GetResult().EnsureSuccess();
|
|
}
|
|
|
|
public async Task<MotionResult> HomeAsync(string axisName, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken), int? alarmId = null)
|
|
{
|
|
if (string.IsNullOrWhiteSpace(axisName))
|
|
{
|
|
throw new ArgumentNullException(nameof(axisName));
|
|
}
|
|
|
|
IAxis axis = ResolveAxis(axisName);
|
|
return await HomeAsync(axis, timeoutMilliseconds, cancellationToken, alarmId).ConfigureAwait(false);
|
|
}
|
|
|
|
public async Task<MotionResult> HomeAsync(IAxis axis, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken), int? alarmId = null)
|
|
{
|
|
IAxis resolvedAxis = axis ?? throw new ArgumentNullException(nameof(axis));
|
|
string axisName = resolvedAxis.Name ?? string.Empty;
|
|
DateTime startedAt = DateTime.Now;
|
|
string.Format("Axis home started. Axis:{0}", axisName).LogInfo();
|
|
|
|
try
|
|
{
|
|
MotionResult result = await _safeAxisMotion.HomeAsync(resolvedAxis, timeoutMilliseconds, cancellationToken, alarmId).ConfigureAwait(false);
|
|
if (result.Succeeded)
|
|
{
|
|
string.Format("Axis home completed. Axis:{0} Duration:{1}ms", axisName, (DateTime.Now - startedAt).TotalMilliseconds.ToString("F0")).LogInfo();
|
|
}
|
|
else if (result.Cancelled)
|
|
{
|
|
string.Format("Axis home cancelled. Axis:{0}", axisName).LogInfo();
|
|
}
|
|
else
|
|
{
|
|
string failureMessage = string.IsNullOrWhiteSpace(result.Message) ? "Unknown axis home failure." : result.Message;
|
|
string.Format("Axis home failed. Axis:{0} Error:{1}", axisName, failureMessage).LogSysError();
|
|
}
|
|
|
|
return result;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
string.Format("Axis home exception. Axis:{0} Error:{1}", axisName, ex.Message).LogSysError();
|
|
throw;
|
|
}
|
|
}
|
|
|
|
public Task<MotionBatchResult> HomeAsync(params string[] axisNames)
|
|
{
|
|
return HomeAsync(MotionController.DefaultTimeoutMilliseconds, CancellationToken.None, axisNames);
|
|
}
|
|
|
|
public Task<MotionBatchResult> HomeAsync(CancellationToken cancellationToken, params string[] axisNames)
|
|
{
|
|
return HomeAsync(MotionController.DefaultTimeoutMilliseconds, cancellationToken, axisNames);
|
|
}
|
|
|
|
public async Task<MotionBatchResult> HomeAsync(int timeoutMilliseconds, CancellationToken cancellationToken, params string[] axisNames)
|
|
{
|
|
string[] normalizedAxisNames = NormalizeAxisNames(axisNames);
|
|
if (normalizedAxisNames.Length == 0)
|
|
{
|
|
return new MotionBatchResult(Array.Empty<MotionResult>());
|
|
}
|
|
|
|
IAxis[] axes = normalizedAxisNames.Select(ResolveAxis).ToArray();
|
|
return await HomeAsync(timeoutMilliseconds, cancellationToken, axes).ConfigureAwait(false);
|
|
}
|
|
|
|
public Task<MotionBatchResult> HomeAsync(params IAxis[] axes)
|
|
{
|
|
return HomeAsync(MotionController.DefaultTimeoutMilliseconds, CancellationToken.None, axes);
|
|
}
|
|
|
|
public Task<MotionBatchResult> HomeAsync(CancellationToken cancellationToken, params IAxis[] axes)
|
|
{
|
|
return HomeAsync(MotionController.DefaultTimeoutMilliseconds, cancellationToken, axes);
|
|
}
|
|
|
|
public async Task<MotionBatchResult> HomeAsync(int timeoutMilliseconds, CancellationToken cancellationToken, params IAxis[] axes)
|
|
{
|
|
IAxis[] normalizedAxes = NormalizeAxes(axes);
|
|
if (normalizedAxes.Length == 0)
|
|
{
|
|
return new MotionBatchResult(Array.Empty<MotionResult>());
|
|
}
|
|
|
|
List<MotionResult> results = new List<MotionResult>(normalizedAxes.Length);
|
|
foreach (IAxis axis in normalizedAxes)
|
|
{
|
|
MotionResult result = await HomeAsync(axis, timeoutMilliseconds, cancellationToken).ConfigureAwait(false);
|
|
results.Add(result);
|
|
}
|
|
|
|
return new MotionBatchResult(results);
|
|
}
|
|
|
|
public Task<MotionBatchResult> SafeHomeAsync(params string[] axisNames)
|
|
{
|
|
return SafeHomeAsync(CancellationToken.None, axisNames);
|
|
}
|
|
|
|
public async Task<MotionBatchResult> SafeHomeAsync(CancellationToken cancellationToken, params string[] axisNames)
|
|
{
|
|
string[] normalizedAxisNames = NormalizeAxisNames(axisNames);
|
|
if (normalizedAxisNames.Length == 0)
|
|
{
|
|
return new MotionBatchResult(Array.Empty<MotionResult>());
|
|
}
|
|
|
|
string.Format("Batch axis home started. Axes:{0}", string.Join(", ", normalizedAxisNames)).LogInfo();
|
|
|
|
try
|
|
{
|
|
MotionBatchResult result = await _safeAxisMotion.SafeHomeAsync(cancellationToken, normalizedAxisNames).ConfigureAwait(false);
|
|
if (result.Succeeded)
|
|
{
|
|
string.Format("Batch axis home completed. Axes:{0}", string.Join(", ", normalizedAxisNames)).LogInfo();
|
|
}
|
|
else
|
|
{
|
|
IEnumerable<string> failedAxes = result.Results.Where(x => x != null && !x.Succeeded).Select(x => x.AxisName ?? string.Empty);
|
|
string.Format("Batch axis home failed. Axes:{0}", string.Join(", ", failedAxes)).LogSysError();
|
|
}
|
|
|
|
return result;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
string.Format("Batch axis home exception. Axes:{0} Error:{1}", string.Join(", ", normalizedAxisNames), ex.Message).LogSysError();
|
|
throw;
|
|
}
|
|
}
|
|
|
|
public IAxis GetAxis(string axisName)
|
|
{
|
|
return ResolveAxis(axisName);
|
|
}
|
|
|
|
public bool TryGetOriginCalibOffset(string axisName, out double offset)
|
|
{
|
|
offset = 0;
|
|
|
|
if (string.IsNullOrWhiteSpace(axisName))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
return _latestOriginCalibOffsets.TryGetValue(axisName, out offset);
|
|
}
|
|
|
|
private IAxis ResolveAxis(string axisName)
|
|
{
|
|
IAxis axis = _hardwareManager.GetAxisByName(axisName);
|
|
if (axis == null)
|
|
{
|
|
throw new ArgumentException(string.Format("Axis with name {0} was not found.", axisName), nameof(axisName));
|
|
}
|
|
|
|
return axis;
|
|
}
|
|
|
|
private static string[] NormalizeAxisNames(IEnumerable<string> axisNames)
|
|
{
|
|
return (axisNames ?? Enumerable.Empty<string>())
|
|
.Where(x => !string.IsNullOrWhiteSpace(x))
|
|
.Distinct(StringComparer.Ordinal)
|
|
.ToArray();
|
|
}
|
|
|
|
private static IAxis[] NormalizeAxes(IEnumerable<IAxis> axes)
|
|
{
|
|
return (axes ?? Enumerable.Empty<IAxis>())
|
|
.Where(x => x != null)
|
|
.GroupBy(x => x.Name ?? string.Empty, StringComparer.Ordinal)
|
|
.Select(x => x.First())
|
|
.ToArray();
|
|
}
|
|
|
|
public void Handle(OriginCalibDataChangedEventArgs eventArgs)
|
|
{
|
|
if (eventArgs == null)
|
|
{
|
|
return;
|
|
}
|
|
|
|
_latestOriginCalibOffsets.Clear();
|
|
|
|
if (eventArgs.Axes != null)
|
|
{
|
|
foreach (OriginCalibAxisOffsetChangedItem axis in eventArgs.Axes)
|
|
{
|
|
if (axis == null || string.IsNullOrWhiteSpace(axis.AxisName))
|
|
{
|
|
continue;
|
|
}
|
|
|
|
_latestOriginCalibOffsets[axis.AxisName] = axis.Offset;
|
|
}
|
|
}
|
|
|
|
string.Format(
|
|
"Origin calibration data changed received. AxisCount:{0}",
|
|
_latestOriginCalibOffsets.Count).LogInfo();
|
|
}
|
|
}
|
|
}
|