DangNV/RobotApp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
Đăng Nguyễn 2025-12-17 15:34:15 +07:00
parent c35da9a73f
commit dd8c17cb6c
7 changed files with 435 additions and 166 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
RobotApp/RobotApp.csproj
View File

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

157
RobotApp/Services/HighPrecisionTimer.cs Normal file
View File

@ -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);
}
}

15
RobotApp/Services/Robot/RobotOrderController.cs
View File

@ -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()

263
RobotApp/Services/Robot/Simulation/Navigation/Algorithm/FuzzyLogic.cs
View File

@ -151,41 +151,48 @@ public class FuzzyLogic
}
/// <summary>
/// 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
/// </summary>
/// <param name="velocity">Vận tốc tuyến tính mong muốn (0.0 - 2.0 m/s)</param>
/// <param name="membershipValues">Mảng lưu giá trị độ thuộc (vị trí 5-13)</param>
/// <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)
{
// 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
/// <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 (14 giá trị: 5 PI + 9 V)</param>
/// <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>
@ -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
/// <summary>
/// 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
/// </summary>
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
];
/// <summary>
/// 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
/// </summary>
private readonly byte[] RULE_CONSEQUENT_INDICES_RIGHT =
private static 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
// 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
];
/// <summary>
/// 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
/// </summary>
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
];
/// <summary>
/// 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
/// </summary>
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
];
/// <summary>
/// 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
/// </summary>
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.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)
/// <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 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)
/// </summary>
/// <param name="desiredVelocity">Vận tốc tuyến tính mong muốn (0.0 - 2.0 m/s)</param>
/// <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)</param>
/// <returns>(leftWheelSpeed, rightWheelSpeed): Tốc độ bánh trái và phải trong [0.0, 2.0]</returns>
/// <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 = 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);
}

22
RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigation.cs
View File

@ -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);

62
RobotApp/Services/WatchTimer.cs
View File

@ -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)
{
lock (Lock)
{
NextDueTime = GetCurrentTimeMs() + Interval;
Timer = new Timer(Handler, null, Timeout.Infinite, Timeout.Infinite);
Timer.Change(Interval, 0);
Timer.Change(Interval, Timeout.Infinite);
}
}
else throw new ObjectDisposedException(nameof(WatchTimer<T>));
}
@ -47,13 +74,22 @@ public class WatchTimer<T>(int Interval, Action Callback, Logger<T>? Logger) : I
public void Stop()
{
if (Disposed) return;
if (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()
{

61
RobotApp/Services/WatchTimerAsync.cs
View File

@ -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,22 +61,35 @@ public class WatchTimerAsync<T>(int Interval, Func<Task> Callback, Logger<T>? Lo
{
if (!Disposed)
{
lock (Lock)
{
NextDueTime = GetCurrentTimeMs() + Interval;
Timer = new Timer(Handler, null, Timeout.Infinite, Timeout.Infinite);
Timer.Change(Interval, 0);
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)
{
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()
{