update motor

Co-authored-by: Copilot <copilot@github.com>
2026-07-13 16:28:07 +07:00
parent bccfb156d7
commit dddf3fc594
9 changed files with 2142 additions and 48 deletions
--- a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Devices/IDualAxisVelocityCommand.cs
+++ b/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);
+}
--- a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/ZLAC/Zlac8015dServo.cs
+++ b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/ZLAC/Zlac8015dServo.cs
--- a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/ModelConfigs/ZLAC-test.json
+++ 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
+  }
+}
--- 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)
--- 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();
--- 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",