Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Navigation/CSharp/MotorDynamicsModel.cs

@@ -0,0 +1,109 @@
+namespace RobotNet10.RobotApp.Services.Navigation.CSharp;
+
+public class MotorDynamicsConfig
+{
+    /// <summary>
+    /// Time constant (τ) - thời gian để motor đạt 63.2% của target velocity
+    /// Đơn vị: giây (s)
+    /// Typical: 0.1 - 0.5s cho DC motor với driver PID
+    /// </summary>
+    public double Tau { get; set; }
+
+    /// <summary>
+    /// Pure delay (δ) - độ trễ trước khi motor bắt đầu phản ứng
+    /// Đơn vị: giây (s)
+    /// Bao gồm: communication delay + driver processing
+    /// Typical: 0.02 - 0.1s
+    /// </summary>
+    public double Delta { get; set; }
+}
+
+/// <summary>
+/// Mô hình động học của motor driver
+/// First-order system: v(t) = v_cmd × (1 - e^(-(t-δ)/τ))
+/// </summary>
+public class MotorDynamicsModel
+{
+    /// <summary>
+    /// Time constant (τ) - thời gian để motor đạt 63.2% của target velocity
+    /// Đơn vị: giây (s)3
+    /// Typical: 0.1 - 0.5s cho DC motor với driver PID
+    /// </summary>
+    public double Tau { get; set; }
+
+    /// <summary>
+    /// Pure delay (δ) - độ trễ trước khi motor bắt đầu phản ứng
+    /// Đơn vị: giây (s)
+    /// Bao gồm: communication delay + driver processing
+    /// Typical: 0.02 - 0.1s
+    /// </summary>
+    public double Delta { get; set; }
+
+    /// <summary>
+    /// Constructor với giá trị mặc định
+    /// </summary>
+    public MotorDynamicsModel()
+    {
+        Tau = 0.3;      // 300ms time constant
+        Delta = 0.05f;   // 50ms delay
+    }
+
+    public MotorDynamicsModel(MotorDynamicsConfig cog)
+    {
+        Tau = cog.Tau;
+        Delta = cog.Delta;
+    }
+
+    /// <summary>
+    /// Predict vận tốc tại thời điểm tương lai
+    /// </summary>
+    /// <param name="vCmd">Velocity command đã gửi</param>
+    /// <param name="vActual">Velocity thực tế hiện tại</param>
+    /// <param name="timeAhead">Thời gian dự đoán về tương lai (s)</param>
+    /// <returns>Vận tốc dự đoán</returns>
+    public double PredictVelocity(double vCmd, double vActual, double timeAhead)
+    {
+        // Nếu thời gian dự đoán < delay
+        // → Motor chưa bắt đầu phản ứng
+        if (timeAhead < Delta)
+        {
+            return vActual;
+        }
+
+        // Thời gian hiệu dụng (sau khi trừ delay)
+        double effectiveTime = timeAhead - Delta;
+
+        // First-order system response
+        // response = 1 - e^(-t/τ)
+        double response = 1.0 - Math.Exp(-effectiveTime / Tau);
+
+        // Velocity prediction
+        // v_future = v_actual + (v_cmd - v_actual) × response
+        double vPredicted = vActual + (vCmd - vActual) * response;
+
+        return vPredicted;
+    }
+
+    /// <summary>
+    /// Tính settling time (thời gian để đạt 95% target)
+    /// </summary>
+    public double GetSettlingTime()
+    {
+        // 95% response: t = -τ × ln(0.05) ≈ 3τ
+        return Delta + 3.0 * Tau;
+    }
+
+    /// <summary>
+    /// Tính rise time (thời gian để đạt từ 10% đến 90%)
+    /// </summary>
+    public double GetRiseTime()
+    {
+        // Rise time ≈ 2.2τ
+        return 2.2 * Tau;
+    }
+
+    public override string ToString()
+    {
+        return $"MotorModel(τ={Tau:F3}s, δ={Delta:F3}s, settling={GetSettlingTime():F3}s)";
+    }
+}