添加 MX-PD-盘古 项目文件
将 MX-PD-盘古 - new 目录下的所有文件添加到主仓库
This commit is contained in:
Shi.Ji
@@ -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;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user