update motor

Co-authored-by: Copilot <copilot@github.com>

srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/Services/PdoManager.cs

@@ -360,6 +360,97 @@ public class PdoManager : IDisposable
         return supportedIndices.Contains(mappingParamIndex);
     }
     
+    /// <summary>
+    /// 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.
+    /// </summary>
+    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);
+    }
+
+    /// <summary>
+    /// Đọ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.
+    /// </summary>
+    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);
+        }
+    }
+
     /// <summary>
     /// Stop và clear tất cả TPDOs trên device
     /// </summary>
@@ -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)
             {