update

RobotApp/RobotApp.csproj

@@ -5,6 +5,7 @@
 		<Nullable>enable</Nullable>
 		<ImplicitUsings>enable</ImplicitUsings>
 		<UserSecretsId>aspnet-RobotApp-1f61caa2-bbbb-40cd-88b6-409b408a84ea</UserSecretsId>
+		<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
 	</PropertyGroup>
 
 	<ItemGroup>

RobotApp/Services/HighPrecisionTimer.cs

@@ -0,0 +1,157 @@
+using System.Diagnostics;
+using System.Runtime.InteropServices;
+
+namespace RobotApp.Services;
+
+internal static partial class Windows
+{
+    [LibraryImport("winmm.dll")]
+    internal static partial uint timeBeginPeriod(uint uPeriod);
+
+    [LibraryImport("winmm.dll")]
+    internal static partial uint timeEndPeriod(uint uPeriod);
+}
+
+public static class HighPrecisionTimerHelper
+{
+    public static void EnableHighPrecision()
+    {
+        if (RuntimeInformation.IsOSPlatform(OSPlatform.Windows))
+        {
+            _ = Windows.timeBeginPeriod(2);
+        }
+    }
+
+    public static void DisableHighPrecision()
+    {
+        if (RuntimeInformation.IsOSPlatform(OSPlatform.Windows))
+        {
+            _ = Windows.timeEndPeriod(2);
+        }
+    }
+}
+
+public class HighPrecisionTimer<T>(int Interval, Action Callback, Logger<T>? Logger) : IDisposable where T : class
+{
+    public bool Disposed;
+    private Thread? Thread;
+    private long IntervalTicks;
+    private long NextDueTime;
+    private readonly Lock Lock = new();
+
+    private void Handler()
+    {
+        while (!Disposed)
+        {
+            long now = Stopwatch.GetTimestamp();
+
+            bool shouldRun = false;
+
+            lock (Lock)
+            {
+                if (Disposed) break;
+                if (now >= NextDueTime)
+                {
+                    shouldRun = true;
+                    long scheduledTime = NextDueTime;
+                    NextDueTime += IntervalTicks;
+
+                    // Tự đồng bộ nếu lệch quá
+                    long driftTicks = now - scheduledTime;
+                    if (driftTicks > IntervalTicks / 2)
+                    {
+                        Logger?.Warning($"High-res timer drift: {driftTicks * 1000.0 / Stopwatch.Frequency:F3}ms. Resync.");
+                        NextDueTime = now + IntervalTicks;
+                    }
+                }
+            }
+
+            // === BƯỚC 2: Chạy callback ===
+            if (shouldRun)
+            {
+                try
+                {
+                    Callback.Invoke();
+                }
+                catch (Exception ex)
+                {
+                    Logger?.Error($"Callback error in high-precision timer: {ex}");
+                }
+            }
+
+            // === BƯỚC 3: Chờ chính xác đến lần sau ===
+            long sleepUntil = NextDueTime;
+            while (!Disposed)
+            {
+                now = Stopwatch.GetTimestamp();
+                long remaining = sleepUntil - now;
+
+                if (remaining <= 0)
+                    break;
+
+                // > 1ms → Sleep
+                if (remaining > Stopwatch.Frequency / 1000)
+                {
+                    Thread.Sleep(1);
+                }
+                // < 1ms → SpinWait
+                else
+                {
+                    Thread.SpinWait((int)(remaining / 10));
+                }
+            }
+        }
+    }
+
+    public void Start()
+    {
+        if (!Disposed)
+        {
+            lock (Lock)
+            {
+                if (Interval < 30) HighPrecisionTimerHelper.EnableHighPrecision();
+                IntervalTicks = (long)(Interval * (Stopwatch.Frequency / 1000.0));
+                Thread = new Thread(Handler) { IsBackground = true, Priority = ThreadPriority.Highest };
+                NextDueTime = Stopwatch.GetTimestamp() + IntervalTicks;
+                Thread.Start();
+            }
+        }
+        else throw new ObjectDisposedException(nameof(WatchTimer<T>));
+    }
+
+    public void Stop()
+    {
+        if (Disposed) return;
+
+        if (Thread != null)
+        {
+            Disposed = true;
+            lock (Lock)
+            {
+                Thread.Join(100);
+                Thread = null;
+                HighPrecisionTimerHelper.DisableHighPrecision();
+            }
+        }
+    }
+
+    public void Dispose()
+    {
+        Dispose(true);
+        GC.SuppressFinalize(this);
+    }
+
+    protected virtual void Dispose(bool disposing)
+    {
+        if (Disposed) return;
+
+        if (disposing) Stop();
+
+        Disposed = true;
+    }
+
+    ~HighPrecisionTimer()
+    {
+        Dispose(false);
+    }
+}

RobotApp/Services/Robot/RobotOrderController.cs

@@ -203,14 +203,14 @@ public class RobotOrderController(INavigation NavigationManager,
                 }
                 else OrderActions.Add(order.Nodes[i].NodeId, order.Edges[i].Actions);
             }
-            //if (order.Nodes[i].SequenceId != i) throw new OrderException(RobotErrors.Error1010(order.Nodes[i].NodeId, order.Nodes[i].SequenceId, i));
+            //if (i > 0 && order.Nodes[i].SequenceId <= order.Nodes[i - 1].SequenceId) throw new OrderException(RobotErrors.Error1010(order.Nodes[i].NodeId, order.Nodes[i].SequenceId, i));
             //if (i < order.Nodes.Length - 1 && order.Edges[i].SequenceId != i) throw new OrderException(RobotErrors.Error1011(order.Edges[i].EdgeId, order.Edges[i].SequenceId, i));
             if (order.Nodes[i].Released) CurrentBaseNode = order.Nodes[i];
         }
         SafetyManager.OnSafetySpeedChanged += OnSafetySpeedChanged;
 
         if (OrderActions.TryGetValue(order.Nodes[^1].NodeId, out Action[]? finalactions) && finalactions is not null && finalactions.Length > 0) FinalAction = [.. finalactions];
-        ActionManager.ClearInstantActions();
+       
         if (OrderActions.Count > 0) ActionManager.AddOrderActions([.. OrderActions.Values.SelectMany(a => a)]);
 
         if (order.Nodes.Length > 1 && order.Edges.Length >= 0)
@@ -314,11 +314,7 @@ public class RobotOrderController(INavigation NavigationManager,
         {
             var robotAction = ActionManager[ActionHard.ActionId];
             if (robotAction is null) return;
-            if (robotAction is not null && robotAction.IsCompleted)
-            {
-                NavigationManager.Resume();
-                ActionHard = null;
-            }
+            if (robotAction is not null && robotAction.IsCompleted) ActionHard = null;
         }
         else
         {
@@ -345,6 +341,11 @@ public class RobotOrderController(INavigation NavigationManager,
                 if (IsWaitingPaused) IsWaitingPaused = false;
             }
         }
+
+        if (ActionHard == null && ActionWaitingRunning.IsEmpty)
+        {
+            NavigationManager.Resume();
+        }
     }
 
     private void OrderHandler()

