1681 lines
62 KiB
C#
1681 lines
62 KiB
C#
using RobotNet10.CANOpen;
|
|
using RobotNet10.CANOpen.CiA402;
|
|
using RobotNet10.CANOpen.CiA402.Enums;
|
|
using RobotNet10.CANOpen.CiA402.Models;
|
|
using RobotNet10.CANOpen.Enums;
|
|
using RobotNet10.CANOpen.Exceptions;
|
|
using RobotNet10.CANOpen.Models;
|
|
using RobotNet10.CANOpen.Services;
|
|
using RobotNet10.RobotApp.Client.Shared.Devices;
|
|
using RobotNet10.RobotApp.Devices;
|
|
using System.Collections.Concurrent;
|
|
|
|
namespace RobotNet10.RobotApp.Drivers.PhenikaaX;
|
|
|
|
/// <summary>
|
|
/// Cấu hình đầy đủ cho MBDV_DulAxes driver
|
|
/// </summary>
|
|
internal class MbdvDulAxesConfig
|
|
{
|
|
public string CanInterface { get; set; } = string.Empty;
|
|
public string NodeId { get; set; } = string.Empty;
|
|
public bool? AutoReconnectEnabled { get; set; }
|
|
public int? ReconnectDelayMs { get; set; }
|
|
public int? MaxReconnectAttempts { get; set; }
|
|
public int? PdoConfigRetryCount { get; set; }
|
|
public int? PdoConfigRetryTimeoutMs { get; set; }
|
|
public bool? UseHeartbeatCheck { get; set; }
|
|
public PdoMappingsConfig? PdoMappings { get; set; }
|
|
/// <summary>
|
|
/// Danh sách các homing method được phép sử dụng
|
|
/// Nếu null hoặc empty, cho phép tất cả methods
|
|
/// </summary>
|
|
public List<byte>? AllowedHomingMethods { get; set; }
|
|
}
|
|
|
|
/// <summary>
|
|
/// Cấu hình PDO Mappings
|
|
/// </summary>
|
|
internal class PdoMappingsConfig
|
|
{
|
|
public List<PdoMappingConfig>? RPDO1 { get; set; }
|
|
public List<PdoMappingConfig>? RPDO2 { get; set; }
|
|
public List<PdoMappingConfig>? RPDO3 { get; set; }
|
|
public List<PdoMappingConfig>? RPDO4 { get; set; }
|
|
public List<PdoMappingConfig>? TPDO1 { get; set; }
|
|
public List<PdoMappingConfig>? TPDO2 { get; set; }
|
|
public List<PdoMappingConfig>? TPDO3 { get; set; }
|
|
public List<PdoMappingConfig>? TPDO4 { get; set; }
|
|
}
|
|
|
|
/// <summary>
|
|
/// PDO Mapping Configuration từ config file
|
|
/// </summary>
|
|
internal class PdoMappingConfig
|
|
{
|
|
public string Index { get; set; } = string.Empty;
|
|
public byte SubIndex { get; set; }
|
|
public byte BitLength { get; set; }
|
|
}
|
|
|
|
/// <summary>
|
|
/// Thông tin mapping của một object vào PDO
|
|
/// </summary>
|
|
internal class PdoMappingInfo
|
|
{
|
|
public bool IsRPDO { get; set; }
|
|
public byte PdoNumber { get; set; }
|
|
public int BitOffset { get; set; }
|
|
public byte BitLength { get; set; }
|
|
public ushort ObjectIndex { get; set; }
|
|
public byte SubIndex { get; set; }
|
|
}
|
|
|
|
/// <summary>
|
|
/// Driver cho Phenikaa X servo motor theo chuẩn CiA402
|
|
/// Kế thừa DeviceBase và implement ICiA402Servo
|
|
/// Sử dụng tối đa TPDO và RPDO dựa trên EDS file
|
|
/// </summary>
|
|
[Device(DeviceType.CiA402Servo, "PhenikaaX", "CiA402Servo", "1.0.0", Description = "Phenikaa X CiA402 Servo Driver")]
|
|
public class CiA402Servo : DeviceBase, ICiA402Servo
|
|
{
|
|
private readonly ICanOpenManager? _canOpenManager;
|
|
private readonly string _canInterface;
|
|
private readonly byte _nodeId;
|
|
private readonly int _pdoConfigRetryCount;
|
|
private readonly int _pdoConfigRetryTimeoutMs;
|
|
private readonly bool _useHeartbeatCheck;
|
|
private readonly ILogger _logger;
|
|
private CanOpenDevice? _canOpenDevice;
|
|
private readonly CiA402ObjectDictionary _objectDictionary = CiA402ObjectDictionary.CreateDefault();
|
|
|
|
// PDO Mapping Configuration
|
|
private readonly Dictionary<string, List<PdoMappingConfig>> _pdoMappingsConfig = [];
|
|
|
|
// Mapping registry: object index -> PDO mapping info
|
|
// Key: (ushort index, byte subIndex), Value: PdoMappingInfo
|
|
private readonly ConcurrentDictionary<(ushort Index, byte SubIndex), PdoMappingInfo> _pdoMappingRegistry = new();
|
|
|
|
// Allowed homing methods (null hoặc empty = cho phép tất cả)
|
|
private readonly HashSet<byte>? _allowedHomingMethods;
|
|
|
|
// Thread-safe cached values
|
|
private readonly Lock _lock = new();
|
|
private Statusword _cachedStatusword;
|
|
private int _cachedPosition;
|
|
private int _cachedVelocity;
|
|
private short _cachedTorque;
|
|
private int _targetPosition;
|
|
|
|
// Cache for all mapped objects (key: object index, value: cached value as byte array)
|
|
private readonly ConcurrentDictionary<ushort, byte[]> _objectCache = new();
|
|
|
|
// Events
|
|
public event EventHandler<StatuswordChangedEventArgs>? StatuswordChanged;
|
|
public event EventHandler<PositionChangedEventArgs>? PositionChanged;
|
|
public event EventHandler<VelocityChangedEventArgs>? VelocityChanged;
|
|
|
|
/// <summary>
|
|
/// Constructor với IServiceProvider để lấy ICanOpenManager
|
|
/// </summary>
|
|
public CiA402Servo(string deviceId, string deviceName, IConfigurationSection connection, IServiceProvider serviceProvider)
|
|
: base(deviceId, deviceName, DeviceType.CiA402Servo)
|
|
{
|
|
// Bind tất cả cấu hình từ IConfigurationSection
|
|
var config = new MbdvDulAxesConfig();
|
|
connection.Bind(config);
|
|
|
|
// Validate required fields
|
|
if (string.IsNullOrWhiteSpace(config.CanInterface))
|
|
throw new InvalidOperationException("CanInterface is required in connection configuration");
|
|
|
|
if (string.IsNullOrWhiteSpace(config.NodeId))
|
|
throw new InvalidOperationException("NodeId is required in connection configuration");
|
|
|
|
_canInterface = config.CanInterface;
|
|
|
|
if (!byte.TryParse(config.NodeId, out _nodeId) || _nodeId == 0 || _nodeId > 127)
|
|
{
|
|
throw new InvalidOperationException($"Invalid NodeId: {config.NodeId}. Must be between 1 and 127");
|
|
}
|
|
|
|
// PDO Configuration retry settings
|
|
_pdoConfigRetryCount = config.PdoConfigRetryCount ?? 3;
|
|
_pdoConfigRetryTimeoutMs = config.PdoConfigRetryTimeoutMs ?? 1000;
|
|
|
|
// Heartbeat check option (default: true)
|
|
_useHeartbeatCheck = config.UseHeartbeatCheck ?? true;
|
|
|
|
// Cấu hình device
|
|
AutoReconnectEnabled = config.AutoReconnectEnabled ?? true;
|
|
ReconnectDelayMs = config.ReconnectDelayMs ?? 3000;
|
|
MaxReconnectAttempts = config.MaxReconnectAttempts ?? 0;
|
|
|
|
// Allowed homing methods
|
|
if (config.AllowedHomingMethods != null && config.AllowedHomingMethods.Count > 0)
|
|
{
|
|
_allowedHomingMethods = [.. config.AllowedHomingMethods];
|
|
}
|
|
|
|
// Khởi tạo giá trị properties
|
|
SetProperty("CanInterface", _canInterface);
|
|
SetProperty("NodeId", _nodeId.ToString());
|
|
SetProperty("UseHeartbeatCheck", _useHeartbeatCheck.ToString());
|
|
|
|
_canOpenManager = serviceProvider.GetRequiredService<ICanOpenManager>();
|
|
_logger = serviceProvider.GetRequiredService<ILoggerFactory>().CreateLogger<CiA402Servo>();;
|
|
|
|
_cachedStatusword = new Statusword(0);
|
|
|
|
// Parse PDO mapping configuration từ config object
|
|
ParsePdoMappingConfiguration(config.PdoMappings);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Parse PDO mapping configuration từ PdoMappingsConfig object
|
|
/// </summary>
|
|
private void ParsePdoMappingConfiguration(PdoMappingsConfig? pdoMappings)
|
|
{
|
|
if (pdoMappings == null)
|
|
{
|
|
// Nếu không có config, sử dụng default mappings
|
|
return;
|
|
}
|
|
|
|
// Parse RPDO1-4
|
|
if (pdoMappings.RPDO1 != null && pdoMappings.RPDO1.Count > 0)
|
|
{
|
|
_pdoMappingsConfig["RPDO1"] = pdoMappings.RPDO1;
|
|
}
|
|
|
|
if (pdoMappings.RPDO2 != null && pdoMappings.RPDO2.Count > 0)
|
|
{
|
|
_pdoMappingsConfig["RPDO2"] = pdoMappings.RPDO2;
|
|
}
|
|
|
|
if (pdoMappings.RPDO3 != null && pdoMappings.RPDO3.Count > 0)
|
|
{
|
|
_pdoMappingsConfig["RPDO3"] = pdoMappings.RPDO3;
|
|
}
|
|
|
|
if (pdoMappings.RPDO4 != null && pdoMappings.RPDO4.Count > 0)
|
|
{
|
|
_pdoMappingsConfig["RPDO4"] = pdoMappings.RPDO4;
|
|
}
|
|
|
|
// Parse TPDO1-4
|
|
if (pdoMappings.TPDO1 != null && pdoMappings.TPDO1.Count > 0)
|
|
{
|
|
_pdoMappingsConfig["TPDO1"] = pdoMappings.TPDO1;
|
|
}
|
|
|
|
if (pdoMappings.TPDO2 != null && pdoMappings.TPDO2.Count > 0)
|
|
{
|
|
_pdoMappingsConfig["TPDO2"] = pdoMappings.TPDO2;
|
|
}
|
|
|
|
if (pdoMappings.TPDO3 != null && pdoMappings.TPDO3.Count > 0)
|
|
{
|
|
_pdoMappingsConfig["TPDO3"] = pdoMappings.TPDO3;
|
|
}
|
|
|
|
if (pdoMappings.TPDO4 != null && pdoMappings.TPDO4.Count > 0)
|
|
{
|
|
_pdoMappingsConfig["TPDO4"] = pdoMappings.TPDO4;
|
|
}
|
|
}
|
|
|
|
protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
|
|
{
|
|
yield return new PropertyDescription("CanInterface", "CAN Interface", "Tên CAN interface (ví dụ: can0, can1)")
|
|
{
|
|
DataType = "string",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 1,
|
|
Category = "Kết nối",
|
|
DefaultValue = ""
|
|
};
|
|
|
|
yield return new PropertyDescription("NodeId", "Node ID", "CANOpen Node ID (1-127)")
|
|
{
|
|
DataType = "number",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 2,
|
|
Category = "Kết nối",
|
|
DefaultValue = "1"
|
|
};
|
|
|
|
yield return new PropertyDescription("UseHeartbeatCheck", "Use Heartbeat Check", "Bật/tắt kiểm tra heartbeat")
|
|
{
|
|
DataType = "boolean",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 4,
|
|
Category = "Cấu hình",
|
|
DefaultValue = "true"
|
|
};
|
|
|
|
yield return new PropertyDescription("Statusword", "Statusword", "Trạng thái servo (hex)")
|
|
{
|
|
DataType = "string",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 5,
|
|
Category = "Trạng thái",
|
|
DefaultValue = "0x0000"
|
|
};
|
|
|
|
yield return new PropertyDescription("Position", "Vị trí", "Vị trí hiện tại (encoder counts)")
|
|
{
|
|
DataType = "number",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 6,
|
|
Category = "Trạng thái",
|
|
DefaultValue = "0"
|
|
};
|
|
|
|
yield return new PropertyDescription("Velocity", "Vận tốc", "Vận tốc hiện tại (encoder counts/s)")
|
|
{
|
|
DataType = "number",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 7,
|
|
Category = "Trạng thái",
|
|
DefaultValue = "0"
|
|
};
|
|
|
|
yield return new PropertyDescription("Torque", "Torque", "Torque hiện tại")
|
|
{
|
|
DataType = "number",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 8,
|
|
Category = "Trạng thái",
|
|
DefaultValue = "0"
|
|
};
|
|
|
|
yield return new PropertyDescription("DriveState", "Drive State", "Trạng thái drive")
|
|
{
|
|
DataType = "string",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 9,
|
|
Category = "Trạng thái",
|
|
DefaultValue = "Unknown"
|
|
};
|
|
}
|
|
|
|
protected override async Task OnInitializeAsync(CancellationToken cancellationToken)
|
|
{
|
|
if (_canOpenManager == null)
|
|
{
|
|
throw new InvalidOperationException("ICanOpenManager service is required. Make sure to register it in DI container.");
|
|
}
|
|
// Lấy hoặc tạo CanOpenDevice
|
|
_canOpenDevice = await _canOpenManager.GetOrCreateDeviceAsync(_canInterface, _nodeId, cancellationToken);
|
|
|
|
// Subscribe to PDO events
|
|
_canOpenDevice.PdoReceived += OnPdoReceived;
|
|
|
|
// Configure PDOs (master-side)
|
|
ConfigurePdos();
|
|
UpdateProperties();
|
|
}
|
|
|
|
protected override async Task OnConnectAsync(CancellationToken cancellationToken)
|
|
{
|
|
if (_canOpenDevice == null)
|
|
throw new InvalidOperationException("Device not initialized. Call InitializeAsync first.");
|
|
|
|
// Kiểm tra kết nối ICanBus
|
|
if (_canOpenManager != null)
|
|
{
|
|
var canBus = _canOpenManager.GetCanBus(_canInterface);
|
|
if (canBus == null || !canBus.IsConnected)
|
|
{
|
|
throw new InvalidOperationException($"ICanBus for interface '{_canInterface}' is not connected.");
|
|
}
|
|
}
|
|
|
|
// Enable heartbeat monitoring
|
|
if (_useHeartbeatCheck)
|
|
{
|
|
_canOpenDevice.AutoVerifyStateViaHeartbeat = true;
|
|
_canOpenDevice.EnableHeartbeatMonitoring(timeoutMs: 2000);
|
|
}
|
|
|
|
// Configure PDOs trên device (ghi xuống device qua SDO)
|
|
await ConfigureDevicePdosAsync(cancellationToken);
|
|
|
|
// Đảm bảo device ở PreOperational state trước khi start node
|
|
var currentState = _canOpenDevice.State;
|
|
if (currentState != NmtState.PreOperational && currentState != NmtState.Stopped)
|
|
{
|
|
await _canOpenDevice.SendNmtCommandAsync(NmtCommand.PreOperational, cancellationToken);
|
|
await Task.Delay(100, cancellationToken);
|
|
}
|
|
|
|
// Start node (chuyển sang Operational state)
|
|
await _canOpenDevice.StartNodeAsync(cancellationToken);
|
|
|
|
UpdateProperties();
|
|
}
|
|
|
|
protected override async Task OnDisconnectAsync(CancellationToken cancellationToken)
|
|
{
|
|
if (_canOpenDevice != null)
|
|
{
|
|
await _canOpenDevice.StopNodeAsync(cancellationToken);
|
|
_canOpenDevice.DisableHeartbeatMonitoring();
|
|
}
|
|
}
|
|
|
|
protected override async Task OnResetAsync(CancellationToken cancellationToken)
|
|
{
|
|
// Reset về trạng thái ban đầu
|
|
try
|
|
{
|
|
await DisableAsync(cancellationToken);
|
|
await Task.Delay(100, cancellationToken);
|
|
|
|
var state = await GetStateAsync(cancellationToken);
|
|
if (state == DriveState.Fault)
|
|
{
|
|
await FaultResetAsync(cancellationToken);
|
|
await Task.Delay(100, cancellationToken);
|
|
}
|
|
|
|
await ShutdownAsync(cancellationToken);
|
|
await Task.Delay(100, cancellationToken);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Log errors during reset nhưng không throw để không block reset process
|
|
_logger.LogWarning(ex, "Error occurred during reset for Node {NodeId}", _nodeId);
|
|
}
|
|
|
|
UpdateProperties();
|
|
}
|
|
|
|
protected override async Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
|
|
{
|
|
if (_canOpenDevice == null)
|
|
return false;
|
|
|
|
try
|
|
{
|
|
// Check heartbeat
|
|
if (_useHeartbeatCheck)
|
|
{
|
|
var nodeInfo = _canOpenDevice.Heartbeat.GetNodeInfo(_nodeId);
|
|
if (nodeInfo == null || !nodeInfo.IsAlive)
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// Try to read statusword
|
|
await GetStatuswordAsync(cancellationToken);
|
|
return true;
|
|
}
|
|
catch(Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error checking connection for Node {NodeId}", _nodeId);
|
|
return false;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Configure PDOs trên device (ghi xuống device qua SDO)
|
|
/// Sử dụng tối đa TPDO và RPDO dựa trên EDS file
|
|
/// </summary>
|
|
private async Task ConfigureDevicePdosAsync(CancellationToken ct)
|
|
{
|
|
// Retry logic với state verification
|
|
Exception? lastException = null;
|
|
for (int attempt = 1; attempt <= _pdoConfigRetryCount; attempt++)
|
|
{
|
|
try
|
|
{
|
|
// Verify và đảm bảo device ở PreOperational state
|
|
await EnsurePreOperationalStateAsync(ct);
|
|
|
|
// Ghi tất cả PDO configurations xuống device
|
|
await _canOpenDevice!.WriteAllPdoConfigurationsToDeviceAsync(null, ct);
|
|
|
|
return;
|
|
}
|
|
catch (CanOpenTimeoutException timeoutEx)
|
|
{
|
|
lastException = timeoutEx;
|
|
|
|
_logger.LogWarning("PDO configuration attempt {Attempt}/{MaxRetries} failed for Node {NodeId}: {Error}",
|
|
attempt, _pdoConfigRetryCount, _nodeId, timeoutEx.Message);
|
|
|
|
if (attempt < _pdoConfigRetryCount)
|
|
{
|
|
int delayMs = _pdoConfigRetryTimeoutMs * attempt;
|
|
await Task.Delay(delayMs, ct);
|
|
|
|
try
|
|
{
|
|
await _canOpenDevice!.SendNmtCommandAsync(NmtCommand.PreOperational, ct);
|
|
await Task.Delay(200, ct);
|
|
}
|
|
catch
|
|
{
|
|
// Ignore errors
|
|
}
|
|
}
|
|
}
|
|
catch (Exception)
|
|
{
|
|
throw;
|
|
}
|
|
}
|
|
|
|
throw new InvalidOperationException(
|
|
$"Failed to configure PDOs after {_pdoConfigRetryCount} attempts for Node {_nodeId}. " +
|
|
$"Last error: {lastException?.Message}", lastException);
|
|
}
|
|
|
|
private async Task EnsurePreOperationalStateAsync(CancellationToken ct)
|
|
{
|
|
var currentState = _canOpenDevice!.State;
|
|
if (currentState != NmtState.PreOperational)
|
|
{
|
|
await _canOpenDevice.SendNmtCommandAsync(NmtCommand.PreOperational, ct);
|
|
await Task.Delay(100, ct);
|
|
}
|
|
|
|
if (_canOpenDevice is CanOpenDevice canOpenDevice)
|
|
{
|
|
try
|
|
{
|
|
var verified = await canOpenDevice.VerifyStateViaHeartbeatAsync(timeoutMs: 1000, ct);
|
|
if (verified)
|
|
{
|
|
var heartbeatState = canOpenDevice.Heartbeat.GetNodeInfo(_nodeId)?.LastState;
|
|
if (heartbeatState.HasValue && heartbeatState.Value != NmtState.PreOperational)
|
|
{
|
|
await _canOpenDevice.SendNmtCommandAsync(NmtCommand.PreOperational, ct);
|
|
await Task.Delay(200, ct);
|
|
}
|
|
}
|
|
}
|
|
catch
|
|
{
|
|
// Ignore
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Configure PDOs cho Phenikaa X - sử dụng configuration từ IConfigurationSection
|
|
/// Nếu không có config, sử dụng default mappings
|
|
/// </summary>
|
|
private void ConfigurePdos()
|
|
{
|
|
if (_objectDictionary == null || _canOpenDevice == null)
|
|
return;
|
|
|
|
// Clear mapping registry trước khi configure lại
|
|
_pdoMappingRegistry.Clear();
|
|
_objectCache.Clear();
|
|
|
|
// Configure RPDO1-4 từ configuration
|
|
for (byte pdoNum = 1; pdoNum <= 4; pdoNum++)
|
|
{
|
|
var pdoKey = $"RPDO{pdoNum}";
|
|
if (_pdoMappingsConfig.TryGetValue(pdoKey, out var mappings) && mappings.Count > 0)
|
|
{
|
|
ConfigureRPDO(pdoNum, mappings);
|
|
}
|
|
else if (pdoNum == 1)
|
|
{
|
|
// Default RPDO1: Controlword
|
|
var defaultMappings = new List<PdoMappingConfig>
|
|
{
|
|
new() { Index = "0x6040", SubIndex = 0, BitLength = 16 }
|
|
};
|
|
ConfigureRPDO(pdoNum, defaultMappings);
|
|
}
|
|
else if (pdoNum == 2)
|
|
{
|
|
// Default RPDO2: TargetPosition
|
|
var defaultMappings = new List<PdoMappingConfig>
|
|
{
|
|
new() { Index = "0x607A", SubIndex = 0, BitLength = 32 }
|
|
};
|
|
ConfigureRPDO(pdoNum, defaultMappings);
|
|
}
|
|
else if (pdoNum == 3)
|
|
{
|
|
// Default RPDO3: TargetVelocity
|
|
var defaultMappings = new List<PdoMappingConfig>
|
|
{
|
|
new() { Index = "0x60FF", SubIndex = 0, BitLength = 32 }
|
|
};
|
|
ConfigureRPDO(pdoNum, defaultMappings);
|
|
}
|
|
else if (pdoNum == 4)
|
|
{
|
|
// Default RPDO4: TargetTorque
|
|
var defaultMappings = new List<PdoMappingConfig>
|
|
{
|
|
new() { Index = "0x6071", SubIndex = 0, BitLength = 16 }
|
|
};
|
|
ConfigureRPDO(pdoNum, defaultMappings);
|
|
}
|
|
}
|
|
|
|
// Configure TPDO1-4 từ configuration
|
|
for (byte pdoNum = 1; pdoNum <= 4; pdoNum++)
|
|
{
|
|
var pdoKey = $"TPDO{pdoNum}";
|
|
if (_pdoMappingsConfig.TryGetValue(pdoKey, out var mappings) && mappings.Count > 0)
|
|
{
|
|
ConfigureTPDO(pdoNum, mappings);
|
|
}
|
|
else if (pdoNum == 1)
|
|
{
|
|
// Default TPDO1: Statusword + PositionActualValue
|
|
var defaultMappings = new List<PdoMappingConfig>
|
|
{
|
|
new() { Index = "0x6041", SubIndex = 0, BitLength = 16 },
|
|
new() { Index = "0x6064", SubIndex = 0, BitLength = 32 }
|
|
};
|
|
ConfigureTPDO(pdoNum, defaultMappings);
|
|
}
|
|
else if (pdoNum == 2)
|
|
{
|
|
// Default TPDO2: VelocityActualValue
|
|
var defaultMappings = new List<PdoMappingConfig>
|
|
{
|
|
new() { Index = "0x606C", SubIndex = 0, BitLength = 32 }
|
|
};
|
|
ConfigureTPDO(pdoNum, defaultMappings);
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Configure một RPDO từ danh sách mappings
|
|
/// </summary>
|
|
private void ConfigureRPDO(byte pdoNumber, List<PdoMappingConfig> mappings)
|
|
{
|
|
if (_canOpenDevice == null) return;
|
|
|
|
var rpdo = new PdoConfiguration(pdoNumber, (uint)(0x200 + (pdoNumber - 1) * 0x100 + _nodeId));
|
|
int bitOffset = 0;
|
|
|
|
foreach (var mappingConfig in mappings)
|
|
{
|
|
if (!CiA402Helper.TryParseObjectIndex(mappingConfig.Index, out ushort objectIndex))
|
|
{
|
|
_logger.LogWarning("Invalid object index in RPDO{Number} mapping: {Index}", pdoNumber, mappingConfig.Index);
|
|
continue;
|
|
}
|
|
|
|
// Validate bitLength
|
|
if (mappingConfig.BitLength == 0 || mappingConfig.BitLength > 64)
|
|
{
|
|
_logger.LogWarning("Invalid bitLength ({BitLength}) in RPDO{Number} mapping for object 0x{Index:X4}. Must be between 1 and 64",
|
|
mappingConfig.BitLength, pdoNumber, objectIndex);
|
|
continue;
|
|
}
|
|
|
|
var mapping = new PdoMapping(objectIndex, mappingConfig.SubIndex, mappingConfig.BitLength);
|
|
rpdo.AddMapping(mapping);
|
|
|
|
// Register mapping vào registry
|
|
var key = (objectIndex, mappingConfig.SubIndex);
|
|
_pdoMappingRegistry[key] = new PdoMappingInfo
|
|
{
|
|
IsRPDO = true,
|
|
PdoNumber = pdoNumber,
|
|
BitOffset = bitOffset,
|
|
BitLength = mappingConfig.BitLength,
|
|
ObjectIndex = objectIndex,
|
|
SubIndex = mappingConfig.SubIndex
|
|
};
|
|
|
|
bitOffset += mappingConfig.BitLength;
|
|
}
|
|
|
|
if (rpdo.Mappings.Count > 0)
|
|
{
|
|
_canOpenDevice.Pdo.ConfigureRPDO(rpdo);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Configure một TPDO từ danh sách mappings
|
|
/// </summary>
|
|
private void ConfigureTPDO(byte pdoNumber, List<PdoMappingConfig> mappings)
|
|
{
|
|
if (_canOpenDevice == null) return;
|
|
|
|
var tpdo = new PdoConfiguration(pdoNumber, (uint)(0x180 + (pdoNumber - 1) * 0x100 + _nodeId))
|
|
{
|
|
EventTimer = 50 // Default event timer
|
|
};
|
|
int bitOffset = 0;
|
|
|
|
foreach (var mappingConfig in mappings)
|
|
{
|
|
if (!CiA402Helper.TryParseObjectIndex(mappingConfig.Index, out ushort objectIndex))
|
|
{
|
|
_logger.LogWarning("Invalid object index in TPDO{Number} mapping: {Index}", pdoNumber, mappingConfig.Index);
|
|
continue;
|
|
}
|
|
|
|
// Validate bitLength
|
|
if (mappingConfig.BitLength == 0 || mappingConfig.BitLength > 64)
|
|
{
|
|
_logger.LogWarning("Invalid bitLength ({BitLength}) in TPDO{Number} mapping for object 0x{Index:X4}. Must be between 1 and 64",
|
|
mappingConfig.BitLength, pdoNumber, objectIndex);
|
|
continue;
|
|
}
|
|
|
|
var mapping = new PdoMapping(objectIndex, mappingConfig.SubIndex, mappingConfig.BitLength);
|
|
tpdo.AddMapping(mapping);
|
|
|
|
// Register mapping vào registry
|
|
var key = (objectIndex, mappingConfig.SubIndex);
|
|
_pdoMappingRegistry[key] = new PdoMappingInfo
|
|
{
|
|
IsRPDO = false,
|
|
PdoNumber = pdoNumber,
|
|
BitOffset = bitOffset,
|
|
BitLength = mappingConfig.BitLength,
|
|
ObjectIndex = objectIndex,
|
|
SubIndex = mappingConfig.SubIndex
|
|
};
|
|
|
|
bitOffset += mappingConfig.BitLength;
|
|
}
|
|
|
|
if (tpdo.Mappings.Count > 0)
|
|
{
|
|
_canOpenDevice.Pdo.ConfigureTPDO(tpdo);
|
|
}
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// Xử lý PDO data nhận được - parse động dựa trên configuration
|
|
/// </summary>
|
|
private void OnPdoReceived(object? sender, PdoReceivedEventArgs e)
|
|
{
|
|
var data = e.Data;
|
|
|
|
// Tìm tất cả mappings cho TPDO này
|
|
var tpdoMappings = _pdoMappingRegistry.Values
|
|
.Where(m => !m.IsRPDO && m.PdoNumber == data.PdoNumber)
|
|
.OrderBy(m => m.BitOffset)
|
|
.ToList();
|
|
|
|
if (tpdoMappings.Count == 0)
|
|
{
|
|
UpdateProperties();
|
|
return;
|
|
}
|
|
|
|
// Tính tổng số bytes cần thiết
|
|
int totalBits = tpdoMappings.Sum(m => m.BitLength);
|
|
int totalBytes = (totalBits + 7) / 8; // Round up
|
|
|
|
if (data.Data.Length < totalBytes)
|
|
{
|
|
_logger.LogWarning("TPDO{Number} data length ({Length}) is less than expected ({Expected})",
|
|
data.PdoNumber, data.Data.Length, totalBytes);
|
|
UpdateProperties();
|
|
return;
|
|
}
|
|
|
|
lock (_lock)
|
|
{
|
|
// Parse từng mapping trong TPDO
|
|
foreach (var mappingInfo in tpdoMappings)
|
|
{
|
|
ProcessTpdoMapping(data.Data, mappingInfo);
|
|
}
|
|
}
|
|
|
|
UpdateProperties();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Xử lý một mapping trong TPDO data và cập nhật cache
|
|
/// </summary>
|
|
private void ProcessTpdoMapping(byte[] pdoData, PdoMappingInfo mappingInfo)
|
|
{
|
|
try
|
|
{
|
|
// Extract value từ PDO data
|
|
byte[] valueBytes = CiA402Helper.ExtractValueFromPdoData(pdoData, mappingInfo.BitOffset, mappingInfo.BitLength);
|
|
|
|
// Cache giá trị
|
|
_objectCache[mappingInfo.ObjectIndex] = valueBytes;
|
|
|
|
// Xử lý các objects đặc biệt (Statusword, Position, Velocity, Torque)
|
|
switch (mappingInfo.ObjectIndex)
|
|
{
|
|
case 0x6041: // Statusword
|
|
if (mappingInfo.BitLength == 16 && valueBytes.Length >= 2)
|
|
{
|
|
var statuswordValue = BitConverter.ToUInt16(valueBytes, 0);
|
|
var newStatusword = new Statusword(statuswordValue);
|
|
if (newStatusword.Value != _cachedStatusword.Value)
|
|
{
|
|
var oldState = _cachedStatusword.GetState();
|
|
var newState = newStatusword.GetState();
|
|
_cachedStatusword = newStatusword;
|
|
|
|
StatuswordChanged?.Invoke(this, new StatuswordChangedEventArgs(newStatusword, oldState, newState));
|
|
}
|
|
}
|
|
break;
|
|
|
|
case 0x6064: // PositionActualValue
|
|
if (mappingInfo.BitLength == 32 && valueBytes.Length >= 4)
|
|
{
|
|
var position = BitConverter.ToInt32(valueBytes, 0);
|
|
if (position != _cachedPosition)
|
|
{
|
|
var oldPosition = _cachedPosition;
|
|
_cachedPosition = position;
|
|
PositionChanged?.Invoke(this, new PositionChangedEventArgs(position, oldPosition));
|
|
}
|
|
}
|
|
break;
|
|
|
|
case 0x606C: // VelocityActualValue
|
|
if (mappingInfo.BitLength == 32 && valueBytes.Length >= 4)
|
|
{
|
|
var velocity = BitConverter.ToInt32(valueBytes, 0);
|
|
if (velocity != _cachedVelocity)
|
|
{
|
|
var oldVelocity = _cachedVelocity;
|
|
_cachedVelocity = velocity;
|
|
VelocityChanged?.Invoke(this, new VelocityChangedEventArgs(velocity, oldVelocity));
|
|
}
|
|
}
|
|
break;
|
|
|
|
case 0x6077: // TorqueActualValue
|
|
if (mappingInfo.BitLength == 16 && valueBytes.Length >= 2)
|
|
{
|
|
var torque = (short)BitConverter.ToUInt16(valueBytes, 0);
|
|
_cachedTorque = torque;
|
|
}
|
|
break;
|
|
|
|
default:
|
|
// Các objects khác chỉ cache, không trigger events
|
|
_logger.LogTrace("Cached object 0x{Index:X4}:{SubIndex} from TPDO{Number}",
|
|
mappingInfo.ObjectIndex, mappingInfo.SubIndex, mappingInfo.PdoNumber);
|
|
break;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error processing TPDO{Number} mapping for object 0x{Index:X4}:{SubIndex}",
|
|
mappingInfo.PdoNumber, mappingInfo.ObjectIndex, mappingInfo.SubIndex);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Đọc giá trị từ object - tự động chọn PDO nếu mapped, fallback SDO
|
|
/// </summary>
|
|
private async Task<T> ReadObjectAsync<T>(ushort index, byte subIndex, CancellationToken ct) where T : struct
|
|
{
|
|
if (_canOpenDevice == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
|
|
// Kiểm tra xem object có mapped vào TPDO không
|
|
var key = (index, subIndex);
|
|
if (_pdoMappingRegistry.TryGetValue(key, out var mappingInfo) && !mappingInfo.IsRPDO)
|
|
{
|
|
// Đọc từ cache nếu có
|
|
if (_objectCache.TryGetValue(index, out var cachedValue))
|
|
{
|
|
return CiA402Helper.ConvertBytesToValue<T>(cachedValue, mappingInfo.BitLength);
|
|
}
|
|
}
|
|
|
|
// Fallback to SDO read
|
|
return await ReadObjectViaSdoAsync<T>(index, subIndex, ct);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Ghi giá trị vào object - tự động chọn RPDO nếu mapped, fallback SDO
|
|
/// </summary>
|
|
private async Task WriteObjectAsync<T>(ushort index, byte subIndex, T value, CancellationToken ct) where T : struct
|
|
{
|
|
if (_canOpenDevice == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
|
|
// Kiểm tra xem object có mapped vào RPDO không
|
|
var key = (index, subIndex);
|
|
if (_pdoMappingRegistry.TryGetValue(key, out var mappingInfo) && mappingInfo.IsRPDO)
|
|
{
|
|
// Ghi qua RPDO
|
|
await WriteObjectViaRpdoAsync(index, subIndex, value, mappingInfo, ct);
|
|
return;
|
|
}
|
|
|
|
// Fallback to SDO write
|
|
await WriteObjectViaSdoAsync(index, subIndex, value, ct);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Đọc object qua SDO
|
|
/// </summary>
|
|
private async Task<T> ReadObjectViaSdoAsync<T>(ushort index, byte subIndex, CancellationToken ct) where T : struct
|
|
{
|
|
if (_canOpenDevice == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
|
|
var type = typeof(T);
|
|
if (type == typeof(ushort))
|
|
{
|
|
var value = await _canOpenDevice.ReadUInt16Async(index, subIndex, ct);
|
|
return (T)(object)value;
|
|
}
|
|
else if (type == typeof(short))
|
|
{
|
|
var value = await _canOpenDevice.ReadInt16Async(index, subIndex, ct);
|
|
return (T)(object)value;
|
|
}
|
|
else if (type == typeof(uint))
|
|
{
|
|
var value = await _canOpenDevice.ReadUInt32Async(index, subIndex, ct);
|
|
return (T)(object)value;
|
|
}
|
|
else if (type == typeof(int))
|
|
{
|
|
var value = await _canOpenDevice.ReadInt32Async(index, subIndex, ct);
|
|
return (T)(object)value;
|
|
}
|
|
else if (type == typeof(byte))
|
|
{
|
|
var value = await _canOpenDevice.ReadUInt8Async(index, subIndex, ct);
|
|
return (T)(object)value;
|
|
}
|
|
else if (type == typeof(sbyte))
|
|
{
|
|
// Read as byte and cast to sbyte
|
|
var value = await _canOpenDevice.ReadUInt8Async(index, subIndex, ct);
|
|
return (T)(object)(sbyte)value;
|
|
}
|
|
|
|
throw new NotSupportedException($"Type {type.Name} is not supported for SDO read");
|
|
}
|
|
|
|
/// <summary>
|
|
/// Ghi object qua SDO
|
|
/// </summary>
|
|
private async Task WriteObjectViaSdoAsync<T>(ushort index, byte subIndex, T value, CancellationToken ct) where T : struct
|
|
{
|
|
if (_canOpenDevice == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
|
|
var type = typeof(T);
|
|
if (type == typeof(ushort))
|
|
{
|
|
await _canOpenDevice.WriteUInt16Async(index, subIndex, (ushort)(object)value!, ct);
|
|
}
|
|
else if (type == typeof(short))
|
|
{
|
|
await _canOpenDevice.WriteInt16Async(index, subIndex, (short)(object)value!, ct);
|
|
}
|
|
else if (type == typeof(uint))
|
|
{
|
|
await _canOpenDevice.WriteUInt32Async(index, subIndex, (uint)(object)value!, ct);
|
|
}
|
|
else if (type == typeof(int))
|
|
{
|
|
await _canOpenDevice.WriteInt32Async(index, subIndex, (int)(object)value!, ct);
|
|
}
|
|
else if (type == typeof(byte))
|
|
{
|
|
await _canOpenDevice.WriteUInt8Async(index, subIndex, (byte)(object)value!, ct);
|
|
}
|
|
else if (type == typeof(sbyte))
|
|
{
|
|
// Write sbyte as byte
|
|
await _canOpenDevice.WriteUInt8Async(index, subIndex, (byte)(sbyte)(object)value!, ct);
|
|
}
|
|
else
|
|
{
|
|
throw new NotSupportedException($"Type {type.Name} is not supported for SDO write");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Ghi object qua RPDO
|
|
/// </summary>
|
|
private async Task WriteObjectViaRpdoAsync<T>(ushort index, byte subIndex, T value, PdoMappingInfo mappingInfo, CancellationToken ct) where T : struct
|
|
{
|
|
if (_canOpenDevice == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
|
|
// Convert value to bytes
|
|
byte[] valueBytes = CiA402Helper.ConvertValueToBytes(value, mappingInfo.BitLength);
|
|
|
|
// Nếu RPDO chỉ có một mapping, gửi trực tiếp
|
|
// Nếu có nhiều mappings, cần đọc các giá trị khác và gộp lại
|
|
// Để đơn giản, chỉ gửi nếu RPDO chỉ có mapping này
|
|
var rpdoMappings = _pdoMappingRegistry.Values
|
|
.Where(m => m.IsRPDO && m.PdoNumber == mappingInfo.PdoNumber)
|
|
.OrderBy(m => m.BitOffset)
|
|
.ToList();
|
|
|
|
if (rpdoMappings.Count == 1)
|
|
{
|
|
// Single mapping - send directly
|
|
await _canOpenDevice.Pdo.SendRPDOAsync(mappingInfo.PdoNumber, valueBytes, ct);
|
|
}
|
|
else
|
|
{
|
|
// Multiple mappings - need to construct full PDO data
|
|
// For now, fallback to SDO
|
|
await WriteObjectViaSdoAsync(index, subIndex, value, ct);
|
|
}
|
|
}
|
|
|
|
private void UpdateProperties()
|
|
{
|
|
try
|
|
{
|
|
lock (_lock)
|
|
{
|
|
SetProperty("Statusword", $"0x{_cachedStatusword.Value:X4}");
|
|
SetProperty("Position", _cachedPosition.ToString());
|
|
SetProperty("Velocity", _cachedVelocity.ToString());
|
|
SetProperty("Torque", _cachedTorque.ToString());
|
|
|
|
var state = _cachedStatusword.GetState();
|
|
SetProperty("DriveState", state.ToString());
|
|
}
|
|
}
|
|
catch
|
|
{
|
|
// Ignore errors
|
|
}
|
|
}
|
|
|
|
#region ICiA402Servo Implementation
|
|
|
|
// Cached values
|
|
public Statusword CachedStatusword
|
|
{
|
|
get { lock (_lock) { return _cachedStatusword; } }
|
|
}
|
|
|
|
public int CachedPosition
|
|
{
|
|
get { lock (_lock) { return _cachedPosition; } }
|
|
}
|
|
|
|
public int CachedVelocity
|
|
{
|
|
get { lock (_lock) { return _cachedVelocity; } }
|
|
}
|
|
|
|
public short CachedTorque
|
|
{
|
|
get { lock (_lock) { return _cachedTorque; } }
|
|
}
|
|
|
|
// Statusword & Controlword
|
|
public async Task<Statusword> GetStatuswordAsync(CancellationToken ct = default)
|
|
{
|
|
if (_canOpenDevice == null || _objectDictionary == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
|
|
// Try cached value first (từ TPDO)
|
|
// Kiểm tra xem có mapped vào TPDO không để quyết định có dùng cache không
|
|
var key = (_objectDictionary.Statusword, (byte)0);
|
|
bool isMappedToTpdo = _pdoMappingRegistry.TryGetValue(key, out var mappingInfo) && !mappingInfo.IsRPDO;
|
|
|
|
if (isMappedToTpdo)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
// Nếu mapped vào TPDO, luôn dùng cache (kể cả khi = 0)
|
|
return _cachedStatusword;
|
|
}
|
|
}
|
|
|
|
// Đọc qua PDO/SDO tự động
|
|
var value = await ReadObjectAsync<ushort>(_objectDictionary.Statusword, 0, ct);
|
|
lock (_lock)
|
|
{
|
|
_cachedStatusword = new Statusword(value);
|
|
return _cachedStatusword;
|
|
}
|
|
}
|
|
|
|
public async Task SetControlwordAsync(Controlword controlword, CancellationToken ct = default)
|
|
{
|
|
if (_canOpenDevice == null || _objectDictionary == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
|
|
// Ghi qua RPDO/SDO tự động
|
|
await WriteObjectAsync(_objectDictionary.Controlword, 0, controlword.Value, ct);
|
|
}
|
|
|
|
public async Task<DriveState> GetStateAsync(CancellationToken ct = default)
|
|
{
|
|
var statusword = await GetStatuswordAsync(ct);
|
|
return statusword.GetState();
|
|
}
|
|
|
|
// Operation Mode
|
|
public async Task SetOperationModeAsync(OperationMode mode, CancellationToken ct = default)
|
|
{
|
|
if (_canOpenDevice == null || _objectDictionary == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
|
|
await _canOpenDevice.WriteObjectAsync(_objectDictionary.ModesOfOperation, 0, [(byte)mode], ct);
|
|
}
|
|
|
|
public async Task<OperationMode> GetOperationModeAsync(CancellationToken ct = default)
|
|
{
|
|
if (_canOpenDevice == null || _objectDictionary == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
|
|
var value = await _canOpenDevice.ReadUInt8Async(_objectDictionary.ModesOfOperationDisplay, 0, ct);
|
|
return (OperationMode)(sbyte)value;
|
|
}
|
|
|
|
// State Machine Control
|
|
public async Task FaultResetAsync(CancellationToken ct = default)
|
|
{
|
|
await SetControlwordAsync(Controlword.FaultResetCmd, ct);
|
|
await Task.Delay(100, ct);
|
|
await SetControlwordAsync(new Controlword(0x0000), ct);
|
|
}
|
|
|
|
public async Task ShutdownAsync(CancellationToken ct = default)
|
|
{
|
|
await SetControlwordAsync(Controlword.Shutdown, ct);
|
|
}
|
|
|
|
public async Task SwitchOnAsync(CancellationToken ct = default)
|
|
{
|
|
await SetControlwordAsync(Controlword.SwitchOnCmd, ct);
|
|
}
|
|
|
|
public async Task EnableOperationAsync(CancellationToken ct = default)
|
|
{
|
|
await SetControlwordAsync(Controlword.EnableOperationCmd, ct);
|
|
}
|
|
|
|
public async Task DisableOperationAsync(CancellationToken ct = default)
|
|
{
|
|
await SetControlwordAsync(Controlword.DisableOperation, ct);
|
|
}
|
|
|
|
public async Task QuickStopAsync(CancellationToken ct = default)
|
|
{
|
|
await SetControlwordAsync(Controlword.QuickStopCmd, ct);
|
|
}
|
|
|
|
public async Task EnableAsync(CancellationToken ct = default)
|
|
{
|
|
var state = await GetStateAsync(ct);
|
|
|
|
if (state == DriveState.Fault)
|
|
{
|
|
await FaultResetAsync(ct);
|
|
await Task.Delay(200, ct);
|
|
state = await GetStateAsync(ct);
|
|
}
|
|
|
|
if (state == DriveState.SwitchOnDisabled || state == DriveState.NotReadyToSwitchOn)
|
|
{
|
|
await ShutdownAsync(ct);
|
|
await Task.Delay(100, ct);
|
|
state = await GetStateAsync(ct);
|
|
}
|
|
|
|
if (state == DriveState.ReadyToSwitchOn)
|
|
{
|
|
await SwitchOnAsync(ct);
|
|
await Task.Delay(100, ct);
|
|
state = await GetStateAsync(ct);
|
|
}
|
|
|
|
if (state == DriveState.SwitchedOn)
|
|
{
|
|
await SetControlwordAsync(Controlword.EnableOperationCmd, ct);
|
|
await Task.Delay(100, ct);
|
|
}
|
|
}
|
|
|
|
public async Task DisableAsync(CancellationToken ct = default)
|
|
{
|
|
await DisableOperationAsync(ct);
|
|
}
|
|
|
|
// Position Control
|
|
public async Task<int> GetActualPositionAsync(CancellationToken ct = default)
|
|
{
|
|
if (_canOpenDevice == null || _objectDictionary == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
|
|
lock (_lock)
|
|
{
|
|
return _cachedPosition;
|
|
}
|
|
}
|
|
|
|
public async Task SetTargetPositionAsync(int position, CancellationToken ct = default)
|
|
{
|
|
if (_canOpenDevice == null || _objectDictionary == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
|
|
lock (_lock)
|
|
{
|
|
_targetPosition = position;
|
|
}
|
|
|
|
// Ghi qua RPDO/SDO tự động
|
|
await WriteObjectAsync(_objectDictionary.TargetPosition, 0, (uint)position, ct);
|
|
}
|
|
|
|
public async Task SetProfileSpeedAsync(uint velocity, CancellationToken ct = default)
|
|
{
|
|
if (_canOpenDevice == null || _objectDictionary == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
|
|
await _canOpenDevice.WriteUInt32Async(_objectDictionary.ProfileSpeed, 0, velocity, ct);
|
|
}
|
|
|
|
public async Task SetProfileVelocityAsync(uint velocity, CancellationToken ct = default)
|
|
{
|
|
if (_canOpenDevice == null || _objectDictionary == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
|
|
await _canOpenDevice.WriteUInt32Async(_objectDictionary.TargetVelocity, 0, velocity, ct);
|
|
}
|
|
|
|
public async Task SetProfileAccelerationAsync(uint acceleration, CancellationToken ct = default)
|
|
{
|
|
if (_canOpenDevice == null || _objectDictionary == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
|
|
await _canOpenDevice.WriteUInt32Async(_objectDictionary.ProfileAcceleration, 0, acceleration, ct);
|
|
}
|
|
|
|
public async Task SetProfileDecelerationAsync(uint deceleration, CancellationToken ct = default)
|
|
{
|
|
if (_canOpenDevice == null || _objectDictionary == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
|
|
await _canOpenDevice.WriteUInt32Async(_objectDictionary.ProfileDeceleration, 0, deceleration, ct);
|
|
}
|
|
|
|
public async Task<uint> GetProfileSpeedAsync(CancellationToken ct = default)
|
|
{
|
|
if (_canOpenDevice == null || _objectDictionary == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
return await _canOpenDevice.ReadUInt32Async(_objectDictionary.ProfileSpeed, 0, ct);
|
|
}
|
|
|
|
public async Task<uint> GetProfileAccelerationAsync(CancellationToken ct = default)
|
|
{
|
|
if (_canOpenDevice == null || _objectDictionary == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
return await _canOpenDevice.ReadUInt32Async(_objectDictionary.ProfileAcceleration, 0, ct);
|
|
}
|
|
|
|
public async Task<uint> GetProfileDecelerationAsync(CancellationToken ct = default)
|
|
{
|
|
if (_canOpenDevice == null || _objectDictionary == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
return await _canOpenDevice.ReadUInt32Async(_objectDictionary.ProfileDeceleration, 0, ct);
|
|
}
|
|
|
|
public async Task StartPositionMoveAsync(CancellationToken ct = default)
|
|
{
|
|
var newSetpoint = new Controlword(0x001F);
|
|
var resetSetpoint = new Controlword(0x000F);
|
|
await SetControlwordAsync(newSetpoint, ct);
|
|
await Task.Delay(50, ct);
|
|
await SetControlwordAsync(resetSetpoint, ct);
|
|
}
|
|
|
|
public async Task MoveToPositionAsync(int position, uint velocity = 10000, uint acceleration = 5000, uint deceleration = 5000, CancellationToken ct = default)
|
|
{
|
|
_logger?.LogInformation("CiA402Servo[{DeviceId}]: MoveToPositionAsync position={Position}, velocity={Velocity} -> writing to drive (SDO/PDO).", DeviceId, position, velocity);
|
|
|
|
OperationMode currentMode;
|
|
DriveState currentState;
|
|
try
|
|
{
|
|
currentMode = await GetOperationModeAsync(ct);
|
|
currentState = await GetStateAsync(ct);
|
|
}
|
|
catch
|
|
{
|
|
currentMode = OperationMode.ProfilePosition;
|
|
currentState = DriveState.SwitchedOn;
|
|
}
|
|
|
|
bool alreadyReady = currentMode == OperationMode.ProfilePosition && currentState == DriveState.OperationEnabled;
|
|
|
|
if (!alreadyReady)
|
|
{
|
|
await DisableOperationAsync(ct);
|
|
await SetOperationModeAsync(OperationMode.ProfilePosition, ct);
|
|
await Task.Delay(50, ct);
|
|
await EnableAsync(ct);
|
|
await Task.Delay(50, ct);
|
|
}
|
|
|
|
await SetProfileSpeedAsync(velocity, ct);
|
|
await SetProfileAccelerationAsync(acceleration, ct);
|
|
await SetProfileDecelerationAsync(deceleration, ct);
|
|
// Set target trước (giống bấm SetTarget trên device UI)
|
|
await SetTargetPositionAsync(position, ct);
|
|
// Delay để drive latch target trước khi nhận lệnh Start (new setpoint bit) — giống bấm SetTarget rồi bấm Start
|
|
await Task.Delay(150, ct);
|
|
await StartPositionMoveAsync(ct);
|
|
}
|
|
|
|
// Velocity Control
|
|
public async Task<int> GetActualVelocityAsync(CancellationToken ct = default)
|
|
{
|
|
if (_canOpenDevice == null || _objectDictionary == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
|
|
lock (_lock)
|
|
{
|
|
return _cachedVelocity;
|
|
}
|
|
}
|
|
|
|
public async Task SetTargetVelocityAsync(int velocity, CancellationToken ct = default)
|
|
{
|
|
if (_canOpenDevice == null || _objectDictionary == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
|
|
// Ghi qua RPDO/SDO tự động
|
|
await WriteObjectAsync(_objectDictionary.TargetVelocity, 0, velocity, ct);
|
|
}
|
|
|
|
public async Task TargetVelocityAsync(int targetVelocity, uint acceleration = 5000, uint deceleration = 5000, CancellationToken ct = default)
|
|
{
|
|
await SetOperationModeAsync(OperationMode.ProfileVelocity, ct);
|
|
await EnableAsync(ct);
|
|
await SetProfileAccelerationAsync(acceleration, ct);
|
|
await SetProfileDecelerationAsync(deceleration, ct);
|
|
await SetTargetVelocityAsync(targetVelocity, ct);
|
|
}
|
|
|
|
public async Task ProfileVelocityAsync(int targetVelocity, uint acceleration = 5000, uint deceleration = 5000, CancellationToken ct = default)
|
|
{
|
|
await SetOperationModeAsync(OperationMode.ProfileVelocity, ct);
|
|
await Task.Delay(50, ct);
|
|
await EnableAsync(ct);
|
|
await SetProfileAccelerationAsync(acceleration, ct);
|
|
await SetProfileDecelerationAsync(deceleration, ct);
|
|
await SetTargetVelocityAsync(targetVelocity, ct);
|
|
}
|
|
|
|
// Torque Control
|
|
public async Task<short> GetActualTorqueAsync(CancellationToken ct = default)
|
|
{
|
|
if (_canOpenDevice == null || _objectDictionary == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
|
|
// Kiểm tra xem có mapped vào TPDO không để quyết định có dùng cache không
|
|
var key = (_objectDictionary.TorqueActualValue, (byte)0);
|
|
bool isMappedToTpdo = _pdoMappingRegistry.TryGetValue(key, out var mappingInfo) && !mappingInfo.IsRPDO;
|
|
|
|
if (isMappedToTpdo)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
// Nếu mapped vào TPDO, luôn dùng cache (kể cả khi = 0)
|
|
return _cachedTorque;
|
|
}
|
|
}
|
|
|
|
// Đọc qua PDO/SDO tự động
|
|
var value = await ReadObjectAsync<ushort>(_objectDictionary.TorqueActualValue, 0, ct);
|
|
lock (_lock)
|
|
{
|
|
_cachedTorque = (short)value;
|
|
return _cachedTorque;
|
|
}
|
|
}
|
|
|
|
public async Task SetTargetTorqueAsync(short torque, CancellationToken ct = default)
|
|
{
|
|
if (_canOpenDevice == null || _objectDictionary == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
|
|
// Ghi qua RPDO/SDO tự động
|
|
await WriteObjectAsync(_objectDictionary.TargetTorque, 0, (ushort)torque, ct);
|
|
}
|
|
|
|
public async Task RunTorqueAsync(short torque, CancellationToken ct = default)
|
|
{
|
|
await SetOperationModeAsync(OperationMode.ProfileTorque, ct);
|
|
await Task.Delay(50, ct);
|
|
await SetTargetTorqueAsync(torque, ct);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Kiểm tra xem homing method có được phép sử dụng không
|
|
/// </summary>
|
|
private bool IsHomingMethodAllowed(byte method)
|
|
{
|
|
// Nếu không có danh sách giới hạn, cho phép tất cả
|
|
if (_allowedHomingMethods == null || _allowedHomingMethods.Count == 0)
|
|
return true;
|
|
|
|
return _allowedHomingMethods.Contains(method);
|
|
}
|
|
|
|
// Homing
|
|
public async Task SetHomingMethodAsync(byte method, CancellationToken ct = default)
|
|
{
|
|
if (_canOpenDevice == null || _objectDictionary == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
|
|
// Kiểm tra method có được phép không
|
|
if (!IsHomingMethodAllowed(method))
|
|
{
|
|
var allowedMethods = _allowedHomingMethods != null && _allowedHomingMethods.Count > 0
|
|
? string.Join(", ", _allowedHomingMethods.OrderBy(m => m))
|
|
: "none";
|
|
throw new ArgumentException($"Homing method {method} is not allowed. Allowed methods: {allowedMethods}", nameof(method));
|
|
}
|
|
|
|
// Đảm bảo device ở PreOperational state trước khi set homing method
|
|
// await EnsurePreOperationalStateAsync(ct);
|
|
await DisableOperationAsync(ct);
|
|
// Ghi homing method xuống device
|
|
await _canOpenDevice.WriteUInt8Async(_objectDictionary.HomingMethod, 0, method, ct);
|
|
}
|
|
|
|
public async Task SetHomingSpeedAsync(int speed, CancellationToken ct = default)
|
|
{
|
|
if (_canOpenDevice == null || _objectDictionary == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
// Sub-index 1: speed during search for switch (CiA402 0x6099)
|
|
await _canOpenDevice.WriteInt32Async(_objectDictionary.HomingSpeed, 1, speed, ct);
|
|
}
|
|
|
|
public async Task SetHomingOffsetAsync(int offset, CancellationToken ct = default)
|
|
{
|
|
if (_canOpenDevice == null || _objectDictionary == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
// 0x607C Home Offset - sub-index 0
|
|
await _canOpenDevice.WriteInt32Async(_objectDictionary.HomingOffset, 0, offset, ct);
|
|
}
|
|
|
|
public async Task<byte> GetHomingMethodAsync(CancellationToken ct = default)
|
|
{
|
|
if (_canOpenDevice == null || _objectDictionary == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
return await _canOpenDevice.ReadUInt8Async(_objectDictionary.HomingMethod, 0, ct);
|
|
}
|
|
|
|
public async Task<int> GetHomingSpeedAsync(CancellationToken ct = default)
|
|
{
|
|
if (_canOpenDevice == null || _objectDictionary == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
return await _canOpenDevice.ReadInt32Async(_objectDictionary.HomingSpeed, 1, ct);
|
|
}
|
|
|
|
public async Task<int> GetHomingOffsetAsync(CancellationToken ct = default)
|
|
{
|
|
if (_canOpenDevice == null || _objectDictionary == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
return await _canOpenDevice.ReadInt32Async(_objectDictionary.HomingOffset, 0, ct);
|
|
}
|
|
|
|
public async Task StartHomingAsync(byte method, int speed, CancellationToken ct = default)
|
|
{
|
|
_logger?.LogInformation("CiA402Servo[{DeviceId}]: StartHomingAsync method={Method}, speed={Speed} -> writing to drive (SDO/Controlword).", DeviceId, method, speed);
|
|
// Kiểm tra method có được phép không
|
|
if (!IsHomingMethodAllowed(method))
|
|
{
|
|
var allowedMethods = _allowedHomingMethods != null && _allowedHomingMethods.Count > 0
|
|
? string.Join(", ", _allowedHomingMethods.OrderBy(m => m))
|
|
: "none";
|
|
throw new ArgumentException($"Homing method {method} is not allowed. Allowed methods: {allowedMethods}", nameof(method));
|
|
}
|
|
|
|
if (_canOpenDevice == null || _objectDictionary == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
|
|
const int pollIntervalMs = 50;
|
|
const int timeoutMs = 5000;
|
|
|
|
// --- Bước 1: PreOperational, ghi đủ tham số homing (giống driver cũ: method, speed switch, speed zero, acceleration, offset) ---
|
|
var currentNmtState = _canOpenDevice.State;
|
|
try
|
|
{
|
|
await EnsurePreOperationalStateAsync(ct);
|
|
await SetControlwordAsync(new Controlword(0x000F), ct);
|
|
await Task.Delay(50, ct);
|
|
|
|
await SetHomingMethodAsync(method, ct);
|
|
// 0x6099 sub-index 1: speed during search for switch
|
|
await _canOpenDevice.WriteInt32Async(_objectDictionary.HomingSpeed, 1, speed, ct);
|
|
// 0x6099 sub-index 2: speed during zero search (dùng cùng giá trị speed)
|
|
await _canOpenDevice.WriteInt32Async(_objectDictionary.HomingSpeed, 2, Math.Abs(speed), ct);
|
|
// 0x609A: Homing acceleration (bắt buộc trên một số drive; default nếu chưa set)
|
|
var homingAccel = (uint)Math.Max(100_000, Math.Abs(speed) * 20);
|
|
await _canOpenDevice.WriteUInt32Async(_objectDictionary.HomingAcceleration, 0, homingAccel, ct);
|
|
}
|
|
finally
|
|
{
|
|
if (currentNmtState == NmtState.Operational)
|
|
{
|
|
await _canOpenDevice.StartNodeAsync(ct);
|
|
await Task.Delay(100, ct);
|
|
}
|
|
}
|
|
|
|
// --- Bước 2: Set mode Homing, chờ drive chấp nhận (Modes of operation display = 6) ---
|
|
await SetOperationModeAsync(OperationMode.Homing, ct);
|
|
var modeDeadline = DateTime.UtcNow.AddMilliseconds(timeoutMs);
|
|
while (DateTime.UtcNow < modeDeadline)
|
|
{
|
|
var currentMode = await GetOperationModeAsync(ct);
|
|
if (currentMode == OperationMode.Homing)
|
|
break;
|
|
await Task.Delay(pollIntervalMs, ct);
|
|
}
|
|
await Task.Delay(100, ct);
|
|
|
|
// --- Bước 3: Gửi Operation enable (0x000F), chờ Statusword Operation enabled (bit 2) ---
|
|
await SetControlwordAsync(new Controlword(0x000F), ct);
|
|
var enableDeadline = DateTime.UtcNow.AddMilliseconds(timeoutMs);
|
|
while (DateTime.UtcNow < enableDeadline)
|
|
{
|
|
var state = await GetStateAsync(ct);
|
|
if (state == DriveState.OperationEnabled)
|
|
break;
|
|
await Task.Delay(pollIntervalMs, ct);
|
|
}
|
|
await Task.Delay(50, ct);
|
|
|
|
// --- Bước 4: Gửi Start homing (bit 4 = 1, 0x001F) ---
|
|
await SetControlwordAsync(new Controlword(0x001F), ct);
|
|
|
|
var homingAttained = false;
|
|
var homingError = false;
|
|
try
|
|
{
|
|
// --- Bước 5: Chờ homing xong (HomingAttained hoặc HomingError/timeout) ---
|
|
const int homingCompleteTimeoutMs = 120_000; // 2 phút
|
|
var homingDeadline = DateTime.UtcNow.AddMilliseconds(homingCompleteTimeoutMs);
|
|
while (DateTime.UtcNow < homingDeadline)
|
|
{
|
|
ct.ThrowIfCancellationRequested();
|
|
var sw = await GetStatuswordAsync(ct);
|
|
if (sw.HomingError)
|
|
{
|
|
homingError = true;
|
|
break;
|
|
}
|
|
if (sw.HomingAttained)
|
|
{
|
|
homingAttained = true;
|
|
break;
|
|
}
|
|
await Task.Delay(pollIntervalMs, ct);
|
|
}
|
|
}
|
|
finally
|
|
{
|
|
// Sau homing (dù thành công, lỗi hay timeout): clear bit 4, chuyển mode về Profile Position để có thể điều khiển pos/vel lại
|
|
await SetControlwordAsync(new Controlword(0x000F), ct);
|
|
await Task.Delay(50, ct);
|
|
await SetOperationModeAsync(OperationMode.ProfilePosition, ct);
|
|
}
|
|
|
|
// Báo kết quả cho caller: throw để LiftModuleService biết lỗi/timeout, return = thành công (tránh poll lại HomingAttained vì bit đã bị xóa khi đổi mode)
|
|
if (homingError)
|
|
throw new InvalidOperationException("Homing failed: drive reported homing error (Statusword HomingError)");
|
|
if (!homingAttained)
|
|
throw new TimeoutException("Homing timeout after 120 seconds");
|
|
}
|
|
|
|
// Target Position Checking
|
|
public bool IsAtTarget(int tolerance = 100, bool useStatusword = true)
|
|
{
|
|
if (useStatusword)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
return _cachedStatusword.TargetReached;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
lock (_lock)
|
|
{
|
|
return Math.Abs(_cachedPosition - _targetPosition) <= tolerance;
|
|
}
|
|
}
|
|
}
|
|
|
|
public async Task WaitUntilAtTargetAsync(int tolerance = 100, bool useStatusword = true, int checkIntervalMs = 10, CancellationToken ct = default)
|
|
{
|
|
while (!ct.IsCancellationRequested)
|
|
{
|
|
if (IsAtTarget(tolerance, useStatusword))
|
|
return;
|
|
|
|
await Task.Delay(checkIntervalMs, ct);
|
|
}
|
|
|
|
throw new OperationCanceledException("Wait for servo at target was cancelled", ct);
|
|
}
|
|
|
|
// Error and Status Information
|
|
public async Task<bool> IsInFaultStateAsync(CancellationToken ct = default)
|
|
{
|
|
var state = await GetStateAsync(ct);
|
|
return state == DriveState.Fault;
|
|
}
|
|
|
|
public async Task<byte> GetErrorRegisterAsync(CancellationToken ct = default)
|
|
{
|
|
if (_canOpenDevice == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
|
|
try
|
|
{
|
|
return await _canOpenDevice.ReadUInt8Async(0x1001, 0, ct);
|
|
}
|
|
catch
|
|
{
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
public async Task<ushort[]> GetErrorHistoryAsync(CancellationToken ct = default)
|
|
{
|
|
if (_canOpenDevice == null)
|
|
throw new InvalidOperationException("Device not initialized");
|
|
|
|
try
|
|
{
|
|
byte errorCount = await _canOpenDevice.ReadUInt8Async(0x1003, 0, ct);
|
|
|
|
if (errorCount == 0) return [];
|
|
|
|
var errors = new List<ushort>();
|
|
for (byte i = 1; i <= errorCount && i <= 8; i++)
|
|
{
|
|
try
|
|
{
|
|
ushort errorCode = await _canOpenDevice.ReadUInt16Async(0x1003, i, ct);
|
|
errors.Add(errorCode);
|
|
}
|
|
catch
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
|
|
return [.. errors];
|
|
}
|
|
catch
|
|
{
|
|
return [];
|
|
}
|
|
}
|
|
|
|
public async Task<ushort> GetLatestErrorCodeAsync(CancellationToken ct = default)
|
|
{
|
|
var errorHistory = await GetErrorHistoryAsync(ct);
|
|
return errorHistory.Length > 0 ? errorHistory[0] : (ushort)0x0000;
|
|
}
|
|
|
|
public async Task<bool> TryFaultResetAsync(CancellationToken ct = default)
|
|
{
|
|
var state = await GetStateAsync(ct);
|
|
if (state != DriveState.Fault)
|
|
return false;
|
|
|
|
await FaultResetAsync(ct);
|
|
return true;
|
|
}
|
|
|
|
public async Task<bool> IsEnabledAsync(CancellationToken ct = default)
|
|
{
|
|
var state = await GetStateAsync(ct);
|
|
return state == DriveState.OperationEnabled;
|
|
}
|
|
|
|
public async Task<bool> IsReadyAsync(CancellationToken ct = default)
|
|
{
|
|
var state = await GetStateAsync(ct);
|
|
return state == DriveState.ReadyToSwitchOn || state == DriveState.SwitchedOn;
|
|
}
|
|
|
|
#endregion
|
|
|
|
protected override void Dispose(bool disposing)
|
|
{
|
|
if (disposing)
|
|
{
|
|
if (_canOpenDevice != null)
|
|
{
|
|
_canOpenDevice.PdoReceived -= OnPdoReceived;
|
|
}
|
|
}
|
|
base.Dispose(disposing);
|
|
}
|
|
}
|
|
|