Initial commit
This commit is contained in:
@@ -0,0 +1,84 @@
|
||||
namespace RobotNet10.RobotApp.Modules;
|
||||
|
||||
/// <summary>
|
||||
/// Interface cho Lift Module - Module điều khiển nâng hạ
|
||||
/// </summary>
|
||||
public interface ILiftModule
|
||||
{
|
||||
/// <summary>
|
||||
/// Nâng lên (lift up)
|
||||
/// </summary>
|
||||
Task LiftUpAsync(CancellationToken cancellationToken = default);
|
||||
|
||||
/// <summary>
|
||||
/// Hạ xuống (lift down)
|
||||
/// </summary>
|
||||
Task LiftDownAsync(CancellationToken cancellationToken = default);
|
||||
|
||||
/// <summary>
|
||||
/// Dừng nâng/hạ ngay lập tức (khi đang di chuyển)
|
||||
/// </summary>
|
||||
Task LiftStopAsync(CancellationToken cancellationToken = default);
|
||||
|
||||
/// <summary>
|
||||
/// Di chuyển đến vị trí cụ thể
|
||||
/// </summary>
|
||||
/// <param name="position">Vị trí target (encoder counts)</param>
|
||||
Task LiftToPositionAsync(int position, CancellationToken cancellationToken = default);
|
||||
|
||||
/// <summary>
|
||||
/// Lấy vị trí hiện tại
|
||||
/// </summary>
|
||||
Task<int> GetCurrentPositionAsync(CancellationToken cancellationToken = default);
|
||||
|
||||
/// <summary>
|
||||
/// Kiểm tra module đã homed chưa
|
||||
/// </summary>
|
||||
Task<bool> IsHomedAsync(CancellationToken cancellationToken = default);
|
||||
|
||||
/// <summary>
|
||||
/// Thực hiện homing thủ công (nếu cần gọi lại sau khi fault hoặc thay đổi cơ khí)
|
||||
/// </summary>
|
||||
Task HomeAsync(CancellationToken cancellationToken = default);
|
||||
|
||||
/// <summary>
|
||||
/// Chuyển chiều cao (m) sang vị trí encoder. Dùng cho action liftCameraByHeight.
|
||||
/// </summary>
|
||||
/// <param name="heightM">Chiều cao theo mét (trong khoảng MinHeightM..MaxHeightM từ config)</param>
|
||||
/// <returns>Vị trí encoder tương ứng</returns>
|
||||
int GetPositionFromHeightM(double heightM);
|
||||
|
||||
bool Enable { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Kiểm tra module đã sẵn sàng chưa (initialized và homed)
|
||||
/// </summary>
|
||||
bool IsReady { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Lấy trạng thái hiện tại của module
|
||||
/// </summary>
|
||||
LiftModuleState State { get; }
|
||||
|
||||
LiftPosition Position { get; }
|
||||
}
|
||||
|
||||
public enum LiftPosition
|
||||
{
|
||||
Bottom,
|
||||
Middle,
|
||||
Top,
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Trạng thái của Lift Module
|
||||
/// </summary>
|
||||
public enum LiftModuleState
|
||||
{
|
||||
Uninitialized,
|
||||
Initializing,
|
||||
Homing,
|
||||
Ready,
|
||||
Moving,
|
||||
Error
|
||||
}
|
||||
@@ -0,0 +1,54 @@
|
||||
namespace RobotNet10.RobotApp.Modules;
|
||||
|
||||
/// <summary>
|
||||
/// Interface cho Rotation Module - Module điều khiển xoay
|
||||
/// </summary>
|
||||
public interface IRotationModule
|
||||
{
|
||||
/// <summary>
|
||||
/// Xoay đến góc cụ thể (absolute angle in degrees)
|
||||
/// </summary>
|
||||
/// <param name="angleDegrees">Góc target (degrees)</param>
|
||||
Task RotateToAngleAsync(double angleDegrees, CancellationToken cancellationToken = default);
|
||||
|
||||
/// <summary>
|
||||
/// Xoay một góc offset (relative angle in degrees)
|
||||
/// </summary>
|
||||
/// <param name="angleOffsetDegrees">Góc offset cần xoay (degrees, positive = clockwise, negative = counter-clockwise)</param>
|
||||
Task RotateOffsetAsync(double angleOffsetDegrees, CancellationToken cancellationToken = default);
|
||||
|
||||
/// <summary>
|
||||
/// Lấy góc hiện tại (degrees)
|
||||
/// </summary>
|
||||
Task<double> GetCurrentAngleAsync(CancellationToken cancellationToken = default);
|
||||
|
||||
/// <summary>
|
||||
/// Kiểm tra module đã homed chưa
|
||||
/// </summary>
|
||||
Task<bool> IsHomedAsync(CancellationToken cancellationToken = default);
|
||||
|
||||
bool Enable{ get; }
|
||||
|
||||
/// <summary>
|
||||
/// Kiểm tra module đã sẵn sàng chưa (initialized và homed)
|
||||
/// </summary>
|
||||
bool IsReady { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Lấy trạng thái hiện tại của module
|
||||
/// </summary>
|
||||
RotationModuleState State { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Trạng thái của Rotation Module
|
||||
/// </summary>
|
||||
public enum RotationModuleState
|
||||
{
|
||||
Uninitialized,
|
||||
Initializing,
|
||||
Homing,
|
||||
Ready,
|
||||
Moving,
|
||||
Error
|
||||
}
|
||||
@@ -0,0 +1,654 @@
|
||||
using Appccelerate.StateMachine;
|
||||
using Appccelerate.StateMachine.Machine;
|
||||
using RobotNet10.CANOpen.CiA402.Enums;
|
||||
using RobotNet10.RobotApp.Devices;
|
||||
using System.Threading;
|
||||
|
||||
namespace RobotNet10.RobotApp.Modules;
|
||||
|
||||
/// <summary>
|
||||
/// Triggers cho LiftModuleService state machine
|
||||
/// </summary>
|
||||
public enum LiftModuleTrigger
|
||||
{
|
||||
StartInitialize,
|
||||
InitializationCompleted,
|
||||
StartHoming,
|
||||
HomingCompleted,
|
||||
HomingFailed,
|
||||
StartMoving,
|
||||
StopMoving,
|
||||
MovingCompleted,
|
||||
MovingFailed,
|
||||
ErrorOccurred,
|
||||
Reset
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Configuration cho LiftModuleService
|
||||
/// </summary>
|
||||
internal class LiftModuleConfiguration
|
||||
{
|
||||
public bool Enable { get; set; }
|
||||
public string DeviceId { get; set; } = "lift-motor";
|
||||
public byte HomingMethod { get; set; } = 35; // Default homing method
|
||||
public int HomingSpeed { get; set; } = 1000; // encoder counts/s
|
||||
public int HomingOffset { get; set; } = 0; // encoder counts
|
||||
public uint ProfileVelocity { get; set; } = 2000; // encoder counts/s
|
||||
public uint ProfileAcceleration { get; set; } = 5000; // encoder counts/s²
|
||||
public uint ProfileDeceleration { get; set; } = 5000; // encoder counts/s²
|
||||
public int MinPosition { get; set; } // Giới hạn dưới (null = không giới hạn)
|
||||
public int MaxPosition { get; set; } // Giới hạn trên (null = không giới hạn)
|
||||
/// <summary>Chiều cao min (m) tương ứng MinPosition. Dùng cho liftCameraByHeight.</summary>
|
||||
public double MinHeightM { get; set; } = 0;
|
||||
/// <summary>Chiều cao max (m) tương ứng MaxPosition. Dùng cho liftCameraByHeight.</summary>
|
||||
public double MaxHeightM { get; set; } = 1;
|
||||
public int StatusWordCheckIntervalMs { get; set; } = 50; // Interval để check status word khi moving
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Service điều khiển module nâng hạ
|
||||
/// Sử dụng CiA402Servo để điều khiển động cơ nâng
|
||||
/// </summary>
|
||||
public class LiftModuleService : IHostedService, ILiftModule, IDisposable
|
||||
{
|
||||
private readonly PassiveStateMachine<LiftModuleState, LiftModuleTrigger> _stateMachine;
|
||||
private readonly LiftModuleConfiguration _config;
|
||||
private readonly IDeviceProvider _deviceProvider;
|
||||
private readonly ILogger<LiftModuleService> _logger;
|
||||
private readonly Lock _lock = new();
|
||||
|
||||
private readonly IRotationModule _rotationModule;
|
||||
private ICiA402Servo? _servo;
|
||||
private LiftModuleState _currentState = LiftModuleState.Uninitialized;
|
||||
private LiftPosition _currentPosition = LiftPosition.Bottom;
|
||||
private bool _isHomed = false;
|
||||
private bool _disposed = false;
|
||||
private int _targetPosition = 0;
|
||||
private CancellationTokenSource? _movementCts;
|
||||
|
||||
public bool Enable => _config.Enable;
|
||||
|
||||
// Nới điều kiện IsReady: chỉ cần module được bật và servo đã lấy được từ DeviceProvider
|
||||
public bool IsReady => _config.Enable && _servo != null;
|
||||
|
||||
public LiftModuleState State => _currentState;
|
||||
public LiftPosition Position => _currentPosition;
|
||||
|
||||
public LiftModuleService(
|
||||
IConfiguration configuration,
|
||||
IDeviceProvider deviceProvider,
|
||||
IRotationModule rotationModule,
|
||||
ILogger<LiftModuleService> logger)
|
||||
{
|
||||
_deviceProvider = deviceProvider;
|
||||
_rotationModule = rotationModule;
|
||||
_logger = logger ?? throw new ArgumentNullException(nameof(logger));
|
||||
|
||||
_stateMachine = BuildStateMachine();
|
||||
|
||||
// Load configuration
|
||||
_config = new LiftModuleConfiguration();
|
||||
var configSection = configuration.GetSection("Modules:LiftModule");
|
||||
if (configSection.Exists())
|
||||
{
|
||||
configSection.Bind(_config);
|
||||
|
||||
// Validate configuration
|
||||
ValidateConfiguration();
|
||||
|
||||
// Build state machine
|
||||
_stateMachine.Start();
|
||||
}
|
||||
}
|
||||
|
||||
private void ValidateConfiguration()
|
||||
{
|
||||
// Nếu Enable = false, không cần validate configuration
|
||||
if (!_config.Enable)
|
||||
{
|
||||
_logger.LogInformation("LiftModule is disabled (Enable = false). Skipping configuration validation.");
|
||||
return;
|
||||
}
|
||||
|
||||
if (string.IsNullOrWhiteSpace(_config.DeviceId))
|
||||
throw new InvalidOperationException("DeviceId is required in LiftModule configuration");
|
||||
|
||||
if (_config.HomingSpeed <= 0)
|
||||
throw new InvalidOperationException("HomingSpeed must be > 0");
|
||||
|
||||
if (_config.ProfileVelocity <= 0)
|
||||
throw new InvalidOperationException("ProfileVelocity must be > 0");
|
||||
|
||||
if (_config.ProfileAcceleration <= 0)
|
||||
throw new InvalidOperationException("ProfileAcceleration must be > 0");
|
||||
|
||||
if (_config.ProfileDeceleration <= 0)
|
||||
throw new InvalidOperationException("ProfileDeceleration must be > 0");
|
||||
|
||||
if (_config.MinPosition >= _config.MaxPosition)
|
||||
throw new InvalidOperationException("MinPosition must be less than MaxPosition");
|
||||
}
|
||||
|
||||
private PassiveStateMachine<LiftModuleState, LiftModuleTrigger> BuildStateMachine()
|
||||
{
|
||||
var builder = new StateMachineDefinitionBuilder<LiftModuleState, LiftModuleTrigger>();
|
||||
|
||||
// Uninitialized state
|
||||
builder.In(LiftModuleState.Uninitialized)
|
||||
.ExecuteOnEntry(() =>
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_currentState = LiftModuleState.Uninitialized;
|
||||
_isHomed = false;
|
||||
}
|
||||
})
|
||||
.On(LiftModuleTrigger.StartInitialize)
|
||||
.Goto(LiftModuleState.Initializing);
|
||||
|
||||
// Initializing state
|
||||
builder.In(LiftModuleState.Initializing)
|
||||
.ExecuteOnEntry(() =>
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_currentState = LiftModuleState.Ready;
|
||||
}
|
||||
_ = Task.Run(async () =>
|
||||
{
|
||||
try
|
||||
{
|
||||
if (_servo == null)
|
||||
throw new InvalidOperationException("Servo is not available");
|
||||
|
||||
// Set profile and homing parameters using async API
|
||||
await _servo.SetProfileVelocityAsync(_config.ProfileVelocity);
|
||||
await _servo.SetProfileAccelerationAsync(_config.ProfileAcceleration);
|
||||
await _servo.SetProfileDecelerationAsync(_config.ProfileDeceleration);
|
||||
await _servo.SetHomingMethodAsync(_config.HomingMethod);
|
||||
await _servo.SetHomingSpeedAsync(_config.HomingSpeed);
|
||||
await _servo.SetHomingOffsetAsync(_config.HomingOffset);
|
||||
_stateMachine.Fire(LiftModuleTrigger.InitializationCompleted);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error during initialization");
|
||||
_stateMachine.Fire(LiftModuleTrigger.ErrorOccurred);
|
||||
}
|
||||
});
|
||||
})
|
||||
.On(LiftModuleTrigger.InitializationCompleted)
|
||||
.Goto(LiftModuleState.Homing)
|
||||
.On(LiftModuleTrigger.ErrorOccurred)
|
||||
.Goto(LiftModuleState.Error);
|
||||
|
||||
// Homing state
|
||||
builder.In(LiftModuleState.Homing)
|
||||
.ExecuteOnEntry(async () =>
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_currentState = LiftModuleState.Homing;
|
||||
}
|
||||
try
|
||||
{
|
||||
await PerformHomingAsync();
|
||||
UpdatePosition();
|
||||
_stateMachine.Fire(LiftModuleTrigger.HomingCompleted);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error during homing");
|
||||
_stateMachine.Fire(LiftModuleTrigger.HomingFailed);
|
||||
}
|
||||
})
|
||||
.On(LiftModuleTrigger.HomingCompleted)
|
||||
.Goto(LiftModuleState.Ready)
|
||||
.On(LiftModuleTrigger.HomingFailed)
|
||||
.Goto(LiftModuleState.Error)
|
||||
.On(LiftModuleTrigger.ErrorOccurred)
|
||||
.Goto(LiftModuleState.Error);
|
||||
|
||||
// Ready state
|
||||
builder.In(LiftModuleState.Ready)
|
||||
.ExecuteOnEntry(() =>
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_currentState = LiftModuleState.Ready;
|
||||
}
|
||||
})
|
||||
.On(LiftModuleTrigger.StartMoving)
|
||||
.Goto(LiftModuleState.Moving)
|
||||
.On(LiftModuleTrigger.ErrorOccurred)
|
||||
.Goto(LiftModuleState.Error);
|
||||
|
||||
// Moving state
|
||||
builder.In(LiftModuleState.Moving)
|
||||
.ExecuteOnEntry(() =>
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_currentState = LiftModuleState.Moving;
|
||||
_movementCts = new CancellationTokenSource();
|
||||
}
|
||||
var cts = _movementCts;
|
||||
_ = Task.Run(async () =>
|
||||
{
|
||||
try
|
||||
{
|
||||
if (_servo is null)
|
||||
{
|
||||
_logger.LogError("LiftModule Moving: servo is null, cannot call motor.");
|
||||
_stateMachine.Fire(LiftModuleTrigger.MovingFailed);
|
||||
return;
|
||||
}
|
||||
|
||||
_logger.LogInformation("LiftModule: Moving state entered, calling servo.MoveToPositionAsync(position={Position}).", _targetPosition);
|
||||
// MoveToPositionAsync handles SetOperationMode, Enable and profile parameters
|
||||
await _servo.MoveToPositionAsync(
|
||||
_targetPosition,
|
||||
_config.ProfileVelocity,
|
||||
_config.ProfileAcceleration,
|
||||
_config.ProfileDeceleration);
|
||||
|
||||
// Wait for movement to complete (cancellable when user presses Stop)
|
||||
await WaitForMovementCompletedAsync(cts?.Token ?? CancellationToken.None);
|
||||
|
||||
UpdatePosition();
|
||||
// Return to Ready state
|
||||
_stateMachine.Fire(LiftModuleTrigger.MovingCompleted);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
// User pressed Stop; state already changed to Ready via StopMoving
|
||||
UpdatePosition();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error during movement to position {Position}", _targetPosition);
|
||||
_stateMachine.Fire(LiftModuleTrigger.MovingFailed);
|
||||
throw;
|
||||
}
|
||||
});
|
||||
})
|
||||
.On(LiftModuleTrigger.StopMoving)
|
||||
.Goto(LiftModuleState.Ready)
|
||||
.On(LiftModuleTrigger.MovingCompleted)
|
||||
.Goto(LiftModuleState.Ready)
|
||||
.On(LiftModuleTrigger.MovingFailed)
|
||||
.Goto(LiftModuleState.Error)
|
||||
.On(LiftModuleTrigger.ErrorOccurred)
|
||||
.Goto(LiftModuleState.Error);
|
||||
|
||||
// Error state
|
||||
builder.In(LiftModuleState.Error)
|
||||
.ExecuteOnEntry(() =>
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_currentState = LiftModuleState.Error;
|
||||
}
|
||||
_logger.LogError("LiftModule state: Error");
|
||||
})
|
||||
.On(LiftModuleTrigger.Reset)
|
||||
.Goto(LiftModuleState.Uninitialized);
|
||||
|
||||
return builder
|
||||
.WithInitialState(LiftModuleState.Uninitialized)
|
||||
.Build()
|
||||
.CreatePassiveStateMachine();
|
||||
}
|
||||
|
||||
#region IHostedService
|
||||
|
||||
public async Task StartAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
try
|
||||
{
|
||||
// Nếu Enable = false, không khởi tạo
|
||||
if (!_config.Enable)
|
||||
{
|
||||
_logger.LogInformation("LiftModule is disabled (Enable = false). Service will not be initialized.");
|
||||
return;
|
||||
}
|
||||
|
||||
// Đợi DeviceProvider kết nối xong tất cả devices
|
||||
var connected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromMinutes(5), cancellationToken);
|
||||
|
||||
if (!connected)
|
||||
{
|
||||
_logger.LogWarning("Timeout waiting for devices to connect. LiftModuleService will not be initialized.");
|
||||
return;
|
||||
}
|
||||
|
||||
// Get CiA402Servo device
|
||||
var device = _deviceProvider.GetDevice(_config.DeviceId);
|
||||
if (device is not ICiA402Servo servo)
|
||||
{
|
||||
_logger.LogError("Device '{DeviceId}' is not an ICiA402Servo", _config.DeviceId);
|
||||
return;
|
||||
}
|
||||
|
||||
_servo = servo;
|
||||
|
||||
// Chỉ chờ rotation module khi rotation được bật (có hardware)
|
||||
if (_rotationModule.Enable)
|
||||
{
|
||||
while (!_rotationModule.IsReady && !cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
await Task.Delay(500, CancellationToken.None);
|
||||
}
|
||||
}
|
||||
|
||||
// Start initialization (will trigger homing automatically)
|
||||
_stateMachine.Fire(LiftModuleTrigger.StartInitialize);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error starting LiftModuleService");
|
||||
}
|
||||
}
|
||||
|
||||
public async Task StopAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
try
|
||||
{
|
||||
// Disable servo if moving
|
||||
if (_servo != null && _currentState == LiftModuleState.Moving)
|
||||
{
|
||||
try
|
||||
{
|
||||
await _servo.DisableAsync(cancellationToken);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogWarning(ex, "Error disabling servo during stop");
|
||||
}
|
||||
}
|
||||
|
||||
_stateMachine.Stop();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "LiftModuleService: Error during StopAsync()");
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Private Methods
|
||||
|
||||
private void UpdatePosition()
|
||||
{
|
||||
if (_servo != null && Math.Abs(_servo.CachedPosition - _config.MinPosition) < 1000) _currentPosition = LiftPosition.Bottom;
|
||||
else if (_servo != null && Math.Abs(_servo.CachedPosition - _config.MaxPosition) < 1000) _currentPosition = LiftPosition.Top;
|
||||
else _currentPosition = LiftPosition.Middle;
|
||||
}
|
||||
|
||||
private async Task PerformHomingAsync()
|
||||
{
|
||||
if (_servo == null)
|
||||
throw new InvalidOperationException("Servo is not available");
|
||||
|
||||
// Check if servo is in fault state
|
||||
var isFault = await _servo.IsInFaultStateAsync();
|
||||
if (isFault)
|
||||
{
|
||||
_logger.LogWarning("Servo is in fault state, attempting fault reset...");
|
||||
await _servo.FaultResetAsync();
|
||||
Thread.Sleep(200);
|
||||
}
|
||||
|
||||
// Enable servo
|
||||
await _servo.EnableAsync();
|
||||
Thread.Sleep(100);
|
||||
|
||||
// StartHomingAsync chờ HomingAttained/HomingError/timeout rồi clear controlword và chuyển mode về Profile Position.
|
||||
// Sau khi chuyển mode, bit HomingAttained có thể bị xóa nên không poll lại statusword ở đây — tin kết quả từ StartHomingAsync (return = thành công, throw = lỗi/timeout).
|
||||
await _servo.StartHomingAsync(_config.HomingMethod, _config.HomingSpeed, CancellationToken.None);
|
||||
|
||||
lock (_lock)
|
||||
{
|
||||
_isHomed = true;
|
||||
}
|
||||
}
|
||||
|
||||
private async Task WaitForMovementCompletedAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
if (_servo == null)
|
||||
throw new InvalidOperationException("Servo is not available");
|
||||
|
||||
var maxWaitTime = TimeSpan.FromMinutes(5); // Long timeout for safety
|
||||
var startTime = DateTime.UtcNow;
|
||||
var checkInterval = TimeSpan.FromMilliseconds(_config.StatusWordCheckIntervalMs);
|
||||
|
||||
while (DateTime.UtcNow - startTime < maxWaitTime)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
var statusword = await _servo.GetStatuswordAsync(cancellationToken);
|
||||
var state = statusword.GetState();
|
||||
|
||||
// Check for fault
|
||||
if (state == DriveState.Fault)
|
||||
{
|
||||
throw new InvalidOperationException("Movement failed: Servo entered fault state");
|
||||
}
|
||||
|
||||
// Check if target reached
|
||||
var currentPosition = await _servo.GetActualPositionAsync(cancellationToken);
|
||||
if (statusword.TargetReached && Math.Abs(currentPosition - _targetPosition) < 900)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
await Task.Delay(checkInterval, cancellationToken);
|
||||
}
|
||||
|
||||
throw new TimeoutException($"Movement timeout after {maxWaitTime.TotalMinutes} minutes");
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region ILiftModule Implementation
|
||||
|
||||
public async Task LiftUpAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
if (!_config.Enable)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (_servo == null)
|
||||
throw new InvalidOperationException("Servo is not available");
|
||||
|
||||
|
||||
await LiftToPositionAsync(_config.MaxPosition, cancellationToken);
|
||||
}
|
||||
|
||||
public async Task LiftDownAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
if (!_config.Enable)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (_servo == null)
|
||||
throw new InvalidOperationException("Servo is not available");
|
||||
|
||||
await LiftToPositionAsync(_config.MinPosition, cancellationToken);
|
||||
}
|
||||
|
||||
public async Task LiftStopAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
if (!_config.Enable)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (_servo == null)
|
||||
return;
|
||||
|
||||
lock (_lock)
|
||||
{
|
||||
if (_currentState != LiftModuleState.Moving)
|
||||
{
|
||||
_logger.LogDebug("LiftStopAsync ignored: module not in Moving state. Current state: {State}", _currentState);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
_movementCts?.Cancel();
|
||||
await _servo.QuickStopAsync(cancellationToken);
|
||||
_stateMachine.Fire(LiftModuleTrigger.StopMoving);
|
||||
_logger.LogInformation("LiftModule: Stop requested, QuickStop sent, state -> Ready.");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogWarning(ex, "LiftStopAsync: error during stop");
|
||||
}
|
||||
}
|
||||
|
||||
public async Task LiftToPositionAsync(int position, CancellationToken cancellationToken = default)
|
||||
{
|
||||
if (!_config.Enable)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (_servo == null)
|
||||
throw new InvalidOperationException("Servo is not available");
|
||||
|
||||
// Check limits
|
||||
if (position < _config.MinPosition)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(position),
|
||||
$"Position {position} is below MinPosition {_config.MinPosition}");
|
||||
}
|
||||
|
||||
if (position > _config.MaxPosition)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(position),
|
||||
$"Position {position} is above MaxPosition {_config.MaxPosition}");
|
||||
}
|
||||
|
||||
// Chỉ chấp nhận lệnh move khi đang ở Ready (state machine chỉ chuyển Ready -> Moving)
|
||||
lock (_lock)
|
||||
{
|
||||
if (_currentState != LiftModuleState.Ready)
|
||||
{
|
||||
_logger.LogWarning("LiftToPositionAsync ignored: module not in Ready state. Current state: {State}", _currentState);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// Fire trigger to enter Moving state (must be done synchronously before async operations)
|
||||
try
|
||||
{
|
||||
_targetPosition = position;
|
||||
_stateMachine.Fire(LiftModuleTrigger.StartMoving);
|
||||
_logger.LogInformation("LiftModule: StartMoving fired, target position {Position} (encoder). Servo will be called in Moving state.", position);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Failed to fire StartMoving trigger");
|
||||
throw new InvalidOperationException("Failed to start movement", ex);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public Task<int> GetCurrentPositionAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
if (!_config.Enable)
|
||||
{
|
||||
return Task.FromResult(0);
|
||||
}
|
||||
|
||||
if (_servo == null)
|
||||
throw new InvalidOperationException("Servo is not available");
|
||||
|
||||
// Use async API; callers can await this Task
|
||||
return _servo.GetActualPositionAsync(cancellationToken);
|
||||
}
|
||||
|
||||
public async Task<bool> IsHomedAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
if (!_config.Enable)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
lock (_lock)
|
||||
{
|
||||
return _isHomed;
|
||||
}
|
||||
}
|
||||
|
||||
public async Task HomeAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
if (!_config.Enable)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (_servo == null)
|
||||
throw new InvalidOperationException("Servo is not available");
|
||||
|
||||
// Làm homing giống hệt UI Devices (CiA402ServoCard)
|
||||
// 1. Reset fault nếu có
|
||||
await _servo.TryFaultResetAsync(cancellationToken);
|
||||
|
||||
// 2. Đảm bảo đã enable
|
||||
await _servo.EnableAsync(cancellationToken);
|
||||
|
||||
// 3. Ghi lại homing params từ config
|
||||
await _servo.SetHomingMethodAsync(_config.HomingMethod, cancellationToken);
|
||||
await _servo.SetHomingSpeedAsync(_config.HomingSpeed, cancellationToken);
|
||||
await _servo.SetHomingOffsetAsync(_config.HomingOffset, cancellationToken);
|
||||
|
||||
// 4. Start homing (non‑blocking đối với caller)
|
||||
_logger.LogInformation("LiftModule: calling servo.StartHomingAsync(method={Method}, speed={Speed}).", _config.HomingMethod, _config.HomingSpeed);
|
||||
await _servo.StartHomingAsync(_config.HomingMethod, _config.HomingSpeed, cancellationToken);
|
||||
|
||||
// Không chờ kết thúc để tránh block UI; chỉ cập nhật trạng thái cục bộ sơ bộ
|
||||
UpdatePosition();
|
||||
}
|
||||
|
||||
public int GetPositionFromHeightM(double heightM)
|
||||
{
|
||||
if (_config.MinHeightM >= _config.MaxHeightM)
|
||||
return _config.MinPosition;
|
||||
var t = (heightM - _config.MinHeightM) / (_config.MaxHeightM - _config.MinHeightM);
|
||||
t = Math.Clamp(t, 0, 1);
|
||||
return _config.MinPosition + (int)(t * (_config.MaxPosition - _config.MinPosition));
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region IDisposable
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (_disposed)
|
||||
return;
|
||||
|
||||
_disposed = true;
|
||||
|
||||
try
|
||||
{
|
||||
_stateMachine.Stop();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error disposing LiftModuleService");
|
||||
}
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
@@ -0,0 +1,36 @@
|
||||
namespace RobotNet10.RobotApp.Modules;
|
||||
|
||||
/// <summary>
|
||||
/// No-op lift module used when hardware lift module is not enabled.
|
||||
/// </summary>
|
||||
public sealed class NoOpLiftModule : ILiftModule
|
||||
{
|
||||
public bool Enable => false;
|
||||
public bool IsReady => false;
|
||||
public LiftModuleState State => LiftModuleState.Uninitialized;
|
||||
public LiftPosition Position => LiftPosition.Bottom;
|
||||
|
||||
public Task LiftUpAsync(CancellationToken cancellationToken = default) => Task.CompletedTask;
|
||||
public Task LiftDownAsync(CancellationToken cancellationToken = default) => Task.CompletedTask;
|
||||
public Task LiftStopAsync(CancellationToken cancellationToken = default) => Task.CompletedTask;
|
||||
public Task LiftToPositionAsync(int position, CancellationToken cancellationToken = default) => Task.CompletedTask;
|
||||
public Task<int> GetCurrentPositionAsync(CancellationToken cancellationToken = default) => Task.FromResult(0);
|
||||
public Task<bool> IsHomedAsync(CancellationToken cancellationToken = default) => Task.FromResult(false);
|
||||
public Task HomeAsync(CancellationToken cancellationToken = default) => Task.CompletedTask;
|
||||
public int GetPositionFromHeightM(double heightM) => 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// No-op rotation module used when hardware rotation module is not enabled.
|
||||
/// </summary>
|
||||
public sealed class NoOpRotationModule : IRotationModule
|
||||
{
|
||||
public bool Enable => false;
|
||||
public bool IsReady => false;
|
||||
public RotationModuleState State => RotationModuleState.Uninitialized;
|
||||
|
||||
public Task RotateToAngleAsync(double angleDegrees, CancellationToken cancellationToken = default) => Task.CompletedTask;
|
||||
public Task RotateOffsetAsync(double angleOffsetDegrees, CancellationToken cancellationToken = default) => Task.CompletedTask;
|
||||
public Task<double> GetCurrentAngleAsync(CancellationToken cancellationToken = default) => Task.FromResult(0.0);
|
||||
public Task<bool> IsHomedAsync(CancellationToken cancellationToken = default) => Task.FromResult(false);
|
||||
}
|
||||
@@ -0,0 +1,607 @@
|
||||
// using Appccelerate.StateMachine;
|
||||
// using Appccelerate.StateMachine.Machine;
|
||||
// using Microsoft.Extensions.Configuration;
|
||||
// using Microsoft.Extensions.Hosting;
|
||||
// using Microsoft.Extensions.Logging;
|
||||
// using RobotNet10.CANOpen.CiA402.Enums;
|
||||
// using RobotNet10.RobotApp.Devices;
|
||||
|
||||
// namespace RobotNet10.RobotApp.Modules;
|
||||
|
||||
// /// <summary>
|
||||
// /// Triggers cho RotationModuleService state machine
|
||||
// /// </summary>
|
||||
// public enum RotationModuleTrigger
|
||||
// {
|
||||
// StartInitialize,
|
||||
// InitializationCompleted,
|
||||
// StartHoming,
|
||||
// HomingCompleted,
|
||||
// HomingFailed,
|
||||
// StartMoving,
|
||||
// MovingCompleted,
|
||||
// MovingFailed,
|
||||
// ErrorOccurred,
|
||||
// Reset
|
||||
// }
|
||||
|
||||
// /// <summary>
|
||||
// /// Configuration cho RotationModuleService
|
||||
// /// </summary>
|
||||
// internal class RotationModuleConfiguration
|
||||
// {
|
||||
// public bool Enable { get; set; }
|
||||
// public string DeviceId { get; set; } = string.Empty;
|
||||
// public byte HomingMethod { get; set; } = 35; // Default homing method
|
||||
// public int HomingSpeed { get; set; } = 1000; // encoder counts/s
|
||||
// public int HomingOffset { get; set; } = 0; // encoder counts
|
||||
// public uint ProfileVelocity { get; set; } = 2000; // encoder counts/s
|
||||
// public uint ProfileAcceleration { get; set; } = 5000; // encoder counts/s²
|
||||
// public uint ProfileDeceleration { get; set; } = 5000; // encoder counts/s²
|
||||
// public double Resolution { get; set; } = 360000.0; // encoder counts per 360 degrees (default: 360000 counts/360deg = 1000 counts/deg)
|
||||
// public bool IsReversed { get; set; } = false; // Đảo chiều động cơ (nếu true, góc dương sẽ quay ngược chiều)
|
||||
// public double? MinAngle { get; set; } // Giới hạn góc dưới (degrees, null = không giới hạn)
|
||||
// public double? MaxAngle { get; set; } // Giới hạn góc trên (degrees, null = không giới hạn)
|
||||
// public int StatusWordCheckIntervalMs { get; set; } = 50; // Interval để check status word khi moving
|
||||
// }
|
||||
|
||||
// /// <summary>
|
||||
// /// Service điều khiển module xoay
|
||||
// /// Sử dụng CiA402Servo để điều khiển động cơ xoay
|
||||
// /// </summary>
|
||||
// public class RotationModuleService : IHostedService, IRotationModule, IDisposable
|
||||
// {
|
||||
// private readonly PassiveStateMachine<RotationModuleState, RotationModuleTrigger> _stateMachine;
|
||||
// private readonly RotationModuleConfiguration _config;
|
||||
// private readonly IDeviceProvider _deviceProvider;
|
||||
// private readonly ILogger<RotationModuleService> _logger;
|
||||
// private readonly Lock _lock = new();
|
||||
|
||||
// private ICiA402Servo? _servo;
|
||||
// private RotationModuleState _currentState = RotationModuleState.Uninitialized;
|
||||
// private bool _isHomed = false;
|
||||
// private bool _disposed = false;
|
||||
|
||||
// public bool Enable => _config.Enable;
|
||||
// public bool IsReady => _currentState == RotationModuleState.Ready && _isHomed && _servo != null;
|
||||
// public RotationModuleState State => _currentState;
|
||||
|
||||
// public RotationModuleService(
|
||||
// IConfiguration configuration,
|
||||
// IDeviceProvider deviceProvider,
|
||||
// ILogger<RotationModuleService> logger)
|
||||
// {
|
||||
// _deviceProvider = deviceProvider ?? throw new ArgumentNullException(nameof(deviceProvider));
|
||||
// _logger = logger ?? throw new ArgumentNullException(nameof(logger));
|
||||
|
||||
// // Load configuration
|
||||
// var configSection = configuration.GetSection("Modules:RotationModule");
|
||||
// if (!configSection.Exists())
|
||||
// {
|
||||
// throw new InvalidOperationException("Configuration section 'Modules:RotationModule' not found in appsettings.json");
|
||||
// }
|
||||
|
||||
// _config = new RotationModuleConfiguration();
|
||||
// configSection.Bind(_config);
|
||||
|
||||
// // Validate configuration
|
||||
// ValidateConfiguration();
|
||||
|
||||
// // Build state machine
|
||||
// _stateMachine = BuildStateMachine();
|
||||
// _stateMachine.Start();
|
||||
// }
|
||||
|
||||
// private void ValidateConfiguration()
|
||||
// {
|
||||
// // Nếu Enable = false, không cần validate configuration
|
||||
// if (!_config.Enable)
|
||||
// {
|
||||
// _logger.LogInformation("RotationModule is disabled (Enable = false). Skipping configuration validation.");
|
||||
// return;
|
||||
// }
|
||||
|
||||
// if (string.IsNullOrWhiteSpace(_config.DeviceId))
|
||||
// throw new InvalidOperationException("DeviceId is required in RotationModule configuration");
|
||||
|
||||
// if (_config.HomingSpeed <= 0)
|
||||
// throw new InvalidOperationException("HomingSpeed must be > 0");
|
||||
|
||||
// if (_config.ProfileVelocity <= 0)
|
||||
// throw new InvalidOperationException("ProfileVelocity must be > 0");
|
||||
|
||||
// if (_config.ProfileAcceleration <= 0)
|
||||
// throw new InvalidOperationException("ProfileAcceleration must be > 0");
|
||||
|
||||
// if (_config.ProfileDeceleration <= 0)
|
||||
// throw new InvalidOperationException("ProfileDeceleration must be > 0");
|
||||
|
||||
// if (_config.Resolution <= 0)
|
||||
// throw new InvalidOperationException("Resolution must be > 0");
|
||||
|
||||
// if (_config.MinAngle.HasValue && _config.MaxAngle.HasValue)
|
||||
// {
|
||||
// if (_config.MinAngle.Value >= _config.MaxAngle.Value)
|
||||
// throw new InvalidOperationException("MinAngle must be less than MaxAngle");
|
||||
// }
|
||||
// }
|
||||
|
||||
// private PassiveStateMachine<RotationModuleState, RotationModuleTrigger> BuildStateMachine()
|
||||
// {
|
||||
// var builder = new StateMachineDefinitionBuilder<RotationModuleState, RotationModuleTrigger>();
|
||||
|
||||
// // Uninitialized state
|
||||
// builder.In(RotationModuleState.Uninitialized)
|
||||
// .ExecuteOnEntry(() =>
|
||||
// {
|
||||
// lock (_lock)
|
||||
// {
|
||||
// _currentState = RotationModuleState.Uninitialized;
|
||||
// _isHomed = false;
|
||||
// }
|
||||
// })
|
||||
// .On(RotationModuleTrigger.StartInitialize)
|
||||
// .Goto(RotationModuleState.Initializing);
|
||||
|
||||
// // Initializing state
|
||||
// builder.In(RotationModuleState.Initializing)
|
||||
// .ExecuteOnEntry(() =>
|
||||
// {
|
||||
// lock (_lock)
|
||||
// {
|
||||
// _currentState = RotationModuleState.Initializing;
|
||||
// }
|
||||
// try
|
||||
// {
|
||||
// if (_servo == null)
|
||||
// throw new InvalidOperationException("Servo is not available");
|
||||
|
||||
// // Set profile parameters (synchronous)
|
||||
// _servo.SetProfileVelocity(_config.ProfileVelocity);
|
||||
// _servo.SetProfileAcceleration(_config.ProfileAcceleration);
|
||||
// _servo.SetProfileDeceleration(_config.ProfileDeceleration);
|
||||
// _servo.SetHomingMethod(_config.HomingMethod);
|
||||
// _servo.SetHomingSpeed(_config.HomingSpeed);
|
||||
// _servo.SetHomingOffset(_config.HomingOffset);
|
||||
// _stateMachine.Fire(RotationModuleTrigger.InitializationCompleted);
|
||||
// }
|
||||
// catch (Exception ex)
|
||||
// {
|
||||
// _logger.LogError(ex, "Error during initialization");
|
||||
// _stateMachine.Fire(RotationModuleTrigger.ErrorOccurred);
|
||||
// }
|
||||
// })
|
||||
// .On(RotationModuleTrigger.InitializationCompleted)
|
||||
// .Goto(RotationModuleState.Homing)
|
||||
// .On(RotationModuleTrigger.ErrorOccurred)
|
||||
// .Goto(RotationModuleState.Error);
|
||||
|
||||
// // Homing state
|
||||
// builder.In(RotationModuleState.Homing)
|
||||
// .ExecuteOnEntry(() =>
|
||||
// {
|
||||
// lock (_lock)
|
||||
// {
|
||||
// _currentState = RotationModuleState.Homing;
|
||||
// }
|
||||
// try
|
||||
// {
|
||||
// PerformHomingAsync();
|
||||
// _stateMachine.Fire(RotationModuleTrigger.HomingCompleted);
|
||||
// }
|
||||
// catch (Exception ex)
|
||||
// {
|
||||
// _logger.LogError(ex, "Error during homing");
|
||||
// _stateMachine.Fire(RotationModuleTrigger.HomingFailed);
|
||||
// }
|
||||
// })
|
||||
// .On(RotationModuleTrigger.HomingCompleted)
|
||||
// .Goto(RotationModuleState.Ready)
|
||||
// .On(RotationModuleTrigger.HomingFailed)
|
||||
// .Goto(RotationModuleState.Error)
|
||||
// .On(RotationModuleTrigger.ErrorOccurred)
|
||||
// .Goto(RotationModuleState.Error);
|
||||
|
||||
// // Ready state
|
||||
// builder.In(RotationModuleState.Ready)
|
||||
// .ExecuteOnEntry(() =>
|
||||
// {
|
||||
// lock (_lock)
|
||||
// {
|
||||
// _currentState = RotationModuleState.Ready;
|
||||
// }
|
||||
// })
|
||||
// .On(RotationModuleTrigger.StartMoving)
|
||||
// .Goto(RotationModuleState.Moving)
|
||||
// .On(RotationModuleTrigger.ErrorOccurred)
|
||||
// .Goto(RotationModuleState.Error);
|
||||
|
||||
// // Moving state
|
||||
// builder.In(RotationModuleState.Moving)
|
||||
// .ExecuteOnEntry(() =>
|
||||
// {
|
||||
// lock (_lock)
|
||||
// {
|
||||
// _currentState = RotationModuleState.Moving;
|
||||
// }
|
||||
// })
|
||||
// .On(RotationModuleTrigger.MovingCompleted)
|
||||
// .Goto(RotationModuleState.Ready)
|
||||
// .On(RotationModuleTrigger.MovingFailed)
|
||||
// .Goto(RotationModuleState.Error)
|
||||
// .On(RotationModuleTrigger.ErrorOccurred)
|
||||
// .Goto(RotationModuleState.Error);
|
||||
|
||||
// // Error state
|
||||
// builder.In(RotationModuleState.Error)
|
||||
// .ExecuteOnEntry(() =>
|
||||
// {
|
||||
// lock (_lock)
|
||||
// {
|
||||
// _currentState = RotationModuleState.Error;
|
||||
// }
|
||||
// _logger.LogError("RotationModule state: Error");
|
||||
// })
|
||||
// .On(RotationModuleTrigger.Reset)
|
||||
// .Goto(RotationModuleState.Uninitialized);
|
||||
|
||||
// return builder
|
||||
// .WithInitialState(RotationModuleState.Uninitialized)
|
||||
// .Build()
|
||||
// .CreatePassiveStateMachine();
|
||||
// }
|
||||
|
||||
// #region IHostedService
|
||||
|
||||
// public async Task StartAsync(CancellationToken cancellationToken)
|
||||
// {
|
||||
// try
|
||||
// {
|
||||
// // Nếu Enable = false, không khởi tạo
|
||||
// if (!_config.Enable)
|
||||
// {
|
||||
// _logger.LogInformation("RotationModule is disabled (Enable = false). Service will not be initialized.");
|
||||
// return;
|
||||
// }
|
||||
|
||||
// // Đợi DeviceProvider kết nối xong tất cả devices
|
||||
// var connected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromMinutes(5), cancellationToken);
|
||||
|
||||
// if (!connected)
|
||||
// {
|
||||
// _logger.LogWarning("Timeout waiting for devices to connect. RotationModuleService will not be initialized.");
|
||||
// return;
|
||||
// }
|
||||
|
||||
// // Get CiA402Servo device
|
||||
// var device = _deviceProvider.GetDevice(_config.DeviceId);
|
||||
// if (device is not ICiA402Servo servo)
|
||||
// {
|
||||
// _logger.LogError("Device '{DeviceId}' is not an ICiA402Servo", _config.DeviceId);
|
||||
// return;
|
||||
// }
|
||||
|
||||
// _servo = servo;
|
||||
|
||||
// // Start initialization (will trigger homing automatically)
|
||||
// _stateMachine.Fire(RotationModuleTrigger.StartInitialize);
|
||||
// }
|
||||
// catch (Exception ex)
|
||||
// {
|
||||
// _logger.LogError(ex, "Error starting RotationModuleService");
|
||||
// }
|
||||
// }
|
||||
|
||||
// public async Task StopAsync(CancellationToken cancellationToken)
|
||||
// {
|
||||
// try
|
||||
// {
|
||||
// // Disable servo if moving
|
||||
// if (_servo != null && _currentState == RotationModuleState.Moving)
|
||||
// {
|
||||
// try
|
||||
// {
|
||||
// _servo.Disable();
|
||||
// }
|
||||
// catch (Exception ex)
|
||||
// {
|
||||
// _logger.LogWarning(ex, "Error disabling servo during stop");
|
||||
// }
|
||||
// }
|
||||
|
||||
// _stateMachine.Stop();
|
||||
// }
|
||||
// catch (Exception ex)
|
||||
// {
|
||||
// _logger.LogError(ex, "RotationModuleService: Error during StopAsync()");
|
||||
// throw;
|
||||
// }
|
||||
// }
|
||||
|
||||
// #endregion
|
||||
|
||||
// #region Private Methods
|
||||
|
||||
// private void PerformHomingAsync()
|
||||
// {
|
||||
// if (_servo == null)
|
||||
// throw new InvalidOperationException("Servo is not available");
|
||||
|
||||
// // Check if servo is in fault state
|
||||
// var isFault = _servo.IsInFaultState();
|
||||
// if (isFault)
|
||||
// {
|
||||
// _logger.LogWarning("Servo is in fault state, attempting fault reset...");
|
||||
// _servo.FaultReset();
|
||||
// Thread.Sleep(200);
|
||||
// }
|
||||
|
||||
// // Enable servo
|
||||
// _servo.Enable();
|
||||
// Thread.Sleep(100);
|
||||
|
||||
// // Perform homing
|
||||
// _servo.StartHoming();
|
||||
|
||||
// // Wait for homing to complete
|
||||
// // Check statusword for homing completed
|
||||
// var maxWaitTime = TimeSpan.FromSeconds(120);
|
||||
// var startTime = DateTime.UtcNow;
|
||||
|
||||
// while (DateTime.UtcNow - startTime < maxWaitTime)
|
||||
// {
|
||||
// var statusword = _servo.GetStatusword();
|
||||
// var state = statusword.GetState();
|
||||
|
||||
// // Check for fault
|
||||
// if (state == DriveState.Fault)
|
||||
// {
|
||||
// throw new InvalidOperationException("Homing failed: Servo entered fault state");
|
||||
// }
|
||||
|
||||
// // Homing completion should be determined by CiA402 homing bits, not by drive state.
|
||||
// // During homing the drive is typically still OperationEnabled.
|
||||
// if (statusword.HomingAttained)
|
||||
// {
|
||||
// lock (_lock)
|
||||
// {
|
||||
// _isHomed = true;
|
||||
// }
|
||||
// return;
|
||||
// }
|
||||
|
||||
// // Check homing error bit
|
||||
// if (statusword.HomingError)
|
||||
// {
|
||||
// throw new InvalidOperationException("Homing failed: Statusword indicates homing error");
|
||||
// }
|
||||
// Thread.Sleep(100);
|
||||
// }
|
||||
|
||||
// throw new TimeoutException($"Homing timeout after {maxWaitTime.TotalSeconds} seconds");
|
||||
// }
|
||||
|
||||
// /// <summary>
|
||||
// /// Convert angle (degrees) to encoder position (counts) relative to home position
|
||||
// /// Resolution is in counts per 360 degrees
|
||||
// /// </summary>
|
||||
// private int AngleToPosition(double angleDegrees)
|
||||
// {
|
||||
// // Convert angle to position: angleDegrees / 360 * Resolution
|
||||
// var position = (int)Math.Round(angleDegrees * _config.Resolution / 360.0);
|
||||
|
||||
// // Apply reversal if needed
|
||||
// if (_config.IsReversed)
|
||||
// {
|
||||
// position = -position;
|
||||
// }
|
||||
|
||||
// return position;
|
||||
// }
|
||||
|
||||
// /// <summary>
|
||||
// /// Convert encoder position (counts) to angle (degrees) relative to home position
|
||||
// /// Resolution is in counts per 360 degrees
|
||||
// /// </summary>
|
||||
// private double PositionToAngle(int position)
|
||||
// {
|
||||
// // Calculate position relative to home
|
||||
// var relativePosition = position;
|
||||
|
||||
// // Apply reversal if needed (before converting to angle)
|
||||
// if (_config.IsReversed)
|
||||
// {
|
||||
// relativePosition = -relativePosition;
|
||||
// }
|
||||
|
||||
// // Convert position to angle: relativePosition / Resolution * 360
|
||||
// return relativePosition * 360.0 / _config.Resolution;
|
||||
// }
|
||||
|
||||
// private async Task WaitForMovementCompletedAsync(CancellationToken cancellationToken = default)
|
||||
// {
|
||||
// if (_servo == null)
|
||||
// throw new InvalidOperationException("Servo is not available");
|
||||
|
||||
// var maxWaitTime = TimeSpan.FromMinutes(5); // Long timeout for safety
|
||||
// var startTime = DateTime.UtcNow;
|
||||
// var checkInterval = TimeSpan.FromMilliseconds(_config.StatusWordCheckIntervalMs);
|
||||
|
||||
// while (DateTime.UtcNow - startTime < maxWaitTime)
|
||||
// {
|
||||
// cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
// var statusword = _servo.GetStatusword();
|
||||
// var state = statusword.GetState();
|
||||
|
||||
// // Check for fault
|
||||
// if (state == DriveState.Fault)
|
||||
// {
|
||||
// throw new InvalidOperationException("Movement failed: Servo entered fault state");
|
||||
// }
|
||||
|
||||
// // Check if target reached
|
||||
// if (statusword.TargetReached)
|
||||
// {
|
||||
// return;
|
||||
// }
|
||||
|
||||
// await Task.Delay(checkInterval, cancellationToken);
|
||||
// }
|
||||
|
||||
// throw new TimeoutException($"Movement timeout after {maxWaitTime.TotalMinutes} minutes");
|
||||
// }
|
||||
|
||||
// #endregion
|
||||
|
||||
// #region IRotationModule Implementation
|
||||
// public async Task RotateToAngleAsync(double angleDegrees, CancellationToken cancellationToken = default)
|
||||
// {
|
||||
// if (!_config.Enable)
|
||||
// {
|
||||
// return;
|
||||
// }
|
||||
|
||||
// // Check state before starting
|
||||
// var currentState = State;
|
||||
|
||||
// if (currentState != RotationModuleState.Ready)
|
||||
// {
|
||||
// throw new InvalidOperationException($"RotationModule is not ready. Current state: {currentState}. Please wait until module is ready.");
|
||||
// }
|
||||
|
||||
// if (_servo == null)
|
||||
// throw new InvalidOperationException("Servo is not available");
|
||||
|
||||
// // Check limits
|
||||
// if (_config.MinAngle.HasValue && angleDegrees < _config.MinAngle.Value)
|
||||
// {
|
||||
// throw new ArgumentOutOfRangeException(nameof(angleDegrees),
|
||||
// $"Angle {angleDegrees}° is below MinAngle {_config.MinAngle.Value}°");
|
||||
// }
|
||||
|
||||
// if (_config.MaxAngle.HasValue && angleDegrees > _config.MaxAngle.Value)
|
||||
// {
|
||||
// throw new ArgumentOutOfRangeException(nameof(angleDegrees),
|
||||
// $"Angle {angleDegrees}° is above MaxAngle {_config.MaxAngle.Value}°");
|
||||
// }
|
||||
|
||||
// // Convert angle to encoder position relative to home
|
||||
// var targetPosition = AngleToPosition(angleDegrees);
|
||||
|
||||
// // Fire trigger to enter Moving state
|
||||
// try
|
||||
// {
|
||||
// _stateMachine.Fire(RotationModuleTrigger.StartMoving);
|
||||
// }
|
||||
// catch (Exception ex)
|
||||
// {
|
||||
// _logger.LogError(ex, "Failed to fire StartMoving trigger");
|
||||
// throw new InvalidOperationException("Failed to start rotation", ex);
|
||||
// }
|
||||
|
||||
// try
|
||||
// {
|
||||
// // MoveToPosition already handles: SetOperationMode, Enable, SetProfile parameters
|
||||
// _servo.MoveToPosition(
|
||||
// targetPosition,
|
||||
// _config.ProfileVelocity,
|
||||
// _config.ProfileAcceleration,
|
||||
// _config.ProfileDeceleration);
|
||||
|
||||
// // Wait for movement to complete
|
||||
// await WaitForMovementCompletedAsync(cancellationToken);
|
||||
|
||||
// // Return to Ready state
|
||||
// try
|
||||
// {
|
||||
// _stateMachine.Fire(RotationModuleTrigger.MovingCompleted);
|
||||
// }
|
||||
// catch (Exception ex)
|
||||
// {
|
||||
// _logger.LogError(ex, "Failed to fire MovingCompleted trigger. Current state: {State}", State);
|
||||
// }
|
||||
// }
|
||||
// catch (Exception ex)
|
||||
// {
|
||||
// _logger.LogError(ex, "Error during rotation to angle {Angle}°", angleDegrees);
|
||||
// try
|
||||
// {
|
||||
// _stateMachine.Fire(RotationModuleTrigger.MovingFailed);
|
||||
// }
|
||||
// catch (Exception stateEx)
|
||||
// {
|
||||
// _logger.LogError(stateEx, "Failed to fire MovingFailed trigger");
|
||||
// }
|
||||
// throw;
|
||||
// }
|
||||
// }
|
||||
|
||||
// public async Task RotateOffsetAsync(double angleOffsetDegrees, CancellationToken cancellationToken = default)
|
||||
// {
|
||||
// if (!_config.Enable)
|
||||
// {
|
||||
// return;
|
||||
// }
|
||||
|
||||
// // Get current angle
|
||||
// var currentAngle = await GetCurrentAngleAsync(cancellationToken);
|
||||
// var targetAngle = currentAngle + angleOffsetDegrees;
|
||||
|
||||
// // Normalize angle to [-180, 180] range for better understanding (optional)
|
||||
// // But we don't restrict it here - let the absolute angle limits handle it
|
||||
// await RotateToAngleAsync(targetAngle, cancellationToken);
|
||||
// }
|
||||
|
||||
// public Task<double> GetCurrentAngleAsync(CancellationToken cancellationToken = default)
|
||||
// {
|
||||
// if (!_config.Enable)
|
||||
// {
|
||||
// return Task.FromResult(0.0);
|
||||
// }
|
||||
|
||||
// if (_servo == null)
|
||||
// throw new InvalidOperationException("Servo is not available");
|
||||
|
||||
// var currentPosition = _servo.GetActualPosition();
|
||||
// return Task.FromResult(PositionToAngle(currentPosition));
|
||||
// }
|
||||
|
||||
// public async Task<bool> IsHomedAsync(CancellationToken cancellationToken = default)
|
||||
// {
|
||||
// if (!_config.Enable)
|
||||
// {
|
||||
// return false;
|
||||
// }
|
||||
|
||||
// lock (_lock)
|
||||
// {
|
||||
// return _isHomed;
|
||||
// }
|
||||
// }
|
||||
|
||||
// #endregion
|
||||
|
||||
// #region IDisposable
|
||||
|
||||
// public void Dispose()
|
||||
// {
|
||||
// if (_disposed)
|
||||
// return;
|
||||
|
||||
// _disposed = true;
|
||||
|
||||
// try
|
||||
// {
|
||||
// _stateMachine.Stop();
|
||||
// }
|
||||
// catch (Exception ex)
|
||||
// {
|
||||
// _logger.LogError(ex, "Error disposing RotationModuleService");
|
||||
// }
|
||||
|
||||
// GC.SuppressFinalize(this);
|
||||
// }
|
||||
|
||||
// #endregion
|
||||
// }
|
||||
Reference in New Issue
Block a user