RobotApp/Services/Robot/Simulation/Navigation/Algorithm/FuzzyLogic.cs

@@ -2,251 +2,537 @@
 
 public class FuzzyLogic
 {
-    private double Gain_P = 0.5;
-    private double Gain_I = 0.01;
-    private double DiscreteTimeIntegrator_DSTATE;
+    // ============================================================================
+    // FUZZY LOGIC CONTROLLER - PHIÊN BẢN GỐC ĐÃ TỔ CHỨC LẠI
+    // Dải đầu ra: 0.0 - 1.0 | Độ phân giải: 5 mức | Số luật: 25 (5x5)
+    // ============================================================================
 
-    public FuzzyLogic WithGainP(double gainP)
+    // ============================================================================
+    // PHẦN 1: CÁC HÀM MEMBERSHIP (HÀM THUỘC)
+    // ============================================================================
+
+    /// <summary>
+    /// Hàm thuộc hình thang (Trapezoidal Membership Function)
+    /// Dạng hình thang với 4 điểm: [left_base, left_top, right_top, right_base]
+    ///
+    ///        left_top ______ right_top
+    ///              /          \
+    ///             /            \
+    ///   _________/              \_________
+    /// left_base                  right_base
+    /// 
+    /// </summary>
+    /// <param name="inputValue">Giá trị đầu vào cần tính độ thuộc</param>
+    /// <param name="left_base">Điểm bắt đầu hình thang (μ = 0)</param>
+    /// <param name="left_top">Điểm bắt đầu vùng phẳng trên (μ = 1)</param>
+    /// <param name="right_top">Điểm kết thúc vùng phẳng trên (μ = 1)</param>
+    /// <param name="right_base">Điểm kết thúc hình thang (μ = 0)</param>
+    /// <returns>Độ thuộc trong khoảng [0, 1]</returns>
+    private static double Fuzzy_trapmf(double inputValue, double left_base, double left_top, double right_top, double right_base)
     {
-        Gain_P = gainP;
-        return this;
+        double membership = 0.0;
+
+        // Trường hợp 1: Nằm ngoài phía trái hình thang
+        if (inputValue <= left_base)
+        {
+            membership = 0.0;
+        }
+        // Trường hợp 2: Nằm trên cạnh tăng dần (trái)
+        else if (inputValue > left_base && inputValue < left_top)
+        {
+            if (left_top != left_base)  // Tránh chia cho 0
+            {
+                membership = (inputValue - left_base) / (left_top - left_base);
+            }
+        }
+        // Trường hợp 3: Nằm trên vùng phẳng (đỉnh)
+        else if (inputValue >= left_top && inputValue <= right_top)
+        {
+            membership = 1.0;
+        }
+        // Trường hợp 4: Nằm trên cạnh giảm dần (phải)
+        else if (inputValue > right_top && inputValue < right_base)
+        {
+            if (right_base != right_top)  // Tránh chia cho 0
+            {
+                membership = (right_base - inputValue) / (right_base - right_top);
+            }
+        }
+        // Trường hợp 5: Nằm ngoài phía phải hình thang
+        else if (inputValue >= right_base)
+        {
+            membership = 0.0;
+        }
+
+        return membership;
     }
 
-    public FuzzyLogic WithGainI(double gainI)
+    /// <summary>
+    /// Hàm thuộc tam giác (Triangular Membership Function)
+    /// Dạng tam giác với 3 điểm: [left, peak, right]
+    ///
+    ///           peak
+    ///           /\
+    ///          /  \
+    ///         /    \
+    ///   _____/      \_____
+    ///      left      right
+    /// 
+    /// </summary>
+    /// <param name="inputValue">Giá trị đầu vào cần tính độ thuộc</param>
+    /// <param name="left">Điểm bắt đầu tam giác (μ = 0)</param>
+    /// <param name="peak">Điểm đỉnh tam giác (μ = 1)</param>
+    /// <param name="right">Điểm kết thúc tam giác (μ = 0)</param>
+    /// <returns>Độ thuộc trong khoảng [0, 1]</returns>
+    private static double Fuzzy_trimf(double inputValue, double left, double peak, double right)
     {
-        Gain_I = gainI;
-        return this;
+        double membership = 0.0;
+
+        // Trường hợp 1: Nằm ngoài phía trái tam giác
+        if (inputValue <= left)
+        {
+            membership = 0.0;
+        }
+        // Trường hợp 2: Nằm trên cạnh tăng dần (trái)
+        else if (inputValue > left && inputValue < peak)
+        {
+            if (peak != left)  // Tránh chia cho 0
+            {
+                membership = (inputValue - left) / (peak - left);
+            }
+        }
+        // Trường hợp 3: Đúng tại đỉnh tam giác
+        else if (inputValue == peak)
+        {
+            membership = 1.0;
+        }
+        // Trường hợp 4: Nằm trên cạnh giảm dần (phải)
+        else if (inputValue > peak && inputValue < right)
+        {
+            if (right != peak)  // Tránh chia cho 0
+            {
+                membership = (right - inputValue) / (right - peak);
+            }
+        }
+        // Trường hợp 5: Nằm ngoài phía phải tam giác
+        else if (inputValue >= right)
+        {
+            membership = 0.0;
+        }
+
+        return membership;
     }
 
-    private static double Fuzzy_trapmf(double x, double[] parame)
+    // ============================================================================
+    // PHẦN 2: FUZZIFICATION (MỜ HÓA ĐẦU VÀO)
+    // ============================================================================
+
+    /// <summary>
+    /// Mờ hóa tín hiệu PI thành 5 tập mờ
+    /// </summary>
+    /// <param name="piSignal">Tín hiệu đầu ra của bộ PI</param>
+    /// <param name="membershipValues">Mảng lưu giá trị độ thuộc (vị trí 0-4)</param>
+    private static void FuzzifyPISignal(double piSignal, double[] membershipValues)
     {
-        double b_y1;
-        double y2;
-        b_y1 = 0.0;
-        y2 = 0.0;
-        if (x >= parame[1])
-        {
-            b_y1 = 1.0;
-        }
-        if (x < parame[0])
-        {
-            b_y1 = 0.0;
-        }
-        if (parame[0] <= x && x < parame[1] && parame[0] != parame[1])
-        {
-            b_y1 = 1.0 / (parame[1] - parame[0]) * (x - parame[0]);
-        }
-        if (x <= parame[2])
-        {
-            y2 = 1.0;
-        }
-        if (x > parame[3])
-        {
-            y2 = 0.0;
-        }
-        if (parame[2] < x && x <= parame[3] && parame[2] != parame[3])
-        {
-            y2 = 1.0 / (parame[3] - parame[2]) * (parame[3] - x);
-        }
-        return b_y1 < y2 ? b_y1 : y2;
+        // 1. NB (Negative Big): [-∞, -∞, -1.0, -0.5]
+        membershipValues[0] = Fuzzy_trapmf(piSignal, -1.0E+10, -1.0E+10, -1.0, -0.5);
+
+        // 2. Z (Zero): [-0.5, 0.0, 0.5]
+        membershipValues[1] = Fuzzy_trimf(piSignal, -0.5, 0.0, 0.5);
+
+        // 3. PB (Positive Big): [0.5, 1.0, +∞, +∞]
+        membershipValues[2] = Fuzzy_trapmf(piSignal, 0.5, 1.0, 1.0E+10, 1.0E+10);
+
+        // 4. NM (Negative Medium): [-1.0, -0.5, 0.0]
+        membershipValues[3] = Fuzzy_trimf(piSignal, -1.0, -0.5, 0.0);
+
+        // 5. PM (Positive Medium): [0.0, 0.5, 1.0]
+        membershipValues[4] = Fuzzy_trimf(piSignal, 0.0, 0.5, 1.0);
     }
 
-    private static double Fuzzy_trimf(double x, double[] parame)
+    /// <summary>
+    /// Mờ hóa vận tốc V thành 11 tập mờ (dải 0.0 - 3.0)
+    /// Độ phân giải: 0.3 m/s - Cân bằng tốt giữa độ chính xác và hiệu suất
+    /// </summary>
+    /// <param name="velocity">Vận tốc tuyến tính mong muốn (0.0 - 3.0 m/s)</param>
+    /// <param name="membershipValues">Mảng lưu giá trị độ thuộc (vị trí 5-15)</param>
+    private static void FuzzifyVelocity(double velocity, double[] membershipValues)
     {
-        double y;
-        y = 0.0;
-        if (parame[0] != parame[1] && parame[0] < x && x < parame[1])
-        {
-            y = 1.0 / (parame[1] - parame[0]) * (x - parame[0]);
-        }
-        if (parame[1] != parame[2] && parame[1] < x && x < parame[2])
-        {
-            y = 1.0 / (parame[2] - parame[1]) * (parame[2] - x);
-        }
-        if (x == parame[1])
-        {
-            y = 1.0;
-        }
-        return y;
+        // 11 tập mờ phân bố đều từ 0.0 đến 3.0
+        // Khoảng cách giữa các đỉnh: 3.0 / 10 = 0.3 m/s
+
+        // 1. VVS (Very Very Slow): [-∞, -∞, 0.0, 0.3]
+        membershipValues[5] = Fuzzy_trapmf(velocity, -1.0E+9, -1.0E+9, 0.0, 0.3);
+
+        // 2. VS (Very Slow): [0.0, 0.3, 0.6]
+        membershipValues[6] = Fuzzy_trimf(velocity, 0.0, 0.3, 0.6);
+
+        // 3. S- (Slow Low): [0.3, 0.6, 0.9]
+        membershipValues[7] = Fuzzy_trimf(velocity, 0.3, 0.6, 0.9);
+
+        // 4. S (Slow): [0.6, 0.9, 1.2]
+        membershipValues[8] = Fuzzy_trimf(velocity, 0.6, 0.9, 1.2);
+
+        // 5. S+ (Slow High): [0.9, 1.2, 1.5]
+        membershipValues[9] = Fuzzy_trimf(velocity, 0.9, 1.2, 1.5);
+
+        // 6. M (Medium): [1.2, 1.5, 1.8]
+        membershipValues[10] = Fuzzy_trimf(velocity, 1.2, 1.5, 1.8);
+
+        // 7. F- (Fast Low): [1.5, 1.8, 2.1]
+        membershipValues[11] = Fuzzy_trimf(velocity, 1.5, 1.8, 2.1);
+
+        // 8. F (Fast): [1.8, 2.1, 2.4]
+        membershipValues[12] = Fuzzy_trimf(velocity, 1.8, 2.1, 2.4);
+
+        // 9. F+ (Fast High): [2.1, 2.4, 2.7]
+        membershipValues[13] = Fuzzy_trimf(velocity, 2.1, 2.4, 2.7);
+
+        // 10. VF (Very Fast): [2.4, 2.7, 3.0]
+        membershipValues[14] = Fuzzy_trimf(velocity, 2.4, 2.7, 3.0);
+
+        // 11. VVF (Very Very Fast): [2.7, 3.0, +∞, +∞]
+        membershipValues[15] = Fuzzy_trapmf(velocity, 2.7, 3.0, 1.0E+9, 1.0E+9);
     }
 
-    public (double wl, double wr) Fuzzy_step(double v, double w, double TimeSample)
+    // ============================================================================
+    // PHẦN 3: RULE EVALUATION (ĐÁNH GIÁ LUẬT MỜ)
+    // ============================================================================
+
+    /// <summary>
+    /// Đánh giá luật mờ cho một bộ điều khiển
+    /// </summary>
+    /// <param name="inputMembershipValues">Độ thuộc của các đầu vào (16 giá trị: 5 PI + 11 V)</param>
+    /// <param name="ruleAntecedentIndices">Ma trận chỉ số tiền đề (numRules * 2 phần tử)</param>
+    /// <param name="ruleConsequentIndices">Ma trận chỉ số hệ quả (numRules phần tử)</param>
+    /// <param name="outputSingletons">Các giá trị singleton đầu ra</param>
+    /// <param name="numRules">Số lượng luật</param>
+    /// <returns>(weightedSum: tổng có trọng số, totalWeight: tổng trọng số)</returns>
+    private static (double weightedSum, double totalWeight) EvaluateRules(
+        double[] inputMembershipValues,
+        byte[] ruleAntecedentIndices,
+        byte[] ruleConsequentIndices,
+        double[] outputSingletons,
+        int numRules)
     {
-        (double wl, double wr) result = new();
-        double[] inputMFCache = new double[10];
-        double[] outputMFCache = new double[5];
-        double[] outputMFCache_0 = new double[5];
-        double[] tmp = new double[3];
-        double aggregatedOutputs;
-        double rtb_TmpSignalConversionAtSFun_0;
-        double rtb_antecedentOutputs_e;
-        double sumAntecedentOutputs;
-        int ruleID;
-        double[] f = [-1.0E+10, -1.0E+10, -1.0, -0.5];
-        double[] e = [0.5, 1.0, 1.0E+10, 1.0E+10];
-        double[] d = [0.75, 1.0, 1.0E+9, 1.0E+9];
-        double[] c = [-1.0E+9, -1.0E+9, 0.0, 0.25];
-        byte[] b = [ 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 4,
-                         4, 4, 4, 4, 5, 5, 5, 5, 5, 1, 2, 3, 4, 5, 1, 2, 3,
-                         4, 5, 1, 2, 3, 4, 5, 3, 4, 5, 1, 2, 1, 2, 3, 4, 5 ];
-        byte[] b_0 = [1, 1, 2, 1, 1, 2, 3, 5, 1, 4, 5, 5, 5, 5, 5, 2, 1, 1, 1, 1, 5, 5, 5, 5, 5];
-        byte[] b_1 = [  1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 4,
-                            4, 4, 4, 4, 5, 5, 5, 5, 5, 1, 2, 3, 4, 5, 4, 1,
-                            2, 3, 5, 3, 1, 2, 4, 5, 1, 2, 3, 4, 5, 1, 2, 4, 5, 3 ];
-        byte[] b_2 = [5, 5, 5, 5, 5, 1, 2, 3, 5, 4, 2, 1, 1, 1, 1, 5, 5, 5, 5, 5, 1, 1, 1, 1, 2];
-        double inputMFCache_tmp;
-        double inputMFCache_tmp_0;
-        double inputMFCache_tmp_1;
-        double inputMFCache_tmp_2;
-        /* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic  Controller1' */
-        /* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic  Controller' */
-        /* SignalConversion generated from: '<S4>/ SFunction ' incorporates:
-         *  Constant: '<Root>/w'
-         *  DiscreteIntegrator: '<Root>/Discrete-Time Integrator'
-         *  Gain: '<Root>/Gain1'
-         *  MATLAB Function: '<S1>/Evaluate Rule Antecedents'
-         *  MATLAB Function: '<S2>/Evaluate Rule Antecedents'
-         *  SignalConversion generated from: '<S7>/ SFunction '
-         *  Sum: '<Root>/Sum'
-         */
-        DiscreteTimeIntegrator_DSTATE += Gain_I * w * TimeSample;
-        rtb_TmpSignalConversionAtSFun_0 = Gain_P * w + DiscreteTimeIntegrator_DSTATE;
-        /* End of Outputs for SubSystem: '<Root>/Fuzzy Logic  Controller1' */
-        /* MATLAB Function: '<S1>/Evaluate Rule Antecedents' incorporates:
-         *  Constant: '<Root>/v'
-         *  MATLAB Function: '<S2>/Evaluate Rule Antecedents'
-         *  SignalConversion generated from: '<S4>/ SFunction '
-         */
-        sumAntecedentOutputs = 0.0;
-        /* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic  Controller1' */
-        inputMFCache_tmp = Fuzzy_trapmf(rtb_TmpSignalConversionAtSFun_0, f);
-        /* End of Outputs for SubSystem: '<Root>/Fuzzy Logic  Controller1' */
-        inputMFCache[0] = inputMFCache_tmp;
-        tmp[0] = -0.5;
-        tmp[1] = 0.0;
-        tmp[2] = 0.5;
-        inputMFCache[1] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
-        /* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic  Controller1' */
-        inputMFCache_tmp_0 = Fuzzy_trapmf(rtb_TmpSignalConversionAtSFun_0, e);
-        /* End of Outputs for SubSystem: '<Root>/Fuzzy Logic  Controller1' */
-        inputMFCache[2] = inputMFCache_tmp_0;
-        tmp[0] = -1.0;
-        tmp[1] = -0.5;
-        tmp[2] = 0.0;
-        inputMFCache[3] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
-        tmp[0] = 0.0;
-        tmp[1] = 0.5;
-        tmp[2] = 1.0;
-        inputMFCache[4] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
-        tmp[0] = 0.0;
-        tmp[1] = 0.25;
-        tmp[2] = 0.5;
-        inputMFCache[5] = Fuzzy_trimf(v, tmp);
-        tmp[0] = 0.25;
-        tmp[1] = 0.5;
-        tmp[2] = 0.75;
-        inputMFCache[6] = Fuzzy_trimf(v, tmp);
-        /* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic  Controller1' */
-        inputMFCache_tmp_1 = Fuzzy_trapmf(v, d);
-        /* End of Outputs for SubSystem: '<Root>/Fuzzy Logic  Controller1' */
-        inputMFCache[7] = inputMFCache_tmp_1;
-        /* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic  Controller1' */
-        inputMFCache_tmp_2 = Fuzzy_trapmf(v, c);
-        /* End of Outputs for SubSystem: '<Root>/Fuzzy Logic  Controller1' */
-        inputMFCache[8] = inputMFCache_tmp_2;
-        tmp[0] = 0.5;
-        tmp[1] = 0.75;
-        tmp[2] = 1.0;
-        inputMFCache[9] = Fuzzy_trimf(v, tmp);
-        /* MATLAB Function: '<S1>/Evaluate Rule Consequents' */
-        aggregatedOutputs = 0.0;
-        outputMFCache[0] = 0.0;
-        outputMFCache[1] = 0.25;
-        outputMFCache[2] = 0.5;
-        outputMFCache[3] = 0.75;
-        outputMFCache[4] = 1.0;
-        for (ruleID = 0; ruleID < 25; ruleID++)
+        const int VELOCITY_OFFSET = 5;  // Chỉ số bắt đầu của V trong inputMembershipValues
+
+        double weightedSum = 0.0;
+        double totalWeight = 0.0;
+
+        for (int ruleIndex = 0; ruleIndex < numRules; ruleIndex++)
         {
-            /* MATLAB Function: '<S1>/Evaluate Rule Antecedents' */
-            rtb_antecedentOutputs_e = inputMFCache[b[ruleID + 25] + 4] * inputMFCache[b[ruleID] - 1];
-            sumAntecedentOutputs += rtb_antecedentOutputs_e;
-            /* MATLAB Function: '<S1>/Evaluate Rule Consequents' */
-            aggregatedOutputs += outputMFCache[b_0[ruleID] - 1] * rtb_antecedentOutputs_e;
+            // Lấy chỉ số tập mờ cho PI (từ 1-5, cần trừ 1 để thành 0-4)
+            int piMembershipIndex = ruleAntecedentIndices[ruleIndex] - 1;
+
+            // Lấy chỉ số tập mờ cho Velocity (từ 1-11, cộng offset)
+            int velocityMembershipIndex = ruleAntecedentIndices[ruleIndex + numRules] + VELOCITY_OFFSET - 1;
+
+            // Tính độ kích hoạt của luật (AND operator = phép nhân)
+            double ruleActivation = inputMembershipValues[piMembershipIndex]
+                                   * inputMembershipValues[velocityMembershipIndex];
+
+            // Lấy giá trị singleton đầu ra tương ứng (từ 1-11, cần trừ 1)
+            int outputIndex = ruleConsequentIndices[ruleIndex] - 1;
+            double outputValue = outputSingletons[outputIndex];
+
+            // Tích lũy tổng trọng số và tổng có trọng số
+            totalWeight += ruleActivation;
+            weightedSum += outputValue * ruleActivation;
         }
-        /* MATLAB Function: '<S1>/Defuzzify Outputs' incorporates:
-         *  MATLAB Function: '<S1>/Evaluate Rule Antecedents'
-         *  MATLAB Function: '<S1>/Evaluate Rule Consequents'
-         */
-        if (sumAntecedentOutputs == 0.0)
+
+        return (weightedSum, totalWeight);
+    }
+
+    // ============================================================================
+    // PHẦN 4: DEFUZZIFICATION (GIẢI MỜ ĐẦU RA)
+    // ============================================================================
+
+    /// <summary>
+    /// Giải mờ bằng phương pháp trọng tâm (Weighted Average / Center of Gravity)
+    /// Công thức: output = Σ(singleton_i × weight_i) / Σ(weight_i)
+    /// </summary>
+    /// <param name="weightedSum">Tổng đầu ra có trọng số</param>
+    /// <param name="totalWeight">Tổng trọng số của tất cả các luật</param>
+    /// <param name="defaultValue">Giá trị mặc định nếu totalWeight = 0</param>
+    /// <returns>Giá trị đầu ra rõ (crisp output)</returns>
+    private static double Defuzzify(double weightedSum, double totalWeight, double defaultValue = 0.5)
+    {
+        // Nếu không có luật nào được kích hoạt, trả về giá trị mặc định
+        if (totalWeight == 0.0)
         {
-            result.wr = 0.5;
+            return defaultValue;
         }
         else
         {
-            result.wr = 1.0 / sumAntecedentOutputs * aggregatedOutputs;
+            // Tính trọng tâm: output = weightedSum / totalWeight
+            return weightedSum / totalWeight;
         }
-        /* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic  Controller1' */
-        /* MATLAB Function: '<S2>/Evaluate Rule Antecedents' incorporates:
-         *  Constant: '<Root>/v'
-         *  SignalConversion generated from: '<S7>/ SFunction '
-         */
-        sumAntecedentOutputs = 0.0;
-        inputMFCache[0] = inputMFCache_tmp;
-        tmp[0] = -0.5;
-        tmp[1] = 0.0;
-        tmp[2] = 0.5;
-        inputMFCache[1] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
-        inputMFCache[2] = inputMFCache_tmp_0;
-        tmp[0] = -1.0;
-        tmp[1] = -0.5;
-        tmp[2] = 0.0;
-        inputMFCache[3] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
-        tmp[0] = 0.0;
-        tmp[1] = 0.5;
-        tmp[2] = 1.0;
-        inputMFCache[4] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
-        tmp[0] = 0.0;
-        tmp[1] = 0.25;
-        tmp[2] = 0.5;
-        inputMFCache[5] = Fuzzy_trimf(v, tmp);
-        tmp[0] = 0.25;
-        tmp[1] = 0.5;
-        tmp[2] = 0.75;
-        inputMFCache[6] = Fuzzy_trimf(v, tmp);
-        inputMFCache[7] = inputMFCache_tmp_1;
-        inputMFCache[8] = inputMFCache_tmp_2;
-        tmp[0] = 0.5;
-        tmp[1] = 0.75;
-        tmp[2] = 1.0;
-        inputMFCache[9] = Fuzzy_trimf(v, tmp);
-        /* MATLAB Function: '<S2>/Evaluate Rule Consequents' */
-        aggregatedOutputs = 0.0;
-        outputMFCache_0[0] = 0.0;
-        outputMFCache_0[1] = 0.25;
-        outputMFCache_0[2] = 0.5;
-        outputMFCache_0[3] = 0.75;
-        outputMFCache_0[4] = 1.0;
-        for (ruleID = 0; ruleID < 25; ruleID++)
-        {
-            /* MATLAB Function: '<S2>/Evaluate Rule Antecedents' */
-            rtb_antecedentOutputs_e = inputMFCache[b_1[ruleID + 25] + 4] * inputMFCache[b_1[ruleID] - 1];
-            sumAntecedentOutputs += rtb_antecedentOutputs_e;
-            /* MATLAB Function: '<S2>/Evaluate Rule Consequents' */
-            aggregatedOutputs += outputMFCache_0[b_2[ruleID] - 1] *
-              rtb_antecedentOutputs_e;
-        }
-        /* MATLAB Function: '<S2>/Defuzzify Outputs' incorporates:
-         *  MATLAB Function: '<S2>/Evaluate Rule Antecedents'
-         *  MATLAB Function: '<S2>/Evaluate Rule Consequents'
-         */
-        if (sumAntecedentOutputs == 0.0)
-        {
-            result.wl = 0.5;
-        }
-        else
-        {
-            result.wl = 1.0 / sumAntecedentOutputs * aggregatedOutputs;
-        }
-        return result;
+    }
+
+    // ============================================================================
+    // PHẦN 5: BẢNG LUẬT VÀ CẤU HÌNH
+    // ============================================================================
+
+    /// <summary>
+    /// Bảng chỉ số tiền đề luật cho bánh phải (Rule Antecedent Indices - Right Wheel)
+    /// 110 phần tử: 55 cho PI + 55 cho Velocity
+    /// Hệ thống: 5 tập mờ PI × 11 tập mờ V = 55 luật
+    /// Giá trị từ 1-5 cho PI, 1-11 cho V
+    /// </summary>
+    private static readonly byte[] RULE_ANTECEDENT_INDICES_RIGHT =
+    [
+        // 55 phần tử đầu: Chỉ số tập mờ của PI Signal (1-5)
+        // Mỗi tập PI lặp lại 11 lần (cho 11 mức vận tốc)
+        1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,  // Luật 1-11:  PI = NB (tập 1)
+        2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2,  // Luật 12-22: PI = Z  (tập 2)
+        3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3,  // Luật 23-33: PI = PB (tập 3)
+        4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4,  // Luật 34-44: PI = NM (tập 4)
+        5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,  // Luật 45-55: PI = PM (tập 5)
+    
+        // 55 phần tử sau: Chỉ số tập mờ của Velocity (1-11)
+        // Lặp lại theo pattern: VVS, VS, S-, S, S+, M, F-, F, F+, VF, VVF
+        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,  // Luật 1-11:  V = VVS đến VVF
+        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,  // Luật 12-22: V = VVS đến VVF
+        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,  // Luật 23-33: V = VVS đến VVF
+        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,  // Luật 34-44: V = VVS đến VVF
+        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11   // Luật 45-55: V = VVS đến VVF
+    ];
+
+    /// <summary>
+    /// Bảng chỉ số hệ quả cho bánh phải (Rule Consequent Indices - Right Wheel)
+    /// 55 phần tử tương ứng với 55 luật
+    /// Giá trị từ 1-11 tương ứng với 11 mức tốc độ đầu ra (0.0 - 3.0)
+    /// 
+    /// Logic bánh phải:
+    /// - PI âm (NB, NM): Bánh phải chậm hơn → Robot rẽ trái
+    /// - PI = 0 (Z): Bánh phải theo vận tốc V → Robot đi thẳng
+    /// - PI dương (PB, PM): Bánh phải nhanh hơn → Robot rẽ phải
+    /// </summary>
+    private static readonly byte[] RULE_CONSEQUENT_INDICES_RIGHT =
+    [   
+        // PI = NB (Negative Big) - Bánh phải RẤT CHẬM (rẽ trái mạnh)
+        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
+             1,  1,  1,  2,  2,  3,  3,  4,  5,  6,  7,    // Luật 1-11
+    
+        // PI = Z (Zero) - Bánh phải THEO VẬN TỐC (đi thẳng)
+        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
+             2,  2,  3,  4,  5,  6,  7,  8,  9,  10, 11,   // Luật 12-22
+    
+        // PI = PB (Positive Big) - Bánh phải RẤT NHANH (rẽ phải mạnh)
+        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
+             4,  5,  6,  7,  8,  9,  10, 10, 11, 11, 11,   // Luật 23-33
+    
+        // PI = NM (Negative Medium) - Bánh phải HƠI CHẬM (rẽ trái nhẹ)
+        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
+             1,  2,  2,  3,  4,  5,  6,  7,  8,  9,  10,   // Luật 34-44
+    
+        // PI = PM (Positive Medium) - Bánh phải HƠI NHANH (rẽ phải nhẹ)
+        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
+             2,  3,  4,  5,  6,  7,  8,  8,  9,  10, 11    // Luật 45-55
+    ];
+
+    /// <summary>
+    /// Bảng chỉ số tiền đề luật cho bánh trái (Rule Antecedent Indices - Left Wheel)
+    /// 110 phần tử: 55 cho PI + 55 cho Velocity
+    /// </summary>
+    private static readonly byte[] RULE_ANTECEDENT_INDICES_LEFT =
+    [
+        // 55 phần tử đầu: Chỉ số tập mờ của PI Signal (1-5)
+        1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,  // Luật 1-11:  PI = NB (tập 1)
+        2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2,  // Luật 12-22: PI = Z  (tập 2)
+        3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3,  // Luật 23-33: PI = PB (tập 3)
+        4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4,  // Luật 34-44: PI = NM (tập 4)
+        5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,  // Luật 45-55: PI = PM (tập 5)
+    
+        // 55 phần tử sau: Chỉ số tập mờ của Velocity (1-11)
+        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,  // Luật 1-11
+        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,  // Luật 12-22
+        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,  // Luật 23-33
+        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,  // Luật 34-44
+        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11   // Luật 45-55
+    ];
+
+    /// <summary>
+    /// Bảng chỉ số hệ quả cho bánh trái (Rule Consequent Indices - Left Wheel)
+    /// 55 phần tử tương ứng với 55 luật
+    /// 
+    /// Logic bánh trái: NGƯỢC LẠI với bánh phải
+    /// - PI âm (NB, NM): Bánh trái nhanh hơn → Robot rẽ trái
+    /// - PI = 0 (Z): Bánh trái theo vận tốc V → Robot đi thẳng
+    /// - PI dương (PB, PM): Bánh trái chậm hơn → Robot rẽ phải
+    /// </summary>
+    private static readonly byte[] RULE_CONSEQUENT_INDICES_LEFT =
+    [
+        // PI = NB (Negative Big) - Bánh trái RẤT NHANH (rẽ trái mạnh)
+        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
+             4,  5,  6,  7,  8,  9,  10, 10, 11, 11, 11,   // Luật 1-11
+    
+        // PI = Z (Zero) - Bánh trái THEO VẬN TỐC (đi thẳng)
+        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
+             2,  2,  3,  4,  5,  6,  7,  8,  9,  10, 11,   // Luật 12-22
+    
+        // PI = PB (Positive Big) - Bánh trái RẤT CHẬM (rẽ phải mạnh)
+        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
+             1,  1,  1,  2,  2,  3,  3,  4,  5,  6,  7,    // Luật 23-33
+    
+        // PI = NM (Negative Medium) - Bánh trái HƠI NHANH (rẽ trái nhẹ)
+        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
+             3,  4,  5,  6,  7,  8,  8,  9,  10, 11, 11,   // Luật 34-44
+    
+        // PI = PM (Positive Medium) - Bánh trái HƠI CHẬM (rẽ phải nhẹ)
+        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
+             1,  2,  2,  3,  4,  4,  5,  6,  7,  8,  9,    // Luật 45-55
+    ];
+
+    /// <summary>
+    /// Các mức đầu ra singleton (Output Singletons)
+    /// 11 mức tốc độ từ 0.0 đến 3.0 m/s
+    /// Độ phân giải: 3.0 / 10 = 0.3 m/s
+    /// </summary>
+    private static readonly double[] OUTPUT_SINGLETON_LEVELS =
+    [
+        0.0,   // Mức 1:  Dừng hoàn toàn (0%)
+        0.3,   // Mức 2:  Rất chậm (10%)
+        0.6,   // Mức 3:  Chậm (20%)
+        0.9,   // Mức 4:  Hơi chậm (30%)
+        1.2,   // Mức 5:  Chậm vừa (40%)
+        1.5,   // Mức 6:  Trung bình (50%) - Điểm chuẩn
+        1.8,   // Mức 7:  Hơi nhanh (60%)
+        2.1,   // Mức 8:  Nhanh (70%)
+        2.4,   // Mức 9:  Nhanh vừa (80%)
+        2.7,   // Mức 10: Rất nhanh (90%)
+        3.0    // Mức 11: Tối đa (100%)
+    ];
+
+    // ============================================================================
+    // PHẦN 6: HÀM CHÍNH - BỘ ĐIỀU KHIỂN PI + FUZZY
+    // ============================================================================
+
+    // Biến trạng thái bộ tích phân
+    private static double integratorState = 0.0;
+
+    // Hệ số bộ điều khiển PI
+    private static double proportionalGain = 1.0;  // Hệ số tỷ lệ (Kp)
+    private static double integralGain = 0.1;      // Hệ số tích phân (Ki)
+
+    /// <summary>
+    /// Hàm điều khiển chính - Tính toán tốc độ bánh trái và phải
+    /// Sử dụng bộ điều khiển PI kết hợp với logic mờ
+    /// 
+    /// CẤU HÌNH PHIÊN BẢN 3.0 - 11 TẬP MỜ:
+    /// - PI Signal: 5 tập mờ (NB, Z, PB, NM, PM)
+    /// - Velocity: 11 tập mờ (VVS, VS, S-, S, S+, M, F-, F, F+, VF, VVF)
+    /// - Tổng số luật: 5 × 11 = 55 luật cho mỗi bánh
+    /// - Dải đầu ra: 0.0 - 3.0 m/s
+    /// - Độ phân giải: 0.3 m/s (tối ưu - cân bằng giữa độ mịn và hiệu suất)
+    /// </summary>
+    /// <param name="desiredVelocity">Vận tốc tuyến tính mong muốn (0.0 - 3.0 m/s)</param>
+    /// <param name="desiredAngularVelocity">Vận tốc góc mong muốn (rad/s)</param>
+    /// <param name="samplingTime">Chu kỳ lấy mẫu (giây, khuyến nghị 0.01s)</param>
+    /// <returns>(leftWheelSpeed, rightWheelSpeed): Tốc độ bánh trái và phải trong [0.0, 3.0]</returns>
+    public (double leftWheelSpeed, double rightWheelSpeed) Fuzzy_step(
+        double desiredVelocity,
+        double desiredAngularVelocity,
+        double samplingTime)
+    {
+        const int NUM_INPUT_MEMBERSHIPS = 16;   // 5 cho PI + 11 cho Velocity
+        const int NUM_OUTPUT_LEVELS = 11;       // 11 mức đầu ra (0.0 - 3.0)
+        const int NUM_RULES = 55;               // 5 × 11 = 55 luật
+
+        // Khởi tạo mảng lưu độ thuộc đầu vào
+        double[] inputMembershipValues = new double[NUM_INPUT_MEMBERSHIPS];
+
+        // Khởi tạo mảng lưu các mức đầu ra
+        double[] outputLevels = new double[NUM_OUTPUT_LEVELS];
+        Array.Copy(OUTPUT_SINGLETON_LEVELS, outputLevels, NUM_OUTPUT_LEVELS);
+
+        // ========== BƯỚC 1: BỘ ĐIỀU KHIỂN PI ==========
+        // Cập nhật trạng thái tích phân: I(t) = I(t-1) + Ki * error * dt
+        integratorState += integralGain * desiredAngularVelocity * samplingTime;
+
+        // Chống bão hòa tích phân (Anti-windup)
+        integratorState = Math.Clamp(integratorState, -1.5, 1.5);
+
+        // Tính tín hiệu điều khiển PI: u(t) = Kp * error + I(t)
+        double piControlSignal = proportionalGain * desiredAngularVelocity + integratorState;
+
+        // ========== BƯỚC 2: MỜ HÓA ĐẦU VÀO ==========
+        FuzzifyPISignal(piControlSignal, inputMembershipValues);
+        FuzzifyVelocity(desiredVelocity, inputMembershipValues);
+
+        // ========== BƯỚC 3: TÍNH TOÁN BÁNH PHẢI ==========
+        var (weightedSum_Right, totalWeight_Right) = EvaluateRules(
+            inputMembershipValues,
+            RULE_ANTECEDENT_INDICES_RIGHT,
+            RULE_CONSEQUENT_INDICES_RIGHT,
+            outputLevels,
+            NUM_RULES
+        );
+
+        double rightWheelSpeed = Defuzzify(weightedSum_Right, totalWeight_Right, defaultValue: 1.5);
+
+        // ========== BƯỚC 4: TÍNH TOÁN BÁNH TRÁI ==========
+        var (weightedSum_Left, totalWeight_Left) = EvaluateRules(
+            inputMembershipValues,
+            RULE_ANTECEDENT_INDICES_LEFT,
+            RULE_CONSEQUENT_INDICES_LEFT,
+            outputLevels,
+            NUM_RULES
+        );
+
+        double leftWheelSpeed = Defuzzify(weightedSum_Left, totalWeight_Left, defaultValue: 1.5);
+
+        // ========== BƯỚC 5: GIỚI HẠN AN TOÀN ==========
+        // Đảm bảo tốc độ không vượt quá giới hạn
+        leftWheelSpeed = Math.Clamp(leftWheelSpeed, 0.0, 3.0);
+        rightWheelSpeed = Math.Clamp(rightWheelSpeed, 0.0, 3.0);
+
+        // ========== BƯỚC 6: TRẢ VỀ KẾT QUẢ ==========
+        return (leftWheelSpeed, rightWheelSpeed);
+    }
+
+    // ============================================================================
+    // PHẦN 7: HÀM HỖ TRỢ
+    // ============================================================================
+
+    /// <summary>
+    /// Reset trạng thái bộ tích phân về 0
+    /// Nên gọi khi bắt đầu chu kỳ điều khiển mới hoặc khi cần reset hệ thống
+    /// </summary>
+    public void ResetIntegrator()
+    {
+        integratorState = 0.0;
+    }
+
+    /// <summary>
+    /// Thiết lập hệ số cho bộ điều khiển PI
+    /// </summary>
+    /// <param name="kp">Hệ số tỷ lệ (Proportional Gain) - Phản ứng với sai số hiện tại</param>
+    /// <param name="ki">Hệ số tích phân (Integral Gain) - Loại bỏ sai số tích lũy</param>
+    public FuzzyLogic WithPIGains(double kp, double ki)
+    {
+        proportionalGain = kp;
+        integralGain = ki;
+        return this;
+    }
+
+    /// <summary>
+    /// Lấy trạng thái hiện tại của bộ tích phân
+    /// Hữu ích cho việc debug và giám sát hệ thống
+    /// </summary>
+    /// <returns>Giá trị tích phân hiện tại</returns>
+    public double GetIntegratorState()
+    {
+        return integratorState;
+    }
+
+    /// <summary>
+    /// Lấy hệ số PI hiện tại
+    /// </summary>
+    /// <returns>(Kp, Ki): Hệ số tỷ lệ và tích phân</returns>
+    public (double Kp, double Ki) GetPIGains()
+    {
+        return (proportionalGain, integralGain);
     }
 }

RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigation.cs

@@ -22,7 +22,9 @@ public class SimulationNavigation : INavigation, IDisposable
     protected bool NavDriving = false;
 
     protected const int CycleHandlerMilliseconds = 50;
-    private WatchTimer<SimulationNavigation>? NavigationTimer;
+    private const double Scale = 2;
+    //private WatchTimer<SimulationNavigation>? NavigationTimer;
+    private HighPrecisionTimer<SimulationNavigation>? NavigationTimer;
 
     protected double TargetAngle = 0;
     protected PID? RotatePID;
@@ -62,7 +64,7 @@ public class SimulationNavigation : INavigation, IDisposable
 
     protected void HandleNavigationStart()
     {
-        NavigationTimer = new(CycleHandlerMilliseconds, NavigationHandler, Logger);
+        NavigationTimer = new((int)(CycleHandlerMilliseconds / Scale), NavigationHandler, Logger);
         NavigationTimer.Start();
     }
 
@@ -91,8 +93,10 @@ public class SimulationNavigation : INavigation, IDisposable
         NavigationPath = PathPlanner.PathSplit(pathDirection, edges);
 
         MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
-        MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
-        MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath([.. NavigationPath]);
+        MoveFuzzy = new FuzzyLogic(); 
+        MovePurePursuit = new PurePursuit()
+            .WithLookheadDistance(0.35) 
+            .WithPath([.. NavigationPath]);
 
         (NavigationNode node, int index) = MovePurePursuit.GetOnNode(Visualization.X, Visualization.Y);
         if(index >= NavigationPath.Length - 1) return;
