diff --git a/RobotApp/RobotApp.csproj b/RobotApp/RobotApp.csproj
index 59b0445..9809486 100644
--- a/RobotApp/RobotApp.csproj
+++ b/RobotApp/RobotApp.csproj
@@ -5,6 +5,7 @@
enable
enable
aspnet-RobotApp-1f61caa2-bbbb-40cd-88b6-409b408a84ea
+ true
diff --git a/RobotApp/Services/HighPrecisionTimer.cs b/RobotApp/Services/HighPrecisionTimer.cs
new file mode 100644
index 0000000..8e5ad70
--- /dev/null
+++ b/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(int Interval, Action Callback, Logger? 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));
+ }
+
+ 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);
+ }
+}
diff --git a/RobotApp/Services/Robot/RobotOrderController.cs b/RobotApp/Services/Robot/RobotOrderController.cs
index 73486e9..a6432e7 100644
--- a/RobotApp/Services/Robot/RobotOrderController.cs
+++ b/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()
diff --git a/RobotApp/Services/Robot/Simulation/Navigation/Algorithm/FuzzyLogic.cs b/RobotApp/Services/Robot/Simulation/Navigation/Algorithm/FuzzyLogic.cs
index 65fb067..5215e47 100644
--- a/RobotApp/Services/Robot/Simulation/Navigation/Algorithm/FuzzyLogic.cs
+++ b/RobotApp/Services/Robot/Simulation/Navigation/Algorithm/FuzzyLogic.cs
@@ -151,41 +151,48 @@ public class FuzzyLogic
}
///
- /// Mờ hóa vận tốc V thành 9 tập mờ (dải 0.0 - 2.0)
+ /// 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
///
- /// Vận tốc tuyến tính mong muốn (0.0 - 2.0 m/s)
- /// Mảng lưu giá trị độ thuộc (vị trí 5-13)
+ /// Vận tốc tuyến tính mong muốn (0.0 - 3.0 m/s)
+ /// Mảng lưu giá trị độ thuộc (vị trí 5-15)
private static void FuzzifyVelocity(double velocity, double[] membershipValues)
{
- // Phân chia 9 tập mờ đều cho dải 0.0 - 2.0
- // Mỗi tập mờ cách nhau 0.25, overlap 50%
+ // 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.25]
- membershipValues[5] = Fuzzy_trapmf(velocity, -1.0E+9, -1.0E+9, 0.0, 0.25);
+ // 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.25, 0.5]
- membershipValues[6] = Fuzzy_trimf(velocity, 0.0, 0.25, 0.5);
+ // 2. VS (Very Slow): [0.0, 0.3, 0.6]
+ membershipValues[6] = Fuzzy_trimf(velocity, 0.0, 0.3, 0.6);
- // 3. S (Slow): [0.25, 0.5, 0.75]
- membershipValues[7] = Fuzzy_trimf(velocity, 0.25, 0.5, 0.75);
+ // 3. S- (Slow Low): [0.3, 0.6, 0.9]
+ membershipValues[7] = Fuzzy_trimf(velocity, 0.3, 0.6, 0.9);
- // 4. SM (Slow-Medium): [0.5, 0.75, 1.0]
- membershipValues[8] = Fuzzy_trimf(velocity, 0.5, 0.75, 1.0);
+ // 4. S (Slow): [0.6, 0.9, 1.2]
+ membershipValues[8] = Fuzzy_trimf(velocity, 0.6, 0.9, 1.2);
- // 5. M (Medium): [0.75, 1.0, 1.25]
- membershipValues[9] = Fuzzy_trimf(velocity, 0.75, 1.0, 1.25);
+ // 5. S+ (Slow High): [0.9, 1.2, 1.5]
+ membershipValues[9] = Fuzzy_trimf(velocity, 0.9, 1.2, 1.5);
- // 6. MF (Medium-Fast): [1.0, 1.25, 1.5]
- membershipValues[10] = Fuzzy_trimf(velocity, 1.0, 1.25, 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): [1.25, 1.5, 1.75]
- membershipValues[11] = Fuzzy_trimf(velocity, 1.25, 1.5, 1.75);
+ // 7. F- (Fast Low): [1.5, 1.8, 2.1]
+ membershipValues[11] = Fuzzy_trimf(velocity, 1.5, 1.8, 2.1);
- // 8. VF (Very Fast): [1.5, 1.75, 2.0]
- membershipValues[12] = Fuzzy_trimf(velocity, 1.5, 1.75, 2.0);
+ // 8. F (Fast): [1.8, 2.1, 2.4]
+ membershipValues[12] = Fuzzy_trimf(velocity, 1.8, 2.1, 2.4);
- // 9. VVF (Very Very Fast): [1.75, 2.0, +∞, +∞]
- membershipValues[13] = Fuzzy_trapmf(velocity, 1.75, 2.0, 1.0E+9, 1.0E+9);
+ // 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);
}
// ============================================================================
@@ -195,7 +202,7 @@ public class FuzzyLogic
///
/// Đánh giá luật mờ cho một bộ điều khiển
///
- /// Độ thuộc của các đầu vào (14 giá trị: 5 PI + 9 V)
+ /// Độ thuộc của các đầu vào (16 giá trị: 5 PI + 11 V)
/// Ma trận chỉ số tiền đề (numRules * 2 phần tử)
/// Ma trận chỉ số hệ quả (numRules phần tử)
/// Các giá trị singleton đầu ra
@@ -218,14 +225,14 @@ public class FuzzyLogic
// 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-9, cộng offset)
+ // 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-9, cần trừ 1)
+ // 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];
@@ -269,120 +276,133 @@ public class FuzzyLogic
///
/// Bảng chỉ số tiền đề luật cho bánh phải (Rule Antecedent Indices - Right Wheel)
- /// 90 phần tử: 45 cho PI + 45 cho Velocity
- /// Hệ thống: 5 tập mờ PI × 9 tập mờ V = 45 luật
- /// Giá trị từ 1-5 cho PI, 1-9 cho V
+ /// 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
///
- private readonly byte[] RULE_ANTECEDENT_INDICES_RIGHT =
+ private static readonly byte[] RULE_ANTECEDENT_INDICES_RIGHT =
[
- // 45 phần tử đầu: Chỉ số tập mờ của PI Signal (1-5)
- // Mỗi tập PI lặp lại 9 lần (cho 9 mức vận tốc)
- 1, 1, 1, 1, 1, 1, 1, 1, 1, // Luật 1-9: PI = NB (tập 1)
- 2, 2, 2, 2, 2, 2, 2, 2, 2, // Luật 10-18: PI = Z (tập 2)
- 3, 3, 3, 3, 3, 3, 3, 3, 3, // Luật 19-27: PI = PB (tập 3)
- 4, 4, 4, 4, 4, 4, 4, 4, 4, // Luật 28-36: PI = NM (tập 4)
- 5, 5, 5, 5, 5, 5, 5, 5, 5, // Luật 37-45: PI = PM (tập 5)
+ // 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)
- // 45 phần tử sau: Chỉ số tập mờ của Velocity (1-9)
- // Lặp lại theo pattern: VVS, VS, S, SM, M, MF, F, VF, VVF
- 1, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 1-9: V = VVS đến VVF
- 1, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 10-18: V = VVS đến VVF
- 1, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 19-27: V = VVS đến VVF
- 1, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 28-36: V = VVS đến VVF
- 1, 2, 3, 4, 5, 6, 7, 8, 9 // Luật 37-45: V = VVS đến VVF
+ // 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
];
///
/// Bảng chỉ số hệ quả cho bánh phải (Rule Consequent Indices - Right Wheel)
- /// 45 phần tử tương ứng với 45 luật
- /// Giá trị từ 1-9 tương ứng với 9 mức tốc độ đầu ra (0.0 - 2.0)
+ /// 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:
- /// - Khi PI âm (NB, NM): Bánh phải chậm hơn (robot rẽ trái)
- /// - Khi PI = 0 (Z): Bánh phải tỷ lệ với vận tốc V
- /// - Khi PI dương (PB, PM): Bánh phải nhanh hơn (robot rẽ phải)
+ /// 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
///
- private readonly byte[] RULE_CONSEQUENT_INDICES_RIGHT =
- [
- // PI = NB (Negative Big) - Bánh phải chậm
- 1, 1, 2, 2, 3, 3, 4, 5, 6, // Luật 1-9: V thấp->cao → Output 1-6
+ 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 thẳng
- 2, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 10-18: V thấp->cao → Output 2-9
+ // 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 nhanh
- 4, 5, 6, 7, 8, 9, 9, 9, 9, // Luật 19-27: V thấp->cao → Output 4-9
+ // 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
- 1, 2, 2, 3, 4, 4, 5, 6, 7, // Luật 28-36: V thấp->cao → Output 1-7
+ // 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
- 3, 4, 5, 6, 7, 8, 8, 9, 9 // Luật 37-45: V thấp->cao → Output 3-9
+ // 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
];
///
/// Bảng chỉ số tiền đề luật cho bánh trái (Rule Antecedent Indices - Left Wheel)
- /// 90 phần tử: 45 cho PI + 45 cho Velocity
+ /// 110 phần tử: 55 cho PI + 55 cho Velocity
///
- private readonly byte[] RULE_ANTECEDENT_INDICES_LEFT =
+ private static readonly byte[] RULE_ANTECEDENT_INDICES_LEFT =
[
- // 45 phần tử đầu: Chỉ số tập mờ của PI Signal (1-5)
- 1, 1, 1, 1, 1, 1, 1, 1, 1, // Luật 1-9: PI = NB (tập 1)
- 2, 2, 2, 2, 2, 2, 2, 2, 2, // Luật 10-18: PI = Z (tập 2)
- 3, 3, 3, 3, 3, 3, 3, 3, 3, // Luật 19-27: PI = PB (tập 3)
- 4, 4, 4, 4, 4, 4, 4, 4, 4, // Luật 28-36: PI = NM (tập 4)
- 5, 5, 5, 5, 5, 5, 5, 5, 5, // Luật 37-45: PI = PM (tập 5)
+ // 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)
- // 45 phần tử sau: Chỉ số tập mờ của Velocity (1-9)
- 1, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 1-9: V = VVS đến VVF
- 1, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 10-18: V = VVS đến VVF
- 1, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 19-27: V = VVS đến VVF
- 1, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 28-36: V = VVS đến VVF
- 1, 2, 3, 4, 5, 6, 7, 8, 9 // Luật 37-45: V = VVS đến VVF
+ // 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
];
///
/// Bảng chỉ số hệ quả cho bánh trái (Rule Consequent Indices - Left Wheel)
- /// 45 phần tử tương ứng với 45 luật
+ /// 55 phần tử tương ứng với 55 luật
///
- /// Logic: NGƯỢC LẠI với bánh phải
- /// - Khi PI âm (NB, NM): Bánh trái nhanh hơn (robot rẽ trái)
- /// - Khi PI = 0 (Z): Bánh trái tỷ lệ với vận tốc V
- /// - Khi PI dương (PB, PM): Bánh trái chậm hơn (robot rẽ phải)
+ /// 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
///
- private readonly byte[] RULE_CONSEQUENT_INDICES_LEFT =
+ private static readonly byte[] RULE_CONSEQUENT_INDICES_LEFT =
[
- // PI = NB (Negative Big) - Bánh trái nhanh
- 4, 5, 6, 7, 8, 9, 9, 9, 9, // Luật 1-9: V thấp->cao → Output 4-9
+ // 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 thẳng
- 2, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 10-18: V thấp->cao → Output 2-9
+ // 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 chậm
- 1, 1, 2, 2, 3, 3, 4, 5, 6, // Luật 19-27: V thấp->cao → Output 1-6
+ // 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
- 3, 4, 5, 6, 7, 8, 8, 9, 9, // Luật 28-36: V thấp->cao → Output 3-9
+ // 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
- 1, 2, 2, 3, 4, 4, 5, 6, 7 // Luật 37-45: V thấp->cao → Output 1-7
+ // 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
];
///
/// Các mức đầu ra singleton (Output Singletons)
- /// 9 mức tốc độ từ 0.0 đến 2.0 (tăng gấp đôi so với phiên bản cũ)
+ /// 11 mức tốc độ từ 0.0 đến 3.0 m/s
+ /// Độ phân giải: 3.0 / 10 = 0.3 m/s
///
- private readonly double[] OUTPUT_SINGLETON_LEVELS =
+ private static readonly double[] OUTPUT_SINGLETON_LEVELS =
[
- 0.0, // Mức 1: Dừng hoàn toàn (0%)
- 0.25, // Mức 2: Rất chậm (12.5%)
- 0.5, // Mức 3: Chậm (25%)
- 0.75, // Mức 4: Hơi chậm (37.5%)
- 1.0, // Mức 5: Trung bình (50%) - Điểm chuẩn cũ
- 1.25, // Mức 6: Hơi nhanh (62.5%)
- 1.5, // Mức 7: Nhanh (75%)
- 1.75, // Mức 8: Rất nhanh (87.5%)
- 2.0 // Mức 9: Tối đa (100%)
+ 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%)
];
// ============================================================================
@@ -390,34 +410,35 @@ public class FuzzyLogic
// ============================================================================
// Biến trạng thái bộ tích phân
- private double integratorState = 0.0;
+ private static double integratorState = 0.0;
// Hệ số bộ điều khiển PI
- private double proportionalGain = 1.0; // Hệ số tỷ lệ (Kp)
- private double integralGain = 0.5; // Hệ số tích phân (Ki)
+ private static double proportionalGain = 1.0; // Hệ số tỷ lệ (Kp)
+ private static double integralGain = 0.1; // Hệ số tích phân (Ki)
///
/// 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 MỚI:
+ /// 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: 9 tập mờ (VVS, VS, S, SM, M, MF, F, VF, VVF)
- /// - Tổng số luật: 5 × 9 = 45 luật cho mỗi bánh
- /// - Dải đầu ra: 0.0 - 2.0 (gấp đôi phiên bản cũ)
+ /// - 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)
///
- /// Vận tốc tuyến tính mong muốn (0.0 - 2.0 m/s)
+ /// Vận tốc tuyến tính mong muốn (0.0 - 3.0 m/s)
/// Vận tốc góc mong muốn (rad/s)
- /// Chu kỳ lấy mẫu (giây)
- /// (leftWheelSpeed, rightWheelSpeed): Tốc độ bánh trái và phải trong [0.0, 2.0]
+ /// Chu kỳ lấy mẫu (giây, khuyến nghị 0.01s)
+ /// (leftWheelSpeed, rightWheelSpeed): Tốc độ bánh trái và phải trong [0.0, 3.0]
public (double leftWheelSpeed, double rightWheelSpeed) Fuzzy_step(
double desiredVelocity,
double desiredAngularVelocity,
double samplingTime)
{
- const int NUM_INPUT_MEMBERSHIPS = 14; // 5 cho PI + 9 cho Velocity
- const int NUM_OUTPUT_LEVELS = 9; // 9 mức đầu ra (0.0 - 2.0)
- const int NUM_RULES = 45; // 5 × 9 = 45 luật
+ 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];
@@ -430,6 +451,9 @@ public class FuzzyLogic
// 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;
@@ -446,7 +470,7 @@ public class FuzzyLogic
NUM_RULES
);
- double rightWheelSpeed = Defuzzify(weightedSum_Right, totalWeight_Right, defaultValue: 1.0);
+ 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(
@@ -457,9 +481,14 @@ public class FuzzyLogic
NUM_RULES
);
- double leftWheelSpeed = Defuzzify(weightedSum_Left, totalWeight_Left, defaultValue: 1.0);
+ double leftWheelSpeed = Defuzzify(weightedSum_Left, totalWeight_Left, defaultValue: 1.5);
- // ========== BƯỚC 5: TRẢ VỀ KẾT QUẢ ==========
+ // ========== 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);
}
diff --git a/RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigation.cs b/RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigation.cs
index 9d70f4c..e6bacc6 100644
--- a/RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigation.cs
+++ b/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? NavigationTimer;
+ private const double Scale = 2;
+ //private WatchTimer? NavigationTimer;
+ private HighPrecisionTimer? 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);
diff --git a/RobotApp/Services/WatchTimer.cs b/RobotApp/Services/WatchTimer.cs
index 3b2d0aa..a3bb983 100644
--- a/RobotApp/Services/WatchTimer.cs
+++ b/RobotApp/Services/WatchTimer.cs
@@ -5,31 +5,54 @@ namespace RobotApp.Services;
public class WatchTimer(int Interval, Action Callback, Logger? 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(int Interval, Action Callback, Logger? 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));
}
@@ -47,14 +74,23 @@ public class WatchTimer(int Interval, Action Callback, Logger? 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);
diff --git a/RobotApp/Services/WatchTimerAsync.cs b/RobotApp/Services/WatchTimerAsync.cs
index b5da39e..4512275 100644
--- a/RobotApp/Services/WatchTimerAsync.cs
+++ b/RobotApp/Services/WatchTimerAsync.cs
@@ -5,25 +5,49 @@ namespace RobotApp.Services;
public class WatchTimerAsync(int Interval, Func Callback, Logger? 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(int Interval, Func Callback, Logger? 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));
+ else throw new ObjectDisposedException(nameof(WatchTimer));
}
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);