Initial commit
This commit is contained in:
@@ -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
|
||||
}
|
||||
Reference in New Issue
Block a user