Initial commit

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

@@ -0,0 +1,457 @@
+using RobotNet10.CANOpen.CiA402.Enums;
+using RobotNet10.RobotApp.Motion;
+using RobotNet10.RobotApp.Services.ConfigManager;
+
+namespace RobotNet10.RobotApp.Services.Navigation.CSharp;
+
+
+
+/// <summary>
+/// Configuration cho signal processing
+/// </summary>
+public class VelocitySignalProcessingConfig
+{
+    /// <summary>
+    /// Hệ số lọc cho encoder velocity
+    /// Giá trị nhỏ (0.1-0.2): Smooth nhưng lag
+    /// Giá trị lớn (0.3-0.4): Responsive nhưng nhiễu
+    /// </summary>
+    public double AlphaFilter { get; set; } = 0.3;
+
+    /// <summary>
+    /// Ngưỡng phát hiện encoder nhiễu (m/s)
+    /// Nếu thay đổi vận tốc > threshold trong 1 cycle → có thể nhiễu
+    /// </summary>
+    public double NoiseThreshold { get; set; } = 0.5;
+}
+
+/// <summary>
+/// Configuration cho velocity estimator
+/// </summary>
+public class VelocityEstimatorConfig
+{
+    // Blend ratio limits
+    public double MinBlendRatio { get; set; } = 0.15f;
+    public double MaxBlendRatio { get; set; } = 0.8f;
+    public double DefaultBlendRatio { get; set; } = 0.6;
+
+    // Adaptive blending thresholds
+    public double GoodTrackingThreshold { get; set; } = 0.12f;   // < 10% error
+    public double ModerateTrackingThreshold { get; set; } = 0.3; // < 30% error
+
+    // Blend ratios for different tracking qualities
+    public double GoodTrackingBlend { get; set; } = 0.7;
+    public double ModerateTrackingBlend { get; set; } = 0.5;
+    public double PoorTrackingBlend { get; set; } = 0.25f;
+
+    // Model confidence decay
+    public double ConfidenceDecayRate { get; set; } = 0.95f;
+    public double MinConfidence { get; set; } = 0.3;
+}
+
+public class VelocityController(IInverseKinematics InverseKinematic,
+                                OdometryService odometryService,
+                                IRobotConfiguration RobtoConfiguration,
+                                INavigationConfig NavigationConfig,
+                                ILogger<VelocityController> _logger) : IVelocityController
+{
+    public (double Linear, double Angular) ActualVelocity => GetCurrentVel();
+    public (double Linear, double Angular) RawVelocity => GetRawCurrentVel();
+
+    // vận tốc tính toán m/s và rad/s đối với vận tốc góc
+    private double _rightVelCmd = 0;
+    private double _leftVelCmd = 0;
+    private double _oldRightVel = 0;
+    private double _oldLeftVel = 0;
+
+    private MotorDynamicsConfig _motorDynamicsConifg = new();
+    private MotorDynamicsModel _motorDynamicsModel = new();
+    private VelocityEstimatorConfig _estimatorConfig = new();
+    public PurePursuitConfig _purePursuitConfig = new();
+    private VelocitySignalProcessingConfig _signalConfig = new();
+    private readonly CircularBuffer<double> _predictionErrors = new(20);
+
+    private double _currentConfidence = 1.0;
+    private readonly double wheelBase = RobtoConfiguration.GetRobotPhysicalConfig().WheelBase;
+    private int _ensureIKReadyCounter = 0; // Counter for logging throttling
+
+    public void SetVelocity(double linearVel, double angularVel)
+    {
+        // InverseKinematic.SetVelocity not available - commented out
+        // InverseKinematic.SetVelocity(new()
+        // {
+        //     Linear = new(){
+        //         X = linearVel,
+        //         Y = 0,
+        //     },
+        //     Angular = new(){
+        //         Z = angularVel,
+        //     },
+        // });
+        _leftVelCmd = linearVel - (wheelBase / 2) * angularVel;
+        _rightVelCmd = linearVel + (wheelBase / 2) * angularVel;
+    }
+    public (double linearVel, double angularVel) GetRawCurrentVel()
+    {
+        try
+        {
+            var odom = odometryService.CurrentOdometry;
+            double vActual = odom.Twist.Twist.Linear.X;
+            double omegaActual = odom.Twist.Twist.Angular.Z;
+            return (vActual, omegaActual);
+        }
+        catch { return (0, 0); }
+    }
+
+    public (double linearVel, double angularVel) GetCurrentVel()
+    {
+        try
+        {
+            var odom = odometryService.CurrentOdometry;
+            var vActual = odom.Twist.Twist.Linear.X;
+            var omegaActual = odom.Twist.Twist.Angular.Z;
+
+            // Convert linear/angular back to left/right wheel velocities for the estimator
+            _oldLeftVel = vActual - (wheelBase / 2) * omegaActual;
+            _oldRightVel = vActual + (wheelBase / 2) * omegaActual;
+
+            return Estimate(_oldLeftVel, _oldRightVel, _leftVelCmd, _rightVelCmd, wheelBase);
+        }
+        catch { return (0, 0); }
+    }
+
+    /// <summary>
+    /// Exponential Moving Average (EMA) Low-Pass Filter
+    /// </summary>
+    /// <param name="newValue">Giá trị mới từ sensor</param>
+    /// <param name="oldValue">Giá trị đã lọc trước đó</param>
+    /// <param name="alpha">Hệ số lọc (0-1). Càng nhỏ càng smooth, càng lớn càng responsive</param>
+    /// <returns>Giá trị sau khi lọc</returns>
+    private static double LowPassFilter(double newValue, double oldValue, double alpha)
+    {
+        // Validate alpha
+        if (alpha < 0 || alpha > 1)
+        {
+            throw new ArgumentException("Alpha must be between 0 and 1", nameof(alpha));
+        }
+
+        return alpha * newValue + (1.0 - alpha) * oldValue;
+    }
+
+    /// <summary>
+    /// MAIN FUNCTION: Estimate velocity
+    /// </summary>
+    private (double linearVel, double angularVel) Estimate(
+        double vLeftActual,      // Từ encoder (filtered)
+        double vRightActual,     // Từ encoder (filtered)
+        double vLeftCmdPrev,     // Command từ cycle trước
+        double vRightCmdPrev,    // Command từ cycle trước
+        double wheelbase)
+    {
+        // 1. Tính vận tốc actual (linear & angular)
+        double vActual = (vLeftActual + vRightActual) / 2.0;
+        double omegaActual = (vRightActual - vLeftActual) / wheelbase;
+
+        // 2. Tính vận tốc command từ cycle trước
+        double vCmdPrev = (vLeftCmdPrev + vRightCmdPrev) / 2.0;
+        double omegaCmdPrev = (vRightCmdPrev - vLeftCmdPrev) / wheelbase;
+
+        // 3. Tính prediction horizon
+        double predictionHorizon = CalculatePredictionHorizon(vActual);
+
+        // 4. Predict velocity cho từng bánh
+        double vLeftPredicted = _motorDynamicsModel.PredictVelocity(
+            vLeftCmdPrev,
+            vLeftActual,
+            predictionHorizon
+        );
+
+        double vRightPredicted = _motorDynamicsModel.PredictVelocity(
+            vRightCmdPrev,
+            vRightActual,
+            predictionHorizon
+        );
+
+        // 5. Tính linear & angular predicted
+        double vPredicted = (vLeftPredicted + vRightPredicted) / 2.0;
+        double omegaPredicted = (vRightPredicted - vLeftPredicted) / wheelbase;
+
+        // 6. Update model confidence
+        UpdateModelConfidence(vPredicted, vActual);
+
+        // 7. Tính tracking error
+        double linearErr = CalculateLinearTrackingError(vCmdPrev, vActual);
+        double angularErr = CalculateAngularTrackingError(omegaCmdPrev, omegaActual);
+        double combinedTrackingError = 0.65f * linearErr + 0.35f * angularErr;
+
+        // 8. Calculate adaptive blend ratio
+        double blendRatio = CalculateAdaptiveBlendRatio(
+            combinedTrackingError,
+            _currentConfidence
+        );
+
+        // 9. Blend predicted và actual
+        var vHybrid = (blendRatio * vPredicted) + ((1.0 - blendRatio) * vActual);
+        double omegaHybrid = blendRatio * omegaPredicted + (1.0 - blendRatio) * omegaActual;
+
+        // 10. Return result
+        return (vHybrid, omegaHybrid);
+    }
+
+    /// <summary>
+    /// Tính prediction horizon dựa vào lookahead distance
+    /// </summary>
+    private double CalculatePredictionHorizon(double vActual)
+    {
+        // Lookahead distance
+        double lookahead = _purePursuitConfig.LookaheadMin + _purePursuitConfig.Kdd * Math.Abs(vActual);
+
+        lookahead = Math.Clamp(lookahead, _purePursuitConfig.LookaheadMin, _purePursuitConfig.LookaheadMax);
+
+        // Prediction time = lookahead / velocity
+        // Nếu vận tốc quá nhỏ, dùng một giá trị minimum
+        double predictionTime = lookahead / Math.Max(Math.Abs(vActual), 0.1);
+
+        // Giới hạn prediction time (không nên quá xa)
+        predictionTime = Math.Clamp(predictionTime, 0.1, 2.0);
+
+        return predictionTime;
+    }
+
+    /// <summary>
+    /// Tính linear velocity tracking error (normalized)
+    /// </summary>
+    private static double CalculateLinearTrackingError(double vCmd, double vActual)
+    {
+        double error = Math.Abs(vCmd - vActual);
+        double normalizedError = error / Math.Max(Math.Abs(vCmd), 0.1);
+        return normalizedError;
+    }
+
+    /// <summary>
+    /// Tính angular velocity tracking error (normalized)
+    /// </summary>
+    private static double CalculateAngularTrackingError(double oCmd, double oActual)
+    {
+        double error = Math.Abs(oCmd - oActual);
+        double normalizedError = error / Math.Max(Math.Abs(oCmd), 0.05f);
+        return normalizedError;
+    }
+
+    /// <summary>
+    /// Update model confidence dựa trên prediction accuracy
+    /// </summary>
+    private void UpdateModelConfidence(double vPredictedPrev, double vActualNow)
+    {
+        // Prediction error từ cycle trước
+        double predError = Math.Abs(vPredictedPrev - vActualNow) / Math.Max(Math.Abs(vActualNow), 0.1);
+
+        _predictionErrors.Add(predError);
+
+        // Tính confidence dựa trên average error
+        if (_predictionErrors.Count > 0)
+        {
+            double avgError = _predictionErrors.Average();
+
+            // Confidence = 1 - avgError (capped)
+            double newConfidence = Math.Clamp(1.0 - avgError, 0.0, 1.0);
+
+            // Smooth update với decay
+            _currentConfidence = _estimatorConfig.ConfidenceDecayRate * _currentConfidence + (1.0 - _estimatorConfig.ConfidenceDecayRate) * newConfidence;
+
+            _currentConfidence = Math.Max(_currentConfidence, _estimatorConfig.MinConfidence);
+        }
+    }
+
+    /// <summary>
+    /// Calculate adaptive blend ratio
+    /// </summary>
+    private double CalculateAdaptiveBlendRatio(
+        double trackingError,
+        double modelConfidence)
+    {
+        double alpha;
+
+        // Factor 1: Tracking error
+        if (trackingError < _estimatorConfig.GoodTrackingThreshold)
+        {
+            // Motor tracking tốt → tin prediction nhiều
+            alpha = _estimatorConfig.GoodTrackingBlend;
+        }
+        else if (trackingError < _estimatorConfig.ModerateTrackingThreshold)
+        {
+            // Moderate error → balanced
+            alpha = _estimatorConfig.ModerateTrackingBlend;
+        }
+        else
+        {
+            // Poor tracking (slip/overload) → tin actual nhiều
+            alpha = _estimatorConfig.PoorTrackingBlend;
+        }
+
+        // Factor 2: Model confidence
+        // Nếu model không chính xác, giảm blend ratio
+        alpha *= modelConfidence;
+
+        // Clamp trong khoảng cho phép
+        alpha = Math.Clamp(alpha, _estimatorConfig.MinBlendRatio, _estimatorConfig.MaxBlendRatio);
+
+        return alpha;
+    }
+
+    /// <summary>
+    /// Reset estimator state
+    /// </summary>
+    public void Reset()
+    {
+        _predictionErrors.Clear();
+        _currentConfidence = 1.0;
+    }
+
+    /// <summary>
+    /// Get current model confidence
+    /// </summary>
+    public double GetModelConfidence()
+    {
+        return _currentConfidence;
+    }
+
+    public void LoadConfig()
+    {
+        _motorDynamicsConifg = NavigationConfig.GetMotorDynamicsConfig();
+        _motorDynamicsModel = new(_motorDynamicsConifg);
+        _estimatorConfig = NavigationConfig.GetVelocityEstimatorConfig();
+        _purePursuitConfig = NavigationConfig.GetPurepursuitConfig();
+        _signalConfig = NavigationConfig.GetVelocitySignalProcessingConfig();
+    }
+
+    public void SetAcceleration(double acc)
+    {
+        // SetAcceleration not available - commented out
+        // InverseKinematic.SetAcceleration(acc);
+    }
+
+    public void SetDeceleration(double dec)
+    {
+        // SetDeceleration not available - commented out
+        // InverseKinematic.SetDeceleration(dec);
+    }
+
+    public bool EnsureInverseKinematicsReady(CancellationToken cancellationToken)
+    {
+        try
+        {
+            // Increment counter for logging throttling
+            _ensureIKReadyCounter++;
+
+            // Check if need to reset fault first
+            // Note: DifferentialDrive doesn't expose IsFaulted, so we try FaultReset if not enabled
+            if (!InverseKinematic.IsOperationEnabled)
+            {
+                // Try fault reset first (in case it's in fault state)
+                InverseKinematic.FaultReset();
+                PreciseDelay(200, cancellationToken);
+            }
+
+            // Check if IInverseKinematics is in OperationEnabled state
+            if (!InverseKinematic.IsOperationEnabled)
+            {
+                // Auto-enable IInverseKinematics through state transitions
+                // Enable() is a convenience method that automatically transitions through all states
+                int maxAttempts = 3;
+                int attemptDelay = 300; // ms
+
+                for (int i = 0; i < maxAttempts && !InverseKinematic.IsOperationEnabled; i++)
+                {
+                    InverseKinematic.Enable();
+                    PreciseDelay(attemptDelay, cancellationToken);
+                }
+
+                // Check if enabled successfully
+                if (!InverseKinematic.IsOperationEnabled)
+                {
+                    // Log only once every 10 times to avoid spam
+                    if (_ensureIKReadyCounter % 10 == 0)
+                    {
+                        _logger.LogWarning("IInverseKinematics is not in OperationEnabled state. Cannot send velocity.");
+                    }
+                    return false;
+                }
+            }
+
+            // Check and set operation mode to ProfileVelocity (synchronous)
+            try
+            {
+                // GetOperationMode/SetOperationMode not available - commented out
+                // OperationMode currentMode = InverseKinematic.GetOperationMode();
+                // if (currentMode != OperationMode.ProfileVelocity)
+                // {
+                //     InverseKinematic.SetOperationMode(OperationMode.ProfileVelocity);
+                // }
+            }
+            catch (Exception ex)
+            {
+                // Log only once every 10 times to avoid spam
+                if (_ensureIKReadyCounter % 10 == 0)
+                {
+                    _logger.LogWarning(ex, "Error checking/setting operation mode");
+                }
+                // Continue anyway to prevent blocking
+            }
+
+            return true;
+        }
+        catch (OperationCanceledException)
+        {
+            throw;
+        }
+        catch (Exception ex)
+        {
+            // Log only once every 10 times to avoid spam
+            if (_ensureIKReadyCounter % 10 == 0)
+            {
+                _logger.LogError(ex, "Error ensuring IInverseKinematics ready");
+            }
+            return false;
+        }
+    }
+
+    /// <summary>
+    /// Precise synchronous delay using Thread.Sleep for longer delays and SpinWait for short delays
+    /// This ensures accurate timing for the update loop without async overhead
+    /// </summary>
+    private static void PreciseDelay(int milliseconds, CancellationToken cancellationToken)
+    {
+        if (milliseconds <= 0)
+            return;
+
+        if (milliseconds > 1)
+        {
+            // Use Thread.Sleep for longer delays (synchronous, more precise in dedicated thread)
+            // Check cancellation periodically during sleep
+            var sleepStart = DateTime.UtcNow;
+            while ((DateTime.UtcNow - sleepStart).TotalMilliseconds < milliseconds)
+            {
+                if (cancellationToken.IsCancellationRequested)
+                    return;
+
+                var remaining = milliseconds - (int)(DateTime.UtcNow - sleepStart).TotalMilliseconds;
+                if (remaining > 0)
+                {
+                    Thread.Sleep(Math.Min(remaining, 10)); // Sleep in 10ms chunks to check cancellation
+                }
+            }
+        }
+        else
+        {
+            // Use SpinWait for very short delays to maintain precise timing
+            var spinWait = new SpinWait();
+            for (int i = 0; i < 10; i++)
+            {
+                if (cancellationToken.IsCancellationRequested)
+                    break;
+                spinWait.SpinOnce();
+            }
+        }
+    }
+}