update motor

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

srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/CanOpenDevice.cs

@@ -30,6 +30,13 @@ public class CanOpenDevice : ICanOpenDevice, IDisposable
     /// </summary>
     public bool AutoVerifyStateViaHeartbeat { get; set; } = false;
 
+    /// <summary>
+    /// 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.
+    /// </summary>
+    public SemaphoreSlim ConfigurationLock { get; } = new(1, 1);
+    
     // Expose các services để user có thể truy cập
     public PdoManager Pdo => _pdoManager;
     public EmergencyMonitor Emergency => _emergencyMonitor;
@@ -155,6 +162,19 @@ public class CanOpenDevice : ICanOpenDevice, IDisposable
         await WriteObjectAsync(index, subIndex, BitConverter.GetBytes(value), cancellationToken);
     }
 
+    /// <summary>
+    /// 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).
+    /// </summary>
+    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

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)
             {

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<string, TaskCompletionSource<SdoResponse>> _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<SdoClient>? _logger;
     private bool _disposed;
@@ -28,6 +31,12 @@ public class SdoClient : IDisposable
     }
     
     public async Task<byte[]> UploadAsync(ushort index, byte subIndex, CancellationToken cancellationToken = default)
+    {
+        if (_disposed)
+            throw new ObjectDisposedException(nameof(SdoClient));
+
+        await _transactionLock.WaitAsync(cancellationToken);
+        try
         {
             if (_disposed)
                 throw new ObjectDisposedException(nameof(SdoClient));
@@ -53,6 +62,11 @@ public class SdoClient : IDisposable
                 return await UploadSegmentedAsync(index, subIndex, response, cancellationToken);
             }
         }
+        finally
+        {
+            _transactionLock.Release();
+        }
+    }
     
     /// <summary>
     /// Upload data using segmented transfer
@@ -123,6 +137,12 @@ public class SdoClient : IDisposable
     }
     
     public async Task DownloadAsync(ushort index, byte subIndex, byte[] data, CancellationToken cancellationToken = default)
+    {
+        if (_disposed)
+            throw new ObjectDisposedException(nameof(SdoClient));
+
+        await _transactionLock.WaitAsync(cancellationToken);
+        try
         {
             if (_disposed)
                 throw new ObjectDisposedException(nameof(SdoClient));
@@ -146,6 +166,54 @@ public class SdoClient : IDisposable
                 await DownloadSegmentedAsync(index, subIndex, data, cancellationToken);
             }
         }
+        finally
+        {
+            _transactionLock.Release();
+        }
+    }
+    
+    /// <summary>
+    /// 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).
+    /// </summary>
+    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();
+        }
+    }
 
     /// <summary>
     /// Download data using segmented transfer (for data > 4 bytes)

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Devices/IDualAxisVelocityCommand.cs

@@ -0,0 +1,16 @@
+namespace RobotNet10.RobotApp.Devices;
+
+/// <summary>
+/// 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.
+/// </summary>
+public interface IDualAxisVelocityCommand
+{
+    /// <summary>
+    /// 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.
+    /// </summary>
+    Task<bool> TrySetBothTargetVelocitiesAsync(int leftCountsPerSec, int rightCountsPerSec, CancellationToken ct = default);
+}

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
+  }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Motion/DifferentialDrive.cs

@@ -686,6 +686,16 @@ public class DifferentialDrive : IInverseKinematics, IOdometryEstimator, IHosted
         var (leftVel, rightVel) = CalculateWheelVelocities(twist);
 
         try
+        {
+            // Ư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)
+            {
+                sentTogether = await dualAxis.TrySetBothTargetVelocitiesAsync(leftVel, rightVel, ct);
+            }
+
+            if (!sentTogether)
             {
                 if (_leftWheelServo != null)
                 {
@@ -697,6 +707,7 @@ public class DifferentialDrive : IInverseKinematics, IOdometryEstimator, IHosted
                     await _rightWheelServo.SetTargetVelocityAsync(rightVel, ct);
                 }
             }
+        }
         catch (Exception ex)
         {
             _logger.LogError(ex, "Error setting wheel velocities");

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();

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,6 +206,54 @@
         }
       }
     },
+    "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",
       "Enabled": false,
@@ -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",