@@ -140,9 +144,13 @@ public class SimulationNavigation : INavigation, IDisposable
             StartNodeId = currentRobotNode.NodeId,
             EndNodeId = goalNode.NodeId,
         }]);
-        MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
-        MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
-        MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath(NavigationPath);
+        
+        MovePID = new PID().WithKp(1.5).WithKi(0.0001).WithKd(0.8);
+        MoveFuzzy = new FuzzyLogic();  
+        MovePurePursuit = new PurePursuit()
+            .WithLookheadDistance(0.25) 
+            .WithPath(NavigationPath);
+        
         double Angle = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X);
         Rotate(Angle * 180 / Math.PI);
         UpdateOrder(goalNode.NodeId);

RobotApp/Services/WatchTimer.cs

@@ -5,31 +5,54 @@ namespace RobotApp.Services;
 public class WatchTimer<T>(int Interval, Action Callback, Logger<T>? Logger) : IDisposable where T : class
 {
     private Timer? Timer;
-    private readonly Stopwatch Watch = new();
     public bool Disposed;
 
+    private long NextDueTime;
+    private readonly Lock Lock = new();
+
     private void Handler(object? state)
     {
         try
         {
-            Watch.Restart();
-
-            Callback.Invoke();
-
-            Watch.Stop();
-            if (Watch.ElapsedMilliseconds >= Interval || Interval - Watch.ElapsedMilliseconds <= 50)
+            bool shouldRun = false;
+            lock (Lock)
             {
-                if(Watch.ElapsedMilliseconds > Interval) Logger?.Warning($"WatchTimer Warning: Elapsed time {Watch.ElapsedMilliseconds}ms exceeds interval {Interval}ms.");
-                Timer?.Change(Interval, Timeout.Infinite);
+                if (Disposed) return;
+                long now = GetCurrentTimeMs();
+                if (now >= NextDueTime)
+                {
+                    shouldRun = true;
+                    long scheduledTime = NextDueTime;
+                    NextDueTime += Interval;
+
+                    if (now - scheduledTime > Interval / 2)
+                    {
+                        NextDueTime = now + Interval;
+                        Logger?.Warning($"WatchTimer Warning: Elapsed time {now - scheduledTime + Interval}ms exceeds interval {Interval}ms.");
+                    }
+                }
             }
-            else
+
+            if (shouldRun)
             {
-                Timer?.Change(Interval - Watch.ElapsedMilliseconds, Timeout.Infinite);
+                var sw = Stopwatch.StartNew();
+                try { Callback.Invoke(); }
+                catch (Exception ex) { Logger?.Error($"Callback error: {ex}"); }
+                sw.Stop();
+            }
+
+            lock (Lock)
+            {
+                if (Disposed) return;
+                long now = GetCurrentTimeMs();
+                long delay = NextDueTime - now;
+                if (delay < 0) delay = 0;
+                Timer?.Change(delay, Timeout.Infinite);
             }
         }
         catch (Exception ex)
         {
-            Logger?.Error($"WatchTimer Error: {ex}");
+            Logger?.Error($"WatchTimerAsync Error: {ex}");
             Timer?.Change(Interval, Timeout.Infinite);
         }
     }
@@ -38,8 +61,12 @@ public class WatchTimer<T>(int Interval, Action Callback, Logger<T>? Logger) : I
     {
         if (!Disposed)
         {
-            Timer = new Timer(Handler, null, Timeout.Infinite, Timeout.Infinite);
-            Timer.Change(Interval, 0);
+            lock (Lock)
+            {
+                NextDueTime = GetCurrentTimeMs() + Interval;
+                Timer = new Timer(Handler, null, Timeout.Infinite, Timeout.Infinite);
+                Timer.Change(Interval, Timeout.Infinite);
+            }
         }
         else throw new ObjectDisposedException(nameof(WatchTimer<T>));
     }
@@ -47,14 +74,23 @@ public class WatchTimer<T>(int Interval, Action Callback, Logger<T>? Logger) : I
     public void Stop()
     {
         if (Disposed) return;
+
         if (Timer != null)
         {
-            Timer.Change(Timeout.Infinite, Timeout.Infinite);
-            Timer.Dispose();
-            Timer = null;
+            lock (Lock)
+            {
+                Timer.Change(Timeout.Infinite, Timeout.Infinite);
+                Timer.Dispose();
+                Timer = null;
+            }
         }
     }
 
+    private static long GetCurrentTimeMs()
+    {
+        return Stopwatch.GetTimestamp() * 1000 / Stopwatch.Frequency;
+    }
+
     public void Dispose()
     {
         Dispose(true);

RobotApp/Services/WatchTimerAsync.cs

@@ -5,25 +5,49 @@ namespace RobotApp.Services;
 public class WatchTimerAsync<T>(int Interval, Func<Task> Callback, Logger<T>? Logger) : IDisposable where T : class
 {
     private Timer? Timer;
-    private readonly Stopwatch Watch = new();
     public bool Disposed;
 
+    private long NextDueTime;
+    private readonly Lock Lock = new();
+
     private async void Handler(object? state)
     {
         try
         {
-            Watch.Restart();
-
-            await Callback.Invoke();
-
-            Watch.Stop();
-            if (Watch.ElapsedMilliseconds >= Interval || Interval - Watch.ElapsedMilliseconds <= 50)
+            bool shouldRun = false;
+            lock (Lock)
             {
-                Timer?.Change(Interval, Timeout.Infinite);
+                if (Disposed) return;
+                long now = GetCurrentTimeMs();
+                if (now >= NextDueTime)
+                {
+                    shouldRun = true;
+                    long scheduledTime = NextDueTime;
+                    NextDueTime += Interval;
+
+                    if (now - scheduledTime > Interval / 2)
+                    {
+                        NextDueTime = now + Interval;
+                        Logger?.Warning($"WatchTimer Warning: Elapsed time {now - scheduledTime + Interval}ms exceeds interval {Interval}ms.");
+                    }
+                }
             }
-            else
+
+            if (shouldRun)
             {
-                Timer?.Change(Interval - Watch.ElapsedMilliseconds, Timeout.Infinite);
+                var sw = Stopwatch.StartNew();
+                try { await Callback.Invoke(); }
+                catch (Exception ex) { Logger?.Error($"Callback error: {ex}"); }
+                sw.Stop();
+            }
+
+            lock (Lock)
+            {
+                if (Disposed) return;
+                long now = GetCurrentTimeMs();
+                long delay = NextDueTime - now;
+                if (delay < 0) delay = 0;
+                Timer?.Change(delay, Timeout.Infinite);
             }
         }
         catch (Exception ex)
@@ -37,23 +61,36 @@ public class WatchTimerAsync<T>(int Interval, Func<Task> Callback, Logger<T>? Lo
     {
         if (!Disposed)
         {
-            Timer = new Timer(Handler, null, Timeout.Infinite, Timeout.Infinite);
-            Timer.Change(Interval, 0);
+            lock (Lock)
+            {
+                NextDueTime = GetCurrentTimeMs() + Interval;
+                Timer = new Timer(Handler, null, Timeout.Infinite, Timeout.Infinite);
+                Timer.Change(Interval, Timeout.Infinite);
+            }
         }
-        else throw new ObjectDisposedException(nameof(WatchTimerAsync<T>));
+        else throw new ObjectDisposedException(nameof(WatchTimer<T>));
     }
 
     public void Stop()
     {
         if (Disposed) return;
+
         if (Timer != null)
         {
-            Timer.Change(Timeout.Infinite, Timeout.Infinite);
-            Timer.Dispose();
-            Timer = null;
+            lock (Lock)
+            {
+                Timer.Change(Timeout.Infinite, Timeout.Infinite);
+                Timer.Dispose();
+                Timer = null;
+            }
         }
     }
 
+    private static long GetCurrentTimeMs()
+    {
+        return Stopwatch.GetTimestamp() * 1000 / Stopwatch.Frequency;
+    }
+
     public void Dispose()
     {
         Dispose(true);