109 lines
3.4 KiB
C#
109 lines
3.4 KiB
C#
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)";
|
||
}
|
||
} |