608 lines
22 KiB
C#
608 lines
22 KiB
C#
// 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
|
|
// }
|