diff --git a/srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/CanOpenDevice.cs b/srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/CanOpenDevice.cs index ef36410..3a53040 100644 --- a/srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/CanOpenDevice.cs +++ b/srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/CanOpenDevice.cs @@ -29,6 +29,13 @@ public class CanOpenDevice : ICanOpenDevice, IDisposable /// When enabled, State will be updated automatically when heartbeat is received /// public bool AutoVerifyStateViaHeartbeat { get; set; } = false; + + /// + /// Lock cho trình tự cấu hình cấp node (PDO config, NMT state, tham số chung). + /// Nhiều facade dùng chung một node vật lý (vd. 2 trục của ZLAC8015D) phải giữ lock này + /// trong suốt trình tự connect để không xen kẽ NMT/PDO của nhau. + /// + public SemaphoreSlim ConfigurationLock { get; } = new(1, 1); // Expose các services để user có thể truy cập public PdoManager Pdo => _pdoManager; @@ -154,7 +161,20 @@ public class CanOpenDevice : ICanOpenDevice, IDisposable { await WriteObjectAsync(index, subIndex, BitConverter.GetBytes(value), cancellationToken); } - + + /// + /// Ghi nhiều giá trị int32 trong một lần giữ kênh SDO liên tục — không giao dịch nào + /// khác chen được vào giữa các lệnh. Dùng cho lệnh cần đi liền nhau (vd vận tốc 2 bánh). + /// + public async Task WriteInt32BatchAsync(IReadOnlyList<(ushort Index, byte SubIndex, int Value)> items, CancellationToken cancellationToken = default) + { + var batch = new (ushort, byte, byte[])[items.Count]; + for (int i = 0; i < items.Count; i++) + batch[i] = (items[i].Index, items[i].SubIndex, BitConverter.GetBytes(items[i].Value)); + + await _sdoClient.DownloadBatchAsync(batch, cancellationToken); + } + #endregion #region NMT Operations diff --git a/srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/Services/PdoManager.cs b/srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/Services/PdoManager.cs index 023cc39..f4bb170 100644 --- a/srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/Services/PdoManager.cs +++ b/srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/Services/PdoManager.cs @@ -360,6 +360,97 @@ public class PdoManager : IDisposable return supportedIndices.Contains(mappingParamIndex); } + /// + /// Poll 0x1000 (device type) cho tới khi drive trả lời SDO, tối đa maxWaitMs. + /// Dùng sau NMT Reset Communication để bước SDO kế tiếp không bị timeout oan. + /// + private async Task WaitUntilSdoRespondsAsync(ICanOpenDevice device, CancellationToken ct, int maxWaitMs = 3000) + { + const int retryDelayMs = 200; + int waited = 0; + + while (waited < maxWaitMs) + { + try + { + await device.ReadUInt32Async(0x1000, 0x00, ct); + return; // Drive đã trả lời -> stack sẵn sàng + } + catch (OperationCanceledException) when (ct.IsCancellationRequested) + { + throw; + } + catch + { + await Task.Delay(retryDelayMs, ct); + waited += retryDelayMs; + } + } + + _logger?.LogWarning( + "Device Node {NodeId} chưa trả lời SDO sau {MaxWaitMs}ms kể từ Reset Communication. Tiếp tục cấu hình PDO...", + device.NodeId, maxWaitMs); + } + + /// + /// Đọc ngược cấu hình TPDO từ drive để biết giá trị nào THỰC SỰ được chấp nhận. + /// Nhiều firmware im lặng bỏ qua hoặc ép lại COB-ID / transmission type / event timer, + /// khiến TPDO không bao giờ phát dù mọi lệnh ghi đều báo thành công. + /// + private async Task LogActualTpdoConfigAsync(ICanOpenDevice device, byte pdoNumber, CancellationToken ct) + { + ushort commParamIndex = (ushort)(0x1800 + (pdoNumber - 1)); + ushort mappingParamIndex = (ushort)(0x1A00 + (pdoNumber - 1)); + + try + { + uint cobId = await device.ReadUInt32Async(commParamIndex, 0x01, ct); + byte transmissionType = await device.ReadUInt8Async(commParamIndex, 0x02, ct); + byte mappingCount = await device.ReadUInt8Async(mappingParamIndex, 0x00, ct); + + ushort eventTimer = 0; + try + { + eventTimer = await device.ReadUInt16Async(commParamIndex, 0x05, ct); + } + catch + { + // Firmware không có event timer -> giữ 0 + } + + bool enabled = (cobId & 0x80000000) == 0; + + _logger?.LogInformation( + "TPDO{PDONumber} readback Node {NodeId}: COB-ID=0x{CobId:X3} ({State}), TransmissionType=0x{Type:X2}, EventTimer={EventTimer}ms, MappingCount={Count}", + pdoNumber, device.NodeId, cobId & 0x7FF, enabled ? "ENABLED" : "DISABLED", + transmissionType, eventTimer, mappingCount); + + if (!enabled) + { + _logger?.LogWarning("TPDO{PDONumber} Node {NodeId}: drive vẫn để bit 31 = 1 (PDO bị DISABLE) -> sẽ không phát frame nào.", + pdoNumber, device.NodeId); + } + + if (mappingCount == 0) + { + _logger?.LogWarning("TPDO{PDONumber} Node {NodeId}: MappingCount = 0 -> drive không giữ mapping đã ghi, TPDO sẽ không có dữ liệu.", + pdoNumber, device.NodeId); + } + + if (transmissionType is not (0xFE or 0xFF) && eventTimer == 0) + { + _logger?.LogWarning( + "TPDO{PDONumber} Node {NodeId}: TransmissionType=0x{Type:X2} là kiểu đồng bộ (theo SYNC) và EventTimer=0. " + + "Không có SYNC producer thì TPDO sẽ không bao giờ phát.", + pdoNumber, device.NodeId, transmissionType); + } + } + catch (Exception ex) + { + _logger?.LogWarning(ex, "Không đọc ngược được cấu hình TPDO{PDONumber} trên Node {NodeId}", pdoNumber, device.NodeId); + } + } + /// /// Stop và clear tất cả TPDOs trên device /// @@ -426,7 +517,12 @@ public class PdoManager : IDisposable try { await device.ResetCommunicationAsync(ct); - await Task.Delay(100, ct); // Wait for device to reset + await Task.Delay(500, ct); + + // Chờ drive dựng xong CANopen stack: poll 0x1000 (device type) cho tới khi + // nó trả lời. Delay cố định không đủ tin cậy — SDO đầu tiên sau reset hay bị + // timeout, gây warning "Failed to stop/clear TPDO1" ở bước sau. + await WaitUntilSdoRespondsAsync(device, ct); } catch (Exception ex) { @@ -534,6 +630,9 @@ public class PdoManager : IDisposable await WritePdoConfigurationToDeviceAsync(device, true, kvp.Key, ct); _configuredTpdoNumbers.Add(kvp.Key); + + // Đọc ngược để xác nhận drive thực sự chấp nhận cấu hình (không chỉ ACK lệnh ghi) + await LogActualTpdoConfigAsync(device, kvp.Key, ct); } catch (Exception ex) { diff --git a/srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/Services/SdoClient.cs b/srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/Services/SdoClient.cs index dd1ceee..65b4354 100644 --- a/srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/Services/SdoClient.cs +++ b/srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/Services/SdoClient.cs @@ -12,6 +12,9 @@ public class SdoClient : IDisposable private readonly ICanBus _canBus; private readonly byte _nodeId; private readonly ConcurrentDictionary> _pendingRequests; + // CANopen chỉ cho phép 1 giao dịch SDO tại một thời điểm trên mỗi node. + // Hai facade trái/phải của ZLAC8015D dùng chung SdoClient nên phải xếp hàng ở đây. + private readonly SemaphoreSlim _transactionLock = new(1, 1); private readonly TimeSpan _timeout; private readonly ILogger? _logger; private bool _disposed; @@ -31,26 +34,37 @@ public class SdoClient : IDisposable { if (_disposed) throw new ObjectDisposedException(nameof(SdoClient)); - - var request = SdoRequest.CreateUpload(index, subIndex); - var response = await SendRequestAsync(request, cancellationToken); - - if (response.IsAbort) + + await _transactionLock.WaitAsync(cancellationToken); + try { - var abortCode = (SdoAbortCode)(uint)response.AbortCode; - var message = $"SDO Upload failed: {abortCode.GetDescription()}"; - throw new SdoException(_nodeId, index, subIndex, (uint)response.AbortCode, message); + if (_disposed) + throw new ObjectDisposedException(nameof(SdoClient)); + + var request = SdoRequest.CreateUpload(index, subIndex); + var response = await SendRequestAsync(request, cancellationToken); + + if (response.IsAbort) + { + var abortCode = (SdoAbortCode)(uint)response.AbortCode; + var message = $"SDO Upload failed: {abortCode.GetDescription()}"; + throw new SdoException(_nodeId, index, subIndex, (uint)response.AbortCode, message); + } + + // Check if expedited or segmented + if (response.IsExpedited) + { + return response.GetDataBytes(); + } + else + { + // Segmented transfer + return await UploadSegmentedAsync(index, subIndex, response, cancellationToken); + } } - - // Check if expedited or segmented - if (response.IsExpedited) + finally { - return response.GetDataBytes(); - } - else - { - // Segmented transfer - return await UploadSegmentedAsync(index, subIndex, response, cancellationToken); + _transactionLock.Release(); } } @@ -126,27 +140,81 @@ public class SdoClient : IDisposable { if (_disposed) throw new ObjectDisposedException(nameof(SdoClient)); - - if (data.Length <= 4) + + await _transactionLock.WaitAsync(cancellationToken); + try { - // Expedited transfer (≤4 bytes) - var request = SdoRequest.CreateDownload(index, subIndex, data); - var response = await SendRequestAsync(request, cancellationToken); - - if (response.IsAbort) + if (_disposed) + throw new ObjectDisposedException(nameof(SdoClient)); + + if (data.Length <= 4) { - var abortCode = (SdoAbortCode)(uint)response.AbortCode; - var message = $"SDO Download failed: {abortCode.GetDescription()}"; - throw new SdoException(_nodeId, index, subIndex, (uint)response.AbortCode, message); + // Expedited transfer (≤4 bytes) + var request = SdoRequest.CreateDownload(index, subIndex, data); + var response = await SendRequestAsync(request, cancellationToken); + + if (response.IsAbort) + { + var abortCode = (SdoAbortCode)(uint)response.AbortCode; + var message = $"SDO Download failed: {abortCode.GetDescription()}"; + throw new SdoException(_nodeId, index, subIndex, (uint)response.AbortCode, message); + } + } + else + { + // Segmented transfer (>4 bytes) + await DownloadSegmentedAsync(index, subIndex, data, cancellationToken); } } - else + finally { - // Segmented transfer (>4 bytes) - await DownloadSegmentedAsync(index, subIndex, data, cancellationToken); + _transactionLock.Release(); } } + /// + /// Ghi nhiều object liên tiếp trong MỘT lần giữ transaction lock. + /// Dùng khi các lệnh phải đi liền nhau không bị giao dịch SDO khác chen giữa, + /// ví dụ lệnh vận tốc hai bánh của ZLAC8015D khi không có RPDO: nếu ghi bằng hai + /// DownloadAsync riêng, poll feedback có thể chiếm kênh giữa hai lệnh làm bánh sau + /// nhận lệnh muộn hơn hẳn. Chỉ hỗ trợ expedited transfer (data ≤ 4 byte mỗi mục). + /// + public async Task DownloadBatchAsync(IReadOnlyList<(ushort Index, byte SubIndex, byte[] Data)> items, CancellationToken cancellationToken = default) + { + if (_disposed) + throw new ObjectDisposedException(nameof(SdoClient)); + + foreach (var item in items) + { + if (item.Data.Length > 4) + throw new NotSupportedException("DownloadBatchAsync chỉ hỗ trợ expedited transfer (≤4 byte mỗi mục)"); + } + + await _transactionLock.WaitAsync(cancellationToken); + try + { + if (_disposed) + throw new ObjectDisposedException(nameof(SdoClient)); + + foreach (var (index, subIndex, data) in items) + { + var request = SdoRequest.CreateDownload(index, subIndex, data); + var response = await SendRequestAsync(request, cancellationToken); + + if (response.IsAbort) + { + var abortCode = (SdoAbortCode)(uint)response.AbortCode; + var message = $"SDO Download failed: {abortCode.GetDescription()}"; + throw new SdoException(_nodeId, index, subIndex, (uint)response.AbortCode, message); + } + } + } + finally + { + _transactionLock.Release(); + } + } + /// /// Download data using segmented transfer (for data > 4 bytes) /// diff --git a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Devices/IDualAxisVelocityCommand.cs b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Devices/IDualAxisVelocityCommand.cs new file mode 100644 index 0000000..a7dc883 --- /dev/null +++ b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Devices/IDualAxisVelocityCommand.cs @@ -0,0 +1,16 @@ +namespace RobotNet10.RobotApp.Devices; + +/// +/// Khả năng gửi vận tốc của hai trục dùng chung một node CAN trong MỘT khung duy nhất, +/// để hai bánh nhận lệnh đồng thời (vd. ZLAC8015D dual-hub). +/// Driver một-trục-một-node không cần implement interface này. +/// +public interface IDualAxisVelocityCommand +{ + /// + /// Gửi vận tốc cả hai trục cùng lúc. Đơn vị counts/s, đã bao gồm hiệu chỉnh IsReversed. + /// Trả false nếu không gửi được theo cơ chế gộp (vd. RPDO chưa cấu hình) — caller nên + /// fallback về cách gửi từng trục. + /// + Task TrySetBothTargetVelocitiesAsync(int leftCountsPerSec, int rightCountsPerSec, CancellationToken ct = default); +} diff --git a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/ZLAC/Zlac8015dServo.cs b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/ZLAC/Zlac8015dServo.cs new file mode 100644 index 0000000..b0c8992 --- /dev/null +++ b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/ZLAC/Zlac8015dServo.cs @@ -0,0 +1,1704 @@ +using RobotNet10.CANOpen; +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 RobotNet10.RobotApp.Drivers.PhenikaaX; +using System.Collections.Concurrent; + +namespace RobotNet10.RobotApp.Drivers.ZLAC; + +/// +/// Trục của motor trên driver ZLAC8015D (1 node CAN điều khiển 2 bánh) +/// +public enum Zlac8015dAxis +{ + Left, + Right +} + +/// +/// Cấu hình cho ZLAC8015D driver +/// +internal class Zlac8015dConfig +{ + public string CanInterface { get; set; } = string.Empty; + public string NodeId { get; set; } = string.Empty; + /// + /// Trục motor mà instance này đại diện: "Left" hoặc "Right" + /// + public string Axis { get; set; } = string.Empty; + /// + /// Số count encoder trên một vòng bánh (ZLAC8015D mặc định 4096) + /// Dùng để quy đổi counts/s (interface ICiA402Servo) sang r/min (đơn vị của drive) + /// + public int CountsPerRevolution { get; set; } = 4096; + /// + /// Độ phân giải tốc độ đặt (object 0x2026 sub 06): đơn vị lệnh 0x60FF = 1/N r/min. + /// N = 1..10. Mặc định 10 (0.1 rpm). Nếu firmware không hỗ trợ, driver tự fallback về 1. + /// + public int SpeedResolution { get; set; } = 10; + /// + /// Thời gian tăng tốc (0x6083, ms, 0-32767). ZLAC dùng đơn vị thời gian, + /// không quy đổi được từ counts/s² nên cấu hình trực tiếp tại đây. + /// + public ushort AccelTimeMs { get; set; } = 200; + /// + /// Thời gian giảm tốc (0x6084, ms, 0-32767) + /// + public ushort DecelTimeMs { get; set; } = 200; + 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; } +} + +/// +/// Driver cho ZLAC8015D dual-wheel hub servo (CANopen CiA402). +/// Đặc thù: MỘT node CAN điều khiển HAI bánh, các object trục dùng sub-index 01 (trái) / 02 (phải); +/// Controlword/Statusword/Mode (0x6040/0x6041/0x6060) dùng chung sub 0 cho cả hai bánh. +/// +/// Để giữ nguyên mô hình "mỗi bánh một ICiA402Servo" của DifferentialDrive/OdometryService, +/// mỗi instance của driver này là một "facade" cho MỘT trục (Axis = Left/Right); +/// hai instance (trái + phải) trỏ cùng CanInterface + NodeId sẽ dùng chung một CanOpenDevice +/// (ICanOpenManager cache theo interface + nodeId). +/// +/// Đơn vị: interface nhận/trả counts và counts/s (giống driver PhenikaaX); +/// driver quy đổi sang r/min của ZLAC theo CountsPerRevolution. +/// Lệnh 0x60FF ghi theo độ phân giải 1/SpeedResolution r/min (0x2026:06); +/// feedback 0x606C cố định 0.1 r/min; feedback 0x6064 là counts (không quy đổi). +/// +/// Feedback qua TPDO. Firmware ZLAC8015D phát TPDO khi DỮ LIỆU THAY ĐỔI (bỏ qua event +/// timer 0x18xx:05 — đo thực tế: TPDO đặt statusword ở đầu không bao giờ phát dù position +/// phía sau đổi liên tục, còn TPDO velocity phát ~240 lần/s theo mỗi thay đổi). Vì vậy: +/// TPDO1 = 0x6064:01 + 0x6064:02 (position 2 bánh - đổi khi lăn bánh -> có event) +/// TPDO2 = 0x6041:00 (statusword - phát ngay khi đổi, vd fault) +/// TPDO3 = 0x606C:01 + 0x606C:02 (velocity 2 bánh) +/// Mỗi TPDO đặt InhibitTime 50ms để firmware không phát dồn dập theo từng thay đổi. +/// Hai instance trái/phải đăng ký mapping GIỐNG HỆT nhau (idempotent), mỗi instance +/// chỉ đọc phần bit của trục mình. Khi TPDO không phát (đứng yên), poll SDO nền làm tươi. +/// +[Device(DeviceType.CiA402Servo, "ZLAC", "ZLAC8015D", "1.0.0", Description = "ZLAC8015D Dual Wheel Hub Servo Driver (CANopen CiA402, 1 node / 2 bánh)")] +public class Zlac8015dServo : DeviceBase, ICiA402Servo, IDualAxisVelocityCommand +{ + // Object dictionary ZLAC8015D + private const ushort ObjControlword = 0x6040; // sub 0, chung 2 bánh + private const ushort ObjStatusword = 0x6041; // sub 0, chung 2 bánh + private const ushort ObjModesOfOperation = 0x6060; // sub 0, chung 2 bánh + private const ushort ObjModesOfOperationDisplay = 0x6061; + private const ushort ObjPositionActual = 0x6064; // sub 1/2, counts + private const ushort ObjVelocityActual = 0x606C; // sub 1/2, 0.1 r/min + private const ushort ObjTargetTorque = 0x6071; // sub 1/2, mA + private const ushort ObjTargetPosition = 0x607A; // sub 1/2, counts + private const ushort ObjProfileVelocity = 0x6081; // sub 1/2, r/min (position mode) + private const ushort ObjProfileAcceleration = 0x6083; // sub 1/2, ms + private const ushort ObjProfileDeceleration = 0x6084; // sub 1/2, ms + private const ushort ObjTargetVelocity = 0x60FF; // sub 1/2, 1/SpeedResolution r/min + private const ushort ObjProducerHeartbeatTime = 0x1017; // sub 0, ms - node tự phát heartbeat + private const ushort HeartbeatProducerTimeMs = 500; // phải nhỏ hơn HeartbeatConsumerTimeoutMs + private const int HeartbeatConsumerTimeoutMs = 2000; + private const int HeartbeatStartupWaitMs = 1500; // ~3 chu kỳ producer + private const int EnableTimeoutMs = 3000; // tổng thời gian đưa drive lên OperationEnabled + private const int StateStepDelayMs = 100; // chờ giữa mỗi bước chuyển trạng thái + // RPDO gộp target velocity của CẢ HAI trục vào MỘT khung: 0x60FF:01 (byte 0-3) + + // 0x60FF:02 (byte 4-7). Một frame lệnh cả hai bánh cùng lúc -> đồng bộ tuyệt đối, + // và không đi qua semaphore SDO nên không bị poll feedback chen ngang. + private const byte DualVelocityRpdoNumber = 1; // RPDO1, COB-ID 0x200 + nodeId + private const ushort ObjSyncControlFlag = 0x200F; // 0 = asynchronous control + private const ushort ObjMotorParams = 0x2026; // sub 6 = given speed resolution (1..10) + private const byte SubSpeedResolution = 0x06; + + // Feedback 0x606C luôn là 0.1 r/min bất kể 0x2026:06 + private const double FeedbackUnitRpm = 0.1; + private const int MaxRpm = 1000; + + private readonly ICanOpenManager? _canOpenManager; + private readonly string _canInterface; + private readonly byte _nodeId; + private readonly Zlac8015dAxis _axis; + private readonly byte _axisSub; // sub-index của trục: 1 = trái, 2 = phải + private readonly int _countsPerRevolution; + private readonly int _configuredSpeedResolution; + private int _speedResolution; // hiệu lực thực tế (fallback = 1 nếu drive từ chối 0x2026:06) + private readonly ushort _accelTimeMs; + private readonly ushort _decelTimeMs; + private readonly int _pdoConfigRetryCount; + private readonly int _pdoConfigRetryTimeoutMs; + private readonly bool _useHeartbeatCheck; + // Trạng thái runtime của heartbeat check: tắt nếu không ghi được 0x1017 (drive không phát heartbeat) + private volatile bool _heartbeatCheckActive; + private readonly ILogger _logger; + private CanOpenDevice? _canOpenDevice; + + // TPDO layout dùng chung cho cả 2 trục (xem doc comment của class) + private const byte PositionTpdoNumber = 1; // 0x6064:01 + 0x6064:02 + private const byte StatuswordTpdoNumber = 2; // 0x6041:00 + private const byte VelocityTpdoNumber = 3; // 0x606C:01 + 0x606C:02 + // Firmware phát TPDO theo thay đổi dữ liệu, inhibit time chặn phát dồn dập (đơn vị 100µs) + private const ushort TpdoInhibitTime100Us = 500; // 50ms + + // Registry mapping TPDO của riêng trục này: (index, subIndex) -> info + private readonly ConcurrentDictionary<(ushort Index, byte SubIndex), PdoMappingInfo> _pdoMappingRegistry = new(); + + // Kế hoạch parse TPDO tính sẵn theo PdoNumber. OnPdoReceived chạy đồng bộ trên thread + // nhận frame CAN (PdoManager.OnFrameReceived) nên tuyệt đối không LINQ/cấp phát mỗi frame. + private Dictionary _tpdoParsePlan = new(); + + // Thread-safe cached values (đơn vị interface: counts, counts/s) + private readonly Lock _lock = new(); + private Statusword _cachedStatusword; + private int _cachedPosition; + private int _cachedVelocity; + + // Thời điểm TPDO cập nhật từng giá trị lần cuối. Chỉ tin cache khi TPDO còn "tươi"; + // nếu TPDO không về (mapping lỗi/chưa Operational) thì đọc thẳng SDO. Không có + // fallback này thì Position/Velocity đứng yên ở 0 vĩnh viễn dù drive vẫn chạy. + private DateTime _statuswordUpdatedAt = DateTime.MinValue; + private DateTime _positionUpdatedAt = DateTime.MinValue; + private DateTime _velocityUpdatedAt = DateTime.MinValue; + private static readonly TimeSpan TpdoCacheTtl = TimeSpan.FromMilliseconds(200); + + // Poll nền làm tươi feedback khi TPDO không về (xem StartFeedbackPolling) + private const int FeedbackPollIntervalMs = 50; + private static readonly TimeSpan TpdoAbsentGracePeriod = TimeSpan.FromSeconds(3); + private CancellationTokenSource? _feedbackPollCts; + private Task? _feedbackPollTask; + private volatile bool _feedbackPollWarned; + private DateTime? _tpdoAbsentSinceUtc; + private short _cachedTorque; + private int _targetPosition; + + public event EventHandler? StatuswordChanged; + public event EventHandler? PositionChanged; + public event EventHandler? VelocityChanged; + + public Zlac8015dServo(string deviceId, string deviceName, IConfigurationSection connection, IServiceProvider serviceProvider) + : base(deviceId, deviceName, DeviceType.CiA402Servo) + { + var config = new Zlac8015dConfig(); + connection.Bind(config); + + 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"); + + if (!Enum.TryParse(config.Axis, ignoreCase: true, out _axis)) + throw new InvalidOperationException($"Invalid Axis: '{config.Axis}'. Must be 'Left' or 'Right'"); + + _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"); + + if (config.CountsPerRevolution <= 0) + throw new InvalidOperationException($"Invalid CountsPerRevolution: {config.CountsPerRevolution}. Must be greater than 0"); + + if (config.SpeedResolution < 1 || config.SpeedResolution > 10) + throw new InvalidOperationException($"Invalid SpeedResolution: {config.SpeedResolution}. Must be between 1 and 10"); + + _countsPerRevolution = config.CountsPerRevolution; + _configuredSpeedResolution = config.SpeedResolution; + _speedResolution = config.SpeedResolution; + _accelTimeMs = config.AccelTimeMs; + _decelTimeMs = config.DecelTimeMs; + + _axisSub = _axis == Zlac8015dAxis.Left ? (byte)1 : (byte)2; + + _pdoConfigRetryCount = config.PdoConfigRetryCount ?? 3; + _pdoConfigRetryTimeoutMs = config.PdoConfigRetryTimeoutMs ?? 1000; + _useHeartbeatCheck = config.UseHeartbeatCheck ?? true; + + AutoReconnectEnabled = config.AutoReconnectEnabled ?? true; + ReconnectDelayMs = config.ReconnectDelayMs ?? 3000; + MaxReconnectAttempts = config.MaxReconnectAttempts ?? 0; + + SetProperty("CanInterface", _canInterface); + SetProperty("NodeId", _nodeId.ToString()); + SetProperty("Axis", _axis.ToString()); + SetProperty("UseHeartbeatCheck", _useHeartbeatCheck.ToString()); + + _canOpenManager = serviceProvider.GetRequiredService(); + _logger = serviceProvider.GetRequiredService().CreateLogger(); + + _cachedStatusword = new Statusword(0); + } + + protected override IEnumerable 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), chung cho cả 2 bánh") + { + DataType = "number", + IsReadOnly = true, + DisplayOrder = 2, + Category = "Kết nối", + DefaultValue = "1" + }; + + yield return new PropertyDescription("Axis", "Trục", "Trục motor: Left hoặc Right") + { + DataType = "string", + IsReadOnly = true, + DisplayOrder = 3, + Category = "Kết nối", + DefaultValue = "Left" + }; + + 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, chung 2 bánh)") + { + 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("DriveState", "Drive State", "Trạng thái drive") + { + DataType = "string", + IsReadOnly = true, + DisplayOrder = 8, + 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."); + } + + // CanOpenManager cache theo (interface, nodeId) nên 2 instance trái/phải nhận cùng một CanOpenDevice + _canOpenDevice = await _canOpenManager.GetOrCreateDeviceAsync(_canInterface, _nodeId, cancellationToken); + + _canOpenDevice.PdoReceived += OnPdoReceived; + + ConfigurePdos(); + UpdateProperties(); + } + + protected override async Task OnConnectAsync(CancellationToken cancellationToken) + { + if (_canOpenDevice == null) + throw new InvalidOperationException("Device not initialized. Call InitializeAsync first."); + + if (_canOpenManager != null) + { + var canBus = _canOpenManager.GetCanBus(_canInterface); + if (canBus == null || !canBus.IsConnected) + { + throw new InvalidOperationException($"ICanBus for interface '{_canInterface}' is not connected."); + } + } + + // Hai facade trái/phải dùng chung node vật lý: giữ lock để trình tự + // PDO config + NMT + tham số của trục này không xen kẽ với trục kia + await _canOpenDevice.ConfigurationLock.WaitAsync(cancellationToken); + try + { + await ConfigureDevicePdosAsync(cancellationToken); + + var currentState = _canOpenDevice.State; + if (currentState != NmtState.PreOperational && currentState != NmtState.Stopped) + { + await _canOpenDevice.SendNmtCommandAsync(NmtCommand.PreOperational, cancellationToken); + await Task.Delay(100, cancellationToken); + } + + // Bật heartbeat SAU khi cấu hình PDO: PdoManager gửi NMT Reset Communication, + // lệnh này reset vùng object 0x1000-0x1FFF về mặc định (0x1017 = 0) nên nếu ghi + // trước thì heartbeat producer bị xóa và node không bao giờ phát heartbeat. + if (_useHeartbeatCheck) + { + await EnableHeartbeatAsync(cancellationToken); + } + + await _canOpenDevice.StartNodeAsync(cancellationToken); + await VerifyOperationalAsync(cancellationToken); + + await ConfigureDriveParametersAsync(cancellationToken); + + bool rpdoReady = _canOpenDevice.Pdo.IsRpdoConfigured(DualVelocityRpdoNumber); + _logger.LogInformation( + "ZLAC8015D Node {NodeId} axis {Axis}: lệnh vận tốc đồng bộ qua RPDO1 = {State}. {Detail}", + _nodeId, _axis, rpdoReady ? "BẬT" : "TẮT", + rpdoReady + ? "Hai bánh nhận lệnh trong cùng một khung CAN." + : "Firmware không nhận RPDO -> fallback ghi SDO tuần tự (có thể lệch nhẹ)."); + } + finally + { + _canOpenDevice.ConfigurationLock.Release(); + } + + StartFeedbackPolling(); + UpdateProperties(); + } + + /// + /// Ghi 0x1017 để drive tự phát heartbeat, bật monitor phía consumer rồi chờ heartbeat đầu tiên về. + /// Nếu drive không phát (firmware không hỗ trợ 0x1017, hoặc bị Reset Communication xóa) thì + /// tắt heartbeat check và fallback sang đọc statusword — nếu không, IsAlive không bao giờ true + /// và device sẽ fail connect vĩnh viễn. + /// + private async Task EnableHeartbeatAsync(CancellationToken ct) + { + try + { + await _canOpenDevice!.WriteUInt16Async(ObjProducerHeartbeatTime, 0, HeartbeatProducerTimeMs, ct); + _canOpenDevice.AutoVerifyStateViaHeartbeat = true; + _canOpenDevice.EnableHeartbeatMonitoring(timeoutMs: HeartbeatConsumerTimeoutMs); + + if (await WaitForFirstHeartbeatAsync(ct)) + { + _heartbeatCheckActive = true; + return; + } + + _heartbeatCheckActive = false; + _canOpenDevice.AutoVerifyStateViaHeartbeat = false; + _canOpenDevice.DisableHeartbeatMonitoring(); + _logger.LogWarning( + "ZLAC8015D Node {NodeId}: đã ghi 0x1017 = {TimeMs}ms nhưng không nhận được heartbeat sau {WaitMs}ms. " + + "Tắt kiểm tra heartbeat, fallback đọc statusword.", + _nodeId, HeartbeatProducerTimeMs, HeartbeatStartupWaitMs); + } + catch (Exception ex) + { + _heartbeatCheckActive = false; + _logger.LogWarning(ex, + "ZLAC8015D Node {NodeId}: không ghi được 0x1017 (producer heartbeat = {TimeMs}ms), tắt kiểm tra heartbeat, fallback đọc statusword", + _nodeId, HeartbeatProducerTimeMs); + } + } + + /// + /// Xác nhận node thật sự vào Operational sau NMT Start. TPDO CHỈ được phát ở Operational, + /// nên nếu lệnh Start bị bỏ qua thì mọi cấu hình TPDO đúng đến đâu cũng vô nghĩa. + /// Trạng thái NMT lấy từ heartbeat (byte dữ liệu của frame 0x700+NodeId). + /// + private async Task VerifyOperationalAsync(CancellationToken ct) + { + if (!_heartbeatCheckActive) + return; // Không có heartbeat thì không quan sát được state, bỏ qua + + const int maxWaitMs = 1500; + int waited = 0; + + while (waited < maxWaitMs) + { + await Task.Delay(100, ct); + waited += 100; + + var nodeInfo = _canOpenDevice!.Heartbeat.GetNodeInfo(_nodeId); + if (nodeInfo?.LastState == NmtState.Operational) + { + _logger.LogInformation("ZLAC8015D Node {NodeId} axis {Axis}: node đã vào Operational, TPDO sẽ bắt đầu phát.", + _nodeId, _axis); + return; + } + } + + var lastState = _canOpenDevice!.Heartbeat.GetNodeInfo(_nodeId)?.LastState; + _logger.LogWarning( + "ZLAC8015D Node {NodeId} axis {Axis}: sau NMT Start, heartbeat vẫn báo trạng thái {State} (không phải Operational) sau {MaxWaitMs}ms. " + + "TPDO sẽ không phát — feedback sẽ chạy bằng fallback SDO.", + _nodeId, _axis, lastState?.ToString() ?? "không rõ", maxWaitMs); + } + + private async Task WaitForFirstHeartbeatAsync(CancellationToken ct) + { + int waited = 0; + while (waited < HeartbeatStartupWaitMs) + { + await Task.Delay(50, ct); + waited += 50; + + var nodeInfo = _canOpenDevice!.Heartbeat.GetNodeInfo(_nodeId); + if (nodeInfo != null && nodeInfo.IsAlive) + return true; + } + + return false; + } + + /// + /// Ghi các tham số vận hành của ZLAC8015D sau khi node Operational: + /// chế độ điều khiển bất đồng bộ, độ phân giải tốc độ, thời gian tăng/giảm tốc. + /// Hai instance trái/phải ghi trùng các object chung (0x200F, 0x2026) — idempotent, không sao. + /// + private async Task ConfigureDriveParametersAsync(CancellationToken ct) + { + if (_canOpenDevice == null) return; + + // 0x200F = 0: asynchronous control (mỗi bánh nhận lệnh độc lập, không chờ frame sync) + await _canOpenDevice.WriteUInt16Async(ObjSyncControlFlag, 0, 0, ct); + + // 0x2026:06 - độ phân giải tốc độ đặt (1/N r/min). Firmware cũ có thể không hỗ trợ. + // QUAN TRỌNG: object này là CHUNG cho cả node nhưng _speedResolution là bản riêng của + // từng instance trái/phải. Nếu một instance ghi lỗi mà tự đoán fallback = 1 trong khi + // instance kia đã ghi 10 thành công, thì mọi lệnh SDO của trục lệch sẽ nhỏ hơn 10 lần + // -> một bánh luôn chạy chậm hơn bánh kia. Vì vậy luôn ĐỌC LẠI giá trị thật từ drive + // để hai instance và drive thống nhất một độ phân giải. + try + { + await _canOpenDevice.WriteUInt16Async(ObjMotorParams, SubSpeedResolution, (ushort)_configuredSpeedResolution, ct); + _speedResolution = _configuredSpeedResolution; + } + catch (Exception ex) + { + _logger.LogWarning(ex, + "ZLAC8015D Node {NodeId} axis {Axis}: không ghi được 0x2026:06 (speed resolution = {Resolution}), sẽ đọc lại giá trị thật từ drive", + _nodeId, _axis, _configuredSpeedResolution); + } + + try + { + ushort actual = await _canOpenDevice.ReadUInt16Async(ObjMotorParams, SubSpeedResolution, ct); + if (actual >= 1 && actual <= 10) + { + if (actual != _speedResolution) + { + _logger.LogWarning( + "ZLAC8015D Node {NodeId} axis {Axis}: 0x2026:06 trên drive = {Actual} khác giá trị dự kiến {Expected}, dùng giá trị của drive", + _nodeId, _axis, actual, _speedResolution); + } + _speedResolution = actual; + } + } + catch (Exception ex) + { + // Không đọc được (firmware không hỗ trợ 0x2026:06) -> chắc chắn không ghi được, + // drive chạy đơn vị mặc định 1 r/min cho cả hai trục nên fallback 1 là nhất quán. + _speedResolution = 1; + _logger.LogWarning(ex, + "ZLAC8015D Node {NodeId} axis {Axis}: không đọc được 0x2026:06, fallback đơn vị lệnh 1 r/min", + _nodeId, _axis); + } + + _logger.LogInformation("ZLAC8015D Node {NodeId} axis {Axis}: speed resolution hiệu lực = 1/{Resolution} r/min", + _nodeId, _axis, _speedResolution); + + // Thời gian tăng/giảm tốc (ms). Ghi cho CẢ HAI trục chứ không chỉ trục mình: khi + // instance kia connect SAU, bước cấu hình PDO của nó gửi NMT Reset Communication làm + // drive khôi phục mặc định (đo thực tế: trục đã ghi 200ms bị trả về 10ms) -> hai bánh + // tăng/giảm tốc lệch nhau 20 lần, một bánh luôn "ì" hơn ở pha quá độ. Hai instance + // dùng chung AccelTimeMs/DecelTimeMs trong config nên ghi trùng là vô hại. + for (byte sub = 1; sub <= 2; sub++) + { + await _canOpenDevice.WriteUInt32Async(ObjProfileAcceleration, sub, _accelTimeMs, ct); + await _canOpenDevice.WriteUInt32Async(ObjProfileDeceleration, sub, _decelTimeMs, ct); + } + } + + protected override async Task OnDisconnectAsync(CancellationToken cancellationToken) + { + StopFeedbackPolling(); + + if (_canOpenDevice != null) + { + // Dừng bánh của trục này trước khi ngắt (node CAN dùng chung với trục còn lại nên không stop node ở đây) + try + { + await _canOpenDevice.WriteInt32Async(ObjTargetVelocity, _axisSub, 0, cancellationToken); + } + catch + { + // Ignore - bus có thể đã mất + } + _canOpenDevice.DisableHeartbeatMonitoring(); + } + } + + protected override async Task OnResetAsync(CancellationToken cancellationToken) + { + 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) + { + _logger.LogWarning(ex, "Error occurred during reset for Node {NodeId} axis {Axis}", _nodeId, _axis); + } + + UpdateProperties(); + } + + protected override async Task OnCheckConnectionAsync(CancellationToken cancellationToken) + { + if (_canOpenDevice == null) + return false; + + try + { + if (_heartbeatCheckActive) + { + var nodeInfo = _canOpenDevice.Heartbeat.GetNodeInfo(_nodeId); + if (nodeInfo == null || !nodeInfo.IsAlive) + { + return false; + } + } + + await GetStatuswordAsync(cancellationToken); + return true; + } + catch (Exception ex) + { + _logger.LogError(ex, "Error checking connection for Node {NodeId} axis {Axis}", _nodeId, _axis); + return false; + } + } + + #region PDO Configuration + + /// + /// Đăng ký TPDO feedback cho trục này lên CanOpenDevice dùng chung. + /// ZLAC8015D chỉ có 3 TPDO (0x1A03 không tồn tại trên firmware). Firmware phát TPDO + /// theo THAY ĐỔI dữ liệu chứ không theo event timer (đo thực tế trên bus), nên mỗi TPDO + /// chỉ chở một loại dữ liệu và object hay thay đổi phải đứng đầu: + /// TPDO1 = position CẢ HAI trục (0x6064:01 + 0x6064:02) = 8 byte + /// TPDO2 = statusword (0x6041:00) = 2 byte + /// TPDO3 = velocity CẢ HAI trục (0x606C:01 + 0x606C:02) = 8 byte + /// (layout cũ TPDO1/2 = statusword + position từng trục KHÔNG BAO GIỜ phát vì object + /// đầu tiên - statusword - hầu như không đổi.) + /// Hai instance cùng đăng ký mapping GIỐNG HỆT nhau (idempotent), mỗi instance chỉ đọc + /// phần bit của trục mình: trái bit 0-31, phải bit 32-63. + /// + private void ConfigurePdos() + { + if (_canOpenDevice == null) + return; + + _pdoMappingRegistry.Clear(); + + // TPDO1 position: 0x6064:01 (bit 0-31) + 0x6064:02 (bit 32-63), counts + var positionTpdo = new PdoConfiguration(PositionTpdoNumber, (uint)(0x180 + (PositionTpdoNumber - 1) * 0x100 + _nodeId)) + { + EventTimer = 50, + InhibitTime = TpdoInhibitTime100Us + }; + positionTpdo.AddMapping(new PdoMapping(ObjPositionActual, 1, 32)); + positionTpdo.AddMapping(new PdoMapping(ObjPositionActual, 2, 32)); + _canOpenDevice.Pdo.ConfigureTPDO(positionTpdo); + + _pdoMappingRegistry[(ObjPositionActual, _axisSub)] = new PdoMappingInfo + { + IsRPDO = false, + PdoNumber = PositionTpdoNumber, + BitOffset = (_axisSub - 1) * 32, + BitLength = 32, + ObjectIndex = ObjPositionActual, + SubIndex = _axisSub + }; + + // TPDO2 statusword: 0x6041:00 (16 bit, chung 2 trục). Statusword ít đổi nên TPDO này + // hiếm khi phát — nhưng khi đổi (vd fault) sẽ phát ngay lập tức; giữa các lần đổi, + // poll SDO nền vẫn làm tươi định kỳ. + var statuswordTpdo = new PdoConfiguration(StatuswordTpdoNumber, (uint)(0x180 + (StatuswordTpdoNumber - 1) * 0x100 + _nodeId)) + { + EventTimer = 50, + InhibitTime = TpdoInhibitTime100Us + }; + statuswordTpdo.AddMapping(new PdoMapping(ObjStatusword, 0, 16)); + _canOpenDevice.Pdo.ConfigureTPDO(statuswordTpdo); + + _pdoMappingRegistry[(ObjStatusword, (byte)0)] = new PdoMappingInfo + { + IsRPDO = false, + PdoNumber = StatuswordTpdoNumber, + BitOffset = 0, + BitLength = 16, + ObjectIndex = ObjStatusword, + SubIndex = 0 + }; + + // TPDO3 velocity: 0x606C:01 (bit 0-31) + 0x606C:02 (bit 32-63), đơn vị 0.1 r/min. + // InhibitTime 50ms để firmware không phát theo từng thay đổi (đo được 240 frame/s khi chạy). + var velocityTpdo = new PdoConfiguration(VelocityTpdoNumber, (uint)(0x180 + (VelocityTpdoNumber - 1) * 0x100 + _nodeId)) + { + EventTimer = 50, + InhibitTime = TpdoInhibitTime100Us + }; + velocityTpdo.AddMapping(new PdoMapping(ObjVelocityActual, 1, 32)); + velocityTpdo.AddMapping(new PdoMapping(ObjVelocityActual, 2, 32)); + _canOpenDevice.Pdo.ConfigureTPDO(velocityTpdo); + + _pdoMappingRegistry[(ObjVelocityActual, _axisSub)] = new PdoMappingInfo + { + IsRPDO = false, + PdoNumber = VelocityTpdoNumber, + BitOffset = (_axisSub - 1) * 32, + BitLength = 32, + ObjectIndex = ObjVelocityActual, + SubIndex = _axisSub + }; + + // RPDO1 lệnh vận tốc đồng bộ: 0x60FF:01 (bit 0-31) + 0x60FF:02 (bit 32-63). + // Cả hai instance đăng ký y hệt nhau (idempotent). Nếu firmware không nhận RPDO + // này thì TrySetBothTargetVelocitiesAsync trả false và tự fallback về SDO tuần tự. + var dualVelocityRpdo = new PdoConfiguration(DualVelocityRpdoNumber, (uint)(0x200 + _nodeId)); + dualVelocityRpdo.AddMapping(new PdoMapping(ObjTargetVelocity, 1, 32)); + dualVelocityRpdo.AddMapping(new PdoMapping(ObjTargetVelocity, 2, 32)); + _canOpenDevice.Pdo.ConfigureRPDO(dualVelocityRpdo); + + // Tính sẵn kế hoạch parse cho OnPdoReceived. RequiredBytes theo bit CUỐI CÙNG cần đọc, + // không phải tổng độ dài: trục phải đọc velocity ở bit 32-63 của TPDO3 nên cần đủ + // 8 byte dù chỉ map 32 bit. + _tpdoParsePlan = _pdoMappingRegistry.Values + .Where(m => !m.IsRPDO) + .GroupBy(m => m.PdoNumber) + .ToDictionary( + g => g.Key, + g => + { + var mappings = g.OrderBy(m => m.BitOffset).ToArray(); + int lastBit = mappings.Max(m => m.BitOffset + m.BitLength); + return (mappings, (lastBit + 7) / 8); + }); + } + + /// + /// Ghi PDO configuration xuống device qua SDO (retry giống driver PhenikaaX) + /// + private async Task ConfigureDevicePdosAsync(CancellationToken ct) + { + Exception? lastException = null; + for (int attempt = 1; attempt <= _pdoConfigRetryCount; attempt++) + { + try + { + await EnsurePreOperationalStateAsync(ct); + await _canOpenDevice!.WriteAllPdoConfigurationsToDeviceAsync(null, ct); + return; + } + catch (CanOpenTimeoutException timeoutEx) + { + lastException = timeoutEx; + + _logger.LogWarning("PDO configuration attempt {Attempt}/{MaxRetries} failed for Node {NodeId} axis {Axis}: {Error}", + attempt, _pdoConfigRetryCount, _nodeId, _axis, 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 + } + } + } + } + + throw new InvalidOperationException( + $"Failed to configure PDOs after {_pdoConfigRetryCount} attempts for Node {_nodeId} axis {_axis}. " + + $"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); + } + } + + #endregion + + #region PDO Receive + + private void OnPdoReceived(object? sender, PdoReceivedEventArgs e) + { + var data = e.Data; + + // Chỉ parse TPDO thuộc trục này (instance của trục kia tự parse TPDO của nó) + if (!_tpdoParsePlan.TryGetValue(data.PdoNumber, out var plan)) + return; + + if (data.Data.Length < plan.RequiredBytes) + { + _logger.LogWarning("TPDO{Number} data length ({Length}) is less than expected ({Expected})", + data.PdoNumber, data.Data.Length, plan.RequiredBytes); + return; + } + + // Cập nhật cache trong lock, nhưng bắn event NGOÀI lock: handler này chạy đồng bộ + // trên thread nhận frame CAN, subscriber chậm (UI/odometry) mà chạy trong lock sẽ + // chặn cả đường nhận CAN (SDO response, heartbeat, TPDO trục kia) -> trễ dây chuyền + // cả điều khiển lẫn hiển thị. + StatuswordChangedEventArgs? statuswordArgs = null; + PositionChangedEventArgs? positionArgs = null; + VelocityChangedEventArgs? velocityArgs = null; + ushort oldStatuswordValue = 0; + + lock (_lock) + { + foreach (var mappingInfo in plan.Mappings) + { + try + { + byte[] valueBytes = CiA402Helper.ExtractValueFromPdoData(data.Data, mappingInfo.BitOffset, mappingInfo.BitLength); + + switch (mappingInfo.ObjectIndex) + { + case ObjStatusword when valueBytes.Length >= 2: + var newStatusword = new Statusword(BitConverter.ToUInt16(valueBytes, 0)); + _statuswordUpdatedAt = DateTime.UtcNow; + + if (newStatusword.Value != _cachedStatusword.Value) + { + oldStatuswordValue = _cachedStatusword.Value; + statuswordArgs = new StatuswordChangedEventArgs( + newStatusword, _cachedStatusword.GetState(), newStatusword.GetState()); + _cachedStatusword = newStatusword; + } + break; + + case ObjPositionActual when valueBytes.Length >= 4: + var position = BitConverter.ToInt32(valueBytes, 0); + _positionUpdatedAt = DateTime.UtcNow; + + if (position != _cachedPosition) + { + positionArgs = new PositionChangedEventArgs(position, _cachedPosition); + _cachedPosition = position; + } + break; + + case ObjVelocityActual when valueBytes.Length >= 4: + var velocity = FeedbackRawToCountsPerSec(BitConverter.ToInt32(valueBytes, 0)); + _velocityUpdatedAt = DateTime.UtcNow; + + if (velocity != _cachedVelocity) + { + velocityArgs = new VelocityChangedEventArgs(velocity, _cachedVelocity); + _cachedVelocity = velocity; + } + break; + } + } + catch (Exception ex) + { + _logger.LogError(ex, "Error processing TPDO{Number} mapping for object 0x{Index:X4}:{SubIndex}", + mappingInfo.PdoNumber, mappingInfo.ObjectIndex, mappingInfo.SubIndex); + } + } + } + + if (statuswordArgs != null) + { + if (statuswordArgs.NewState != statuswordArgs.OldState) + { + _logger.LogInformation( + "ZLAC8015D Node {NodeId} axis {Axis}: DriveState {OldState} -> {NewState} (statusword 0x{Old:X4} -> 0x{New:X4})", + _nodeId, _axis, statuswordArgs.OldState, statuswordArgs.NewState, + oldStatuswordValue, statuswordArgs.Statusword.Value); + } + + StatuswordChanged?.Invoke(this, statuswordArgs); + } + + if (positionArgs != null) + PositionChanged?.Invoke(this, positionArgs); + + if (velocityArgs != null) + VelocityChanged?.Invoke(this, velocityArgs); + + // Chỉ tốn công cập nhật property khi thực sự có giá trị thay đổi + if (statuswordArgs != null || positionArgs != null || velocityArgs != null) + UpdateProperties(); + } + + #endregion + + #region Unit Conversion + + /// + /// counts/s (đơn vị interface) -> giá trị lệnh 0x60FF (1/SpeedResolution r/min), kẹp ±1000 r/min + /// + private int CountsPerSecToCommandRaw(int countsPerSec) + { + double rpm = (double)countsPerSec * 60.0 / _countsPerRevolution; + double clamped = Math.Clamp(rpm, -MaxRpm, MaxRpm); + return (int)Math.Round(clamped * _speedResolution); + } + + /// + /// Giá trị feedback 0x606C (0.1 r/min) -> counts/s (đơn vị interface) + /// + private int FeedbackRawToCountsPerSec(int feedbackRaw) + { + return (int)Math.Round(feedbackRaw * FeedbackUnitRpm / 60.0 * _countsPerRevolution); + } + + /// + /// counts/s -> r/min nguyên (dùng cho 0x6081 profile velocity, range 1-1000) + /// + private uint CountsPerSecToRpm(uint countsPerSec) + { + double rpm = (double)countsPerSec * 60.0 / _countsPerRevolution; + return (uint)Math.Clamp(Math.Round(rpm), 1, MaxRpm); + } + + #endregion + + private void UpdateProperties() + { + try + { + lock (_lock) + { + SetProperty("Statusword", $"0x{_cachedStatusword.Value:X4}"); + SetProperty("Position", _cachedPosition.ToString()); + SetProperty("Velocity", _cachedVelocity.ToString()); + + var state = _cachedStatusword.GetState(); + SetProperty("DriveState", state.ToString()); + } + } + catch + { + // Ignore errors + } + } + + private CanOpenDevice RequireDevice() + { + return _canOpenDevice ?? throw new InvalidOperationException("Device not initialized"); + } + + #region ICiA402Servo Implementation + + 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 (0x6040/0x6041 sub 0 - chung cho cả 2 bánh) + public async Task GetStatuswordAsync(CancellationToken ct = default) + { + var device = RequireDevice(); + + // Chỉ dùng cache khi TPDO vừa cập nhật trong TTL. Trước đây cache được dùng + // mãi mãi khi != 0, nên nếu TPDO ngừng về thì GetStateAsync trả giá trị cũ + // vĩnh viễn và EnableAsync không bao giờ thấy trạng thái tiến triển. + lock (_lock) + { + if (DateTime.UtcNow - _statuswordUpdatedAt < TpdoCacheTtl) + return _cachedStatusword; + } + + return await ReadStatuswordViaSdoAsync(device, ct); + } + + /// + /// Đọc statusword trực tiếp qua SDO, bỏ qua cache TPDO. State machine dùng hàm này + /// để mỗi bước chuyển trạng thái đều thấy giá trị mới nhất từ drive. + /// + private async Task ReadStatuswordViaSdoAsync(CanOpenDevice device, CancellationToken ct) + { + var value = await device.ReadUInt16Async(ObjStatusword, 0, ct); + var statusword = new Statusword(value); + + bool changed; + Statusword previous; + lock (_lock) + { + previous = _cachedStatusword; + changed = statusword.Value != previous.Value; + _cachedStatusword = statusword; + // Không cập nhật _statuswordUpdatedAt: mốc đó chỉ dành cho TPDO, + // để lần đọc sau vẫn lấy giá trị tươi nếu TPDO chưa hoạt động. + } + + if (changed) + { + var oldState = previous.GetState(); + var newState = statusword.GetState(); + + if (newState != oldState) + { + _logger.LogInformation( + "ZLAC8015D Node {NodeId} axis {Axis}: DriveState {OldState} -> {NewState} (statusword 0x{Old:X4} -> 0x{New:X4}, đọc qua SDO)", + _nodeId, _axis, oldState, newState, previous.Value, statusword.Value); + } + + StatuswordChanged?.Invoke(this, new StatuswordChangedEventArgs(statusword, oldState, newState)); + } + + return statusword; + } + + private async Task ReadStateViaSdoAsync(CancellationToken ct) + { + var statusword = await ReadStatuswordViaSdoAsync(RequireDevice(), ct); + return statusword.GetState(); + } + + public async Task SetControlwordAsync(Controlword controlword, CancellationToken ct = default) + { + await RequireDevice().WriteUInt16Async(ObjControlword, 0, controlword.Value, ct); + } + + public async Task GetStateAsync(CancellationToken ct = default) + { + var statusword = await GetStatuswordAsync(ct); + return statusword.GetState(); + } + + // Operation Mode (0x6060 sub 0 - chung cho cả 2 bánh) + public async Task SetOperationModeAsync(OperationMode mode, CancellationToken ct = default) + { + await RequireDevice().WriteUInt8Async(ObjModesOfOperation, 0, (byte)mode, ct); + } + + public async Task GetOperationModeAsync(CancellationToken ct = default) + { + var value = await RequireDevice().ReadUInt8Async(ObjModesOfOperationDisplay, 0, ct); + return (OperationMode)(sbyte)value; + } + + // State Machine Control (chung 2 bánh - enable một trục là enable cả drive) + 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); + } + + /// + /// Đưa drive lên OperationEnabled theo state machine CiA402. + /// Chạy vòng lặp cho tới khi đạt trạng thái hoặc hết thời gian, mỗi vòng đọc statusword + /// tươi qua SDO (không tin cache TPDO), và ném exception kèm statusword nếu thất bại — + /// bản cũ chạy một lượt tuyến tính rồi im lặng bỏ cuộc nếu drive chưa kịp chuyển trạng thái. + /// + public async Task EnableAsync(CancellationToken ct = default) + { + var deadline = DateTime.UtcNow + TimeSpan.FromMilliseconds(EnableTimeoutMs); + DriveState state = DriveState.Unknown; + + while (DateTime.UtcNow < deadline) + { + state = await ReadStateViaSdoAsync(ct); + + switch (state) + { + case DriveState.OperationEnabled: + return; + + case DriveState.NotReadyToSwitchOn: + // Trạng thái tự động của CiA402: drive đang tự khởi tạo, KHÔNG nhận lệnh + // controlword nào. Chỉ được chờ nó tự chuyển sang SwitchOnDisabled. + await Task.Delay(StateStepDelayMs, ct); + break; + + case DriveState.Fault: + await FaultResetAsync(ct); + await Task.Delay(StateStepDelayMs, ct); + break; + + case DriveState.SwitchOnDisabled: + await ShutdownAsync(ct); // 0x0006 -> ReadyToSwitchOn + await Task.Delay(StateStepDelayMs, ct); + break; + + case DriveState.ReadyToSwitchOn: + await SwitchOnAsync(ct); // 0x0007 -> SwitchedOn + await Task.Delay(StateStepDelayMs, ct); + break; + + case DriveState.SwitchedOn: + await EnableOperationAsync(ct); // 0x000F -> OperationEnabled + await Task.Delay(StateStepDelayMs, ct); + break; + + default: + await Task.Delay(StateStepDelayMs, ct); + break; + } + } + + var statusword = await ReadStatuswordViaSdoAsync(RequireDevice(), ct); + throw new InvalidOperationException( + $"ZLAC8015D Node {_nodeId} axis {_axis}: không đạt được OperationEnabled sau {EnableTimeoutMs}ms. " + + $"Trạng thái cuối: {state}, statusword = 0x{statusword.Value:X4}. " + + (statusword.Value == 0 + ? "Statusword = 0 nghĩa là drive không trả về trạng thái hợp lệ — kiểm tra nguồn động lực (motor power) và dây encoder." + : "Kiểm tra fault code và nguồn động lực của drive.")); + } + + public async Task DisableAsync(CancellationToken ct = default) + { + // Đưa tốc độ trục này về 0 trước; controlword là chung nên chỉ disable + // khi caller thực sự muốn tắt cả drive (DifferentialDrive luôn disable cả 2 bánh) + try + { + await RequireDevice().WriteInt32Async(ObjTargetVelocity, _axisSub, 0, ct); + } + catch + { + // Ignore - vẫn tiếp tục disable + } + await DisableOperationAsync(ct); + } + + // Position Control (0x607A sub theo trục, counts) + /// + /// Vị trí thực tế (counts). Ưu tiên cache TPDO khi còn tươi, ngược lại đọc SDO 0x6064:sub. + /// Trước đây hàm này CHỈ trả cache TPDO nên khi TPDO không về (mapping lỗi) thì + /// Position đứng yên ở 0 mãi mãi dù động cơ vẫn quay. + /// + public async Task GetActualPositionAsync(CancellationToken ct = default) + { + var device = RequireDevice(); + + lock (_lock) + { + if (DateTime.UtcNow - _positionUpdatedAt < TpdoCacheTtl) + return _cachedPosition; + } + + var position = await device.ReadInt32Async(ObjPositionActual, _axisSub, ct); + + bool changed; + int oldPosition; + lock (_lock) + { + oldPosition = _cachedPosition; + changed = position != _cachedPosition; + _cachedPosition = position; + } + + if (changed) + { + PositionChanged?.Invoke(this, new PositionChangedEventArgs(position, oldPosition)); + UpdateProperties(); + } + + return position; + } + + public async Task SetTargetPositionAsync(int position, CancellationToken ct = default) + { + lock (_lock) + { + _targetPosition = position; + } + + await RequireDevice().WriteInt32Async(ObjTargetPosition, _axisSub, position, ct); + } + + public async Task SetProfileSpeedAsync(uint velocity, CancellationToken ct = default) + { + // 0x6081 sub theo trục: max speed trong position mode, đơn vị r/min (1-1000) + await RequireDevice().WriteUInt32Async(ObjProfileVelocity, _axisSub, CountsPerSecToRpm(velocity), ct); + } + + public async Task SetProfileVelocityAsync(uint velocity, CancellationToken ct = default) + { + // Giống driver PhenikaaX: profile velocity ghi vào target velocity (0x60FF) + await RequireDevice().WriteInt32Async(ObjTargetVelocity, _axisSub, CountsPerSecToCommandRaw((int)velocity), ct); + } + + public async Task SetProfileAccelerationAsync(uint acceleration, CancellationToken ct = default) + { + // ZLAC dùng thời gian tăng tốc (ms), không quy đổi được từ counts/s² + // -> luôn ghi giá trị AccelTimeMs từ config, bỏ qua tham số + _logger.LogTrace("ZLAC8015D axis {Axis}: SetProfileAcceleration({Value}) -> ghi AccelTimeMs={AccelMs}ms từ config", + _axis, acceleration, _accelTimeMs); + await RequireDevice().WriteUInt32Async(ObjProfileAcceleration, _axisSub, _accelTimeMs, ct); + } + + public async Task SetProfileDecelerationAsync(uint deceleration, CancellationToken ct = default) + { + _logger.LogTrace("ZLAC8015D axis {Axis}: SetProfileDeceleration({Value}) -> ghi DecelTimeMs={DecelMs}ms từ config", + _axis, deceleration, _decelTimeMs); + await RequireDevice().WriteUInt32Async(ObjProfileDeceleration, _axisSub, _decelTimeMs, ct); + } + + public async Task GetProfileSpeedAsync(CancellationToken ct = default) + { + return await RequireDevice().ReadUInt32Async(ObjProfileVelocity, _axisSub, ct); + } + + public async Task GetProfileAccelerationAsync(CancellationToken ct = default) + { + return await RequireDevice().ReadUInt32Async(ObjProfileAcceleration, _axisSub, ct); + } + + public async Task GetProfileDecelerationAsync(CancellationToken ct = default) + { + return await RequireDevice().ReadUInt32Async(ObjProfileDeceleration, _axisSub, 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 = 1000, uint acceleration = 5000, uint deceleration = 5000, CancellationToken ct = default) + { + _logger.LogInformation("Zlac8015dServo[{DeviceId}] axis {Axis}: MoveToPositionAsync position={Position}, velocity={Velocity}", + DeviceId, _axis, 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); + await SetTargetPositionAsync(position, ct); + await Task.Delay(150, ct); + await StartPositionMoveAsync(ct); + } + + // Velocity Control (0x60FF sub theo trục) + /// + /// Vận tốc thực tế (counts/s). Ưu tiên cache TPDO khi còn tươi, ngược lại đọc SDO 0x606C:sub. + /// + public async Task GetActualVelocityAsync(CancellationToken ct = default) + { + var device = RequireDevice(); + + lock (_lock) + { + if (DateTime.UtcNow - _velocityUpdatedAt < TpdoCacheTtl) + return _cachedVelocity; + } + + var raw = await device.ReadInt32Async(ObjVelocityActual, _axisSub, ct); + var velocity = FeedbackRawToCountsPerSec(raw); + + bool changed; + int oldVelocity; + lock (_lock) + { + oldVelocity = _cachedVelocity; + changed = velocity != _cachedVelocity; + _cachedVelocity = velocity; + } + + if (changed) + { + VelocityChanged?.Invoke(this, new VelocityChangedEventArgs(velocity, oldVelocity)); + UpdateProperties(); + } + + return velocity; + } + + public async Task SetTargetVelocityAsync(int velocity, CancellationToken ct = default) + { + await RequireDevice().WriteInt32Async(ObjTargetVelocity, _axisSub, CountsPerSecToCommandRaw(velocity), ct); + } + + /// + /// Gửi vận tốc CẢ HAI trục trong MỘT khung RPDO để hai bánh nhận lệnh đồng thời. + /// Vì 0x60FF:01 và 0x60FF:02 dùng chung một node vật lý, hai lệnh SDO tuần tự sẽ tới + /// lệch nhau (nặng hơn khi feedback poll đang chiếm kênh SDO), gây hai bánh chạy lệch. + /// Nếu RPDO không có, fallback ghi SDO THEO LÔ (giữ kênh SDO liên tục cho cả hai lệnh): + /// độ lệch giữa hai bánh chỉ còn đúng một vòng SDO (~ms), không bị poll feedback chen + /// vào giữa như khi caller tự ghi hai lệnh SetTargetVelocityAsync riêng lẻ. + /// Trả false chỉ khi cả hai đường đều thất bại -> caller fallback về SDO tuần tự. + /// Đơn vị leftCountsPerSec/rightCountsPerSec giống SetTargetVelocityAsync (đã gồm IsReversed). + /// + public async Task TrySetBothTargetVelocitiesAsync(int leftCountsPerSec, int rightCountsPerSec, CancellationToken ct = default) + { + var device = RequireDevice(); + + // Conversion counts/s -> raw là như nhau cho cả hai trục (cùng countsPerRevolution + // và speedResolution), nên tính cả hai từ một instance là chính xác. + int leftRaw = CountsPerSecToCommandRaw(leftCountsPerSec); + int rightRaw = CountsPerSecToCommandRaw(rightCountsPerSec); + + if (device.Pdo.IsRpdoConfigured(DualVelocityRpdoNumber)) + { + var frame = new byte[8]; + BitConverter.TryWriteBytes(frame.AsSpan(0, 4), leftRaw); // 0x60FF:01 + BitConverter.TryWriteBytes(frame.AsSpan(4, 4), rightRaw); // 0x60FF:02 + + try + { + await device.Pdo.SendRPDOAsync(DualVelocityRpdoNumber, frame, ct); + return true; + } + catch (Exception ex) + { + _logger.LogWarning(ex, "ZLAC8015D Node {NodeId}: gửi RPDO vận tốc gộp thất bại, thử fallback SDO theo lô", _nodeId); + } + } + + try + { + await device.WriteInt32BatchAsync( + [ + (ObjTargetVelocity, (byte)1, leftRaw), + (ObjTargetVelocity, (byte)2, rightRaw) + ], ct); + return true; + } + catch (Exception ex) + { + _logger.LogWarning(ex, "ZLAC8015D Node {NodeId}: ghi SDO theo lô vận tốc hai trục thất bại", _nodeId); + return false; + } + } + + 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 (0x6071 sub theo trục, đơn vị mA) + public async Task GetActualTorqueAsync(CancellationToken ct = default) + { + var value = await RequireDevice().ReadInt16Async(ObjTargetTorque, _axisSub, ct); + lock (_lock) + { + _cachedTorque = value; + return _cachedTorque; + } + } + + public async Task SetTargetTorqueAsync(short torque, CancellationToken ct = default) + { + await RequireDevice().WriteInt16Async(ObjTargetTorque, _axisSub, 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); + } + + // Homing - ZLAC8015D là drive bánh xe hub, không hỗ trợ homing + public Task SetHomingMethodAsync(byte method, CancellationToken ct = default) + => throw new NotSupportedException("ZLAC8015D không hỗ trợ homing"); + + public Task SetHomingSpeedAsync(int speed, CancellationToken ct = default) + => throw new NotSupportedException("ZLAC8015D không hỗ trợ homing"); + + public Task SetHomingOffsetAsync(int offset, CancellationToken ct = default) + => throw new NotSupportedException("ZLAC8015D không hỗ trợ homing"); + + public Task GetHomingMethodAsync(CancellationToken ct = default) + => throw new NotSupportedException("ZLAC8015D không hỗ trợ homing"); + + public Task GetHomingSpeedAsync(CancellationToken ct = default) + => throw new NotSupportedException("ZLAC8015D không hỗ trợ homing"); + + public Task GetHomingOffsetAsync(CancellationToken ct = default) + => throw new NotSupportedException("ZLAC8015D không hỗ trợ homing"); + + public Task StartHomingAsync(byte method, int speed, CancellationToken ct = default) + => throw new NotSupportedException("ZLAC8015D không hỗ trợ homing"); + + // 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 IsInFaultStateAsync(CancellationToken ct = default) + { + var state = await GetStateAsync(ct); + return state == DriveState.Fault; + } + + public async Task GetErrorRegisterAsync(CancellationToken ct = default) + { + try + { + return await RequireDevice().ReadUInt8Async(0x1001, 0, ct); + } + catch + { + return 0; + } + } + + public async Task GetErrorHistoryAsync(CancellationToken ct = default) + { + try + { + var device = RequireDevice(); + byte errorCount = await device.ReadUInt8Async(0x1003, 0, ct); + + if (errorCount == 0) return []; + + var errors = new List(); + for (byte i = 1; i <= errorCount && i <= 8; i++) + { + try + { + ushort errorCode = await device.ReadUInt16Async(0x1003, i, ct); + errors.Add(errorCode); + } + catch + { + break; + } + } + + return [.. errors]; + } + catch + { + return []; + } + } + + public async Task GetLatestErrorCodeAsync(CancellationToken ct = default) + { + var errorHistory = await GetErrorHistoryAsync(ct); + return errorHistory.Length > 0 ? errorHistory[0] : (ushort)0x0000; + } + + public async Task TryFaultResetAsync(CancellationToken ct = default) + { + var state = await GetStateAsync(ct); + if (state != DriveState.Fault) + return false; + + await FaultResetAsync(ct); + return true; + } + + public async Task IsEnabledAsync(CancellationToken ct = default) + { + var state = await GetStateAsync(ct); + return state == DriveState.OperationEnabled; + } + + public async Task IsReadyAsync(CancellationToken ct = default) + { + var state = await GetStateAsync(ct); + return state == DriveState.ReadyToSwitchOn || state == DriveState.SwitchedOn; + } + + #endregion + + #region Feedback polling fallback + + /// + /// Vòng poll nền làm tươi Position/Velocity qua SDO khi TPDO không về. + /// Cần thiết vì OdometryService và UI đọc CachedPosition/CachedVelocity một cách đồng bộ, + /// mà cache đó chỉ do TPDO ghi — TPDO hỏng thì odometry đứng yên mà không báo lỗi gì. + /// Nếu TPDO hoạt động, TTL còn tươi nên vòng lặp này không phát sinh SDO nào. + /// + private void StartFeedbackPolling() + { + StopFeedbackPolling(); + + _feedbackPollCts = new CancellationTokenSource(); + var ct = _feedbackPollCts.Token; + + _feedbackPollTask = Task.Run(async () => + { + // Xoay vòng statusword -> position -> velocity, mỗi tick đọc TỐI ĐA MỘT object. + // Kênh SDO là một hàng đợi tuần tự dùng chung cho cả node (2 trục): nếu mỗi tick + // đọc cả 3 object x 2 instance thì lệnh điều khiển (controlword, target velocity + // fallback SDO) phải xếp hàng sau feedback -> điều khiển bị trễ rõ rệt. + // Với tick 50ms, mỗi giá trị vẫn được làm tươi tối đa mỗi 150ms khi TPDO mất hẳn. + int pollPhase = 0; + + while (!ct.IsCancellationRequested) + { + try + { + await Task.Delay(FeedbackPollIntervalMs, ct); + + bool statuswordStale, positionStale, velocityStale; + lock (_lock) + { + var now = DateTime.UtcNow; + statuswordStale = now - _statuswordUpdatedAt >= TpdoCacheTtl; + positionStale = now - _positionUpdatedAt >= TpdoCacheTtl; + velocityStale = now - _velocityUpdatedAt >= TpdoCacheTtl; + } + + int selected = -1; + for (int i = 0; i < 3; i++) + { + int idx = (pollPhase + i) % 3; + bool isStale = idx switch { 0 => statuswordStale, 1 => positionStale, _ => velocityStale }; + if (isStale) + { + selected = idx; + pollPhase = idx + 1; + break; + } + } + + switch (selected) + { + // Statusword cũng phải được làm tươi, nếu không _cachedStatusword đứng + // mãi ở giá trị lần cuối (thường là OperationEnabled sau khi enable xong) + // và UI báo OperationEnabled vĩnh viễn kể cả khi drive đã fault. + case 0: + var before = CachedStatusword.Value; + var after = await ReadStatuswordViaSdoAsync(RequireDevice(), ct); + if (after.Value != before) + UpdateProperties(); + break; + + case 1: + await GetActualPositionAsync(ct); + break; + + case 2: + await GetActualVelocityAsync(ct); + break; + } + + // Báo cáo trung thực trạng thái TPDO: chỉ cảnh báo khi TPDO vắng LIÊN TỤC + // đủ lâu (không tính giai đoạn khởi động, lúc trục kia còn đang reset/cấu hình + // lại node), và báo lại khi TPDO hoạt động trở lại. + bool tpdoAlive = !positionStale; + + if (tpdoAlive) + { + _tpdoAbsentSinceUtc = null; + + if (_feedbackPollWarned) + { + _feedbackPollWarned = false; + _logger.LogInformation( + "ZLAC8015D Node {NodeId} axis {Axis}: TPDO feedback đã hoạt động trở lại, ngừng fallback SDO.", + _nodeId, _axis); + } + } + else + { + _tpdoAbsentSinceUtc ??= DateTime.UtcNow; + + if (!_feedbackPollWarned && + DateTime.UtcNow - _tpdoAbsentSinceUtc.Value >= TpdoAbsentGracePeriod) + { + _feedbackPollWarned = true; + _logger.LogWarning( + "ZLAC8015D Node {NodeId} axis {Axis}: không nhận được TPDO suốt {Seconds}s, đang fallback poll Statusword/Position/Velocity qua SDO mỗi {IntervalMs}ms. " + + "Kiểm tra node đã ở trạng thái Operational chưa.", + _nodeId, _axis, TpdoAbsentGracePeriod.TotalSeconds, FeedbackPollIntervalMs); + } + } + } + catch (OperationCanceledException) + { + return; + } + catch (Exception ex) + { + _logger.LogDebug(ex, "Feedback poll failed for Node {NodeId} axis {Axis}", _nodeId, _axis); + } + } + }, ct); + } + + private void StopFeedbackPolling() + { + if (_feedbackPollCts == null) + return; + + try + { + _feedbackPollCts.Cancel(); + _feedbackPollCts.Dispose(); + } + catch + { + // Ignore + } + + _feedbackPollCts = null; + _feedbackPollTask = null; + } + + #endregion + + protected override void Dispose(bool disposing) + { + if (disposing) + { + StopFeedbackPolling(); + + if (_canOpenDevice != null) + { + _canOpenDevice.PdoReceived -= OnPdoReceived; + } + } + base.Dispose(disposing); + } +} diff --git a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/ModelConfigs/ZLAC-test.json b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/ModelConfigs/ZLAC-test.json new file mode 100644 index 0000000..cd2c8cf --- /dev/null +++ b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/ModelConfigs/ZLAC-test.json @@ -0,0 +1,110 @@ +{ + "_Comment": "Config test cho ZLAC8015D: MỘT node CAN (can0, node 1, 500kbps) điều khiển CẢ HAI bánh. Hai device dưới đây là 2 facade trái/phải của cùng một drive - phải trỏ cùng CanInterface + NodeId, khác nhau ở Axis. WheelDiameter/CountsPerRevolution/PulsesPerRevolution cần đo lại theo bánh thực tế (mặc định: encoder ZLAC 4096 counts/vòng, bánh 8.5 inch ~0.17m).", + "Devices": { + "ZLAC_LeftWheel": { + "DeviceId": "left-wheel", + "Enabled": true, + "DeviceType": "CiA402Servo", + "DeviceName": "Left Wheel (ZLAC8015D)", + "DriverName": "ZLAC8015D", + "DriverVersion": "1.0.0", + "Description": "ZLAC8015D dual hub servo - truc trai", + "Connection": { + "CanInterface": "can0", + "NodeId": "1", + "Axis": "Left", + "CountsPerRevolution": 4096, + "SpeedResolution": 1, + "AccelTimeMs": 200, + "DecelTimeMs": 200, + "AutoReconnectEnabled": true, + "ReconnectDelayMs": 3000, + "MaxReconnectAttempts": 0, + "PdoConfigRetryCount": 3, + "PdoConfigRetryTimeoutMs": 1000, + "UseHeartbeatCheck": true + } + }, + "ZLAC_RightWheel": { + "DeviceId": "right-wheel", + "Enabled": true, + "DeviceType": "CiA402Servo", + "DeviceName": "Right Wheel (ZLAC8015D)", + "DriverName": "ZLAC8015D", + "DriverVersion": "1.0.0", + "Description": "ZLAC8015D dual hub servo - truc phai", + "Connection": { + "CanInterface": "can0", + "NodeId": "1", + "Axis": "Right", + "CountsPerRevolution": 4096, + "SpeedResolution": 1, + "AccelTimeMs": 200, + "DecelTimeMs": 200, + "AutoReconnectEnabled": true, + "ReconnectDelayMs": 3000, + "MaxReconnectAttempts": 0, + "PdoConfigRetryCount": 3, + "PdoConfigRetryTimeoutMs": 1000, + "UseHeartbeatCheck": true + } + } + }, + "Motion": { + "DifferentialDrive": { + "Enable": true, + "LeftWheel": { + "DeviceId": "left-wheel", + "Position": { + "Position": { + "X": 0.0, + "Y": 0.2, + "Z": 0.0 + }, + "Orientation": { + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067, + "W": 0.7071067 + } + }, + "WheelDiameter": 0.17, + "PulsesPerRevolution": 4096, + "IsReversed": false + }, + "RightWheel": { + "DeviceId": "right-wheel", + "Position": { + "Position": { + "X": 0.0, + "Y": -0.2, + "Z": 0.0 + }, + "Orientation": { + "X": 0.0, + "Y": 0.0, + "Z": -0.7071067, + "W": 0.7071067 + } + }, + "WheelDiameter": 0.17, + "PulsesPerRevolution": 4096, + "IsReversed": true + } + }, + "ManualControl": { + "Enable": true, + "UsingKeyboard": true, + "MinLinearVelocity": 0.0, + "MaxLinearVelocity": 0.5, + "MinAngularVelocity": 0.0, + "MaxAngularVelocity": 0.3, + "UpdateRate": 20.0, + "Acceleration": 0.5, + "Deceleration": 0.5 + } + }, + "Cartographer": { + "Enable": false + } +} diff --git a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Motion/DifferentialDrive.cs b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Motion/DifferentialDrive.cs index cf5f571..eee4975 100644 --- a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Motion/DifferentialDrive.cs +++ b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Motion/DifferentialDrive.cs @@ -684,17 +684,28 @@ public class DifferentialDrive : IInverseKinematics, IOdometryEstimator, IHosted } var (leftVel, rightVel) = CalculateWheelVelocities(twist); - + try { - if (_leftWheelServo != null) + // Ưu tiên gửi cả hai bánh trong một khung (RPDO) để chúng nhận lệnh đồng thời. + // Nếu driver/firmware không hỗ trợ thì fallback về ghi SDO từng bánh. + bool sentTogether = false; + if (_leftWheelServo is IDualAxisVelocityCommand dualAxis) { - await _leftWheelServo.SetTargetVelocityAsync(leftVel, ct); + sentTogether = await dualAxis.TrySetBothTargetVelocitiesAsync(leftVel, rightVel, ct); } - if (_rightWheelServo != null) + if (!sentTogether) { - await _rightWheelServo.SetTargetVelocityAsync(rightVel, ct); + if (_leftWheelServo != null) + { + await _leftWheelServo.SetTargetVelocityAsync(leftVel, ct); + } + + if (_rightWheelServo != null) + { + await _rightWheelServo.SetTargetVelocityAsync(rightVel, ct); + } } } catch (Exception ex) diff --git a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Program.cs b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Program.cs index 11cf220..90ed969 100644 --- a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Program.cs +++ b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Program.cs @@ -247,6 +247,24 @@ if (!string.IsNullOrEmpty(netCoreDir)) var app = builder.Build(); +// SQLite không tự tạo thư mục chứa file database - tạo trước các thư mục trong ConnectionStrings +// (đường dẫn tương đối như ./bin/dbs phụ thuộc working directory nên có thể chưa tồn tại) +foreach (var connectionStringEntry in builder.Configuration.GetSection("ConnectionStrings").GetChildren()) +{ + var dataSource = connectionStringEntry.Value? + .Split(';') + .Select(part => part.Split('=', 2)) + .Where(kv => kv.Length == 2 && kv[0].Trim().Equals("Data Source", StringComparison.OrdinalIgnoreCase)) + .Select(kv => kv[1].Trim()) + .FirstOrDefault(); + + var dbDirectory = string.IsNullOrEmpty(dataSource) ? null : Path.GetDirectoryName(dataSource); + if (!string.IsNullOrEmpty(dbDirectory)) + { + Directory.CreateDirectory(dbDirectory); + } +} + await app.Services.SeedApplicationDbAsync(); await app.Services.SeedScriptEngineDbAsync(); await app.Services.SeedMapManagerAsync(); diff --git a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/appsettings.json b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/appsettings.json index 11d0718..e14ca4e 100644 --- a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/appsettings.json +++ b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/appsettings.json @@ -70,7 +70,7 @@ }, "MBDV_Servo01": { "DeviceId": "right-wheel", - "Enabled": true, + "Enabled": false, "DeviceType": "CiA402Servo", "DeviceName": "Right Wheel Servo", "DriverName": "CiA402Servo", @@ -139,7 +139,7 @@ }, "MBDV_Servo02": { "DeviceId": "left-wheel", - "Enabled": true, + "Enabled": false, "DeviceType": "CiA402Servo", "DeviceName": "Left Wheel Servo", "DriverName": "CiA402Servo", @@ -206,8 +206,56 @@ } } }, + "ZLAC_LeftWheel": { + "DeviceId": "left-wheel", + "Enabled": true, + "DeviceType": "CiA402Servo", + "DeviceName": "Left Wheel (ZLAC8015D)", + "DriverName": "ZLAC8015D", + "DriverVersion": "1.0.0", + "Description": "ZLAC8015D dual hub servo - truc trai (1 node CAN dieu khien ca 2 banh, cung NodeId voi truc phai)", + "Connection": { + "CanInterface": "can0", + "NodeId": "1", + "Axis": "Left", + "CountsPerRevolution": 4096, + "SpeedResolution": 10, + "AccelTimeMs": 200, + "DecelTimeMs": 200, + "AutoReconnectEnabled": true, + "ReconnectDelayMs": 3000, + "MaxReconnectAttempts": 0, + "PdoConfigRetryCount": 3, + "PdoConfigRetryTimeoutMs": 1000, + "UseHeartbeatCheck": true + } + }, + "ZLAC_RightWheel": { + "DeviceId": "right-wheel", + "Enabled": true, + "DeviceType": "CiA402Servo", + "DeviceName": "Right Wheel (ZLAC8015D)", + "DriverName": "ZLAC8015D", + "DriverVersion": "1.0.0", + "Description": "ZLAC8015D dual hub servo - truc phai (1 node CAN dieu khien ca 2 banh, cung NodeId voi truc trai)", + "Connection": { + "CanInterface": "can0", + "NodeId": "1", + "Axis": "Right", + "CountsPerRevolution": 4096, + "SpeedResolution": 10, + "AccelTimeMs": 200, + "DecelTimeMs": 200, + "AutoReconnectEnabled": true, + "ReconnectDelayMs": 3000, + "MaxReconnectAttempts": 0, + "PdoConfigRetryCount": 3, + "PdoConfigRetryTimeoutMs": 1000, + "UseHeartbeatCheck": true + } + }, "MDX_Servo03": { - "DeviceId": "lift-motor", + "DeviceId": "lift-motor", "Enabled": false, "DeviceType": "CiA402Servo", "DeviceName": "Lift Motor", @@ -469,7 +517,7 @@ }, "Olei-front": { "DeviceId": "scan_1", - "Enabled": true, + "Enabled": false, "DeviceName": "Olei LR-1BS2", "DeviceType": "Lidar", "DriverName": "Olei2dLidarDriver", @@ -494,7 +542,7 @@ }, "Olei-rear": { "DeviceId": "scan_2", - "Enabled": true, + "Enabled": false, "DeviceName": "Olei LR-1BS5", "DeviceType": "Lidar", "DriverName": "OleiLidarDriver", @@ -618,7 +666,7 @@ }, "WheeltecIMU": { "DeviceId": "imu", - "Enabled": true, + "Enabled": false, "DeviceName": "Wheetec N100 IMU", "DeviceType": "IMU", "DriverName": "WheeltecN100IMU", @@ -657,7 +705,7 @@ }, "PLC": { "DeviceId": "plc-001", - "Enabled": true, + "Enabled": false, "DeviceName": "PLC_Controller", "DeviceType": "ModbusTcp", "DriverName": "ModbusTCP", @@ -771,7 +819,7 @@ } }, "WheelDiameter": 0.195, - "PulsesPerRevolution": 100000, + "PulsesPerRevolution": 4096, "IsReversed": false }, "RightWheel": { @@ -790,7 +838,7 @@ } }, "WheelDiameter": 0.195, - "PulsesPerRevolution": 100000, + "PulsesPerRevolution": 4096, "IsReversed": true } }, @@ -815,12 +863,12 @@ "LeftWheel": { "DeviceId": "left-wheel", "WheelDiameter": 0.182, - "PulsesPerRevolution": 100000 + "PulsesPerRevolution": 4096 }, "RightWheel": { "DeviceId": "right-wheel", "WheelDiameter": 0.182, - "PulsesPerRevolution": 100000 + "PulsesPerRevolution": 4096 }, "Wheelbase": 0.452, "FrameId": "odom",