update
This commit is contained in:
Đăng Nguyễn
parent
c35da9a73f
commit
dd8c17cb6c
|
|
@ -5,6 +5,7 @@
|
||||||
<Nullable>enable</Nullable>
|
<Nullable>enable</Nullable>
|
||||||
<ImplicitUsings>enable</ImplicitUsings>
|
<ImplicitUsings>enable</ImplicitUsings>
|
||||||
<UserSecretsId>aspnet-RobotApp-1f61caa2-bbbb-40cd-88b6-409b408a84ea</UserSecretsId>
|
<UserSecretsId>aspnet-RobotApp-1f61caa2-bbbb-40cd-88b6-409b408a84ea</UserSecretsId>
|
||||||
|
<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
|
||||||
</PropertyGroup>
|
</PropertyGroup>
|
||||||
|
|
||||||
<ItemGroup>
|
<ItemGroup>
|
||||||
|
|
|
||||||
157
RobotApp/Services/HighPrecisionTimer.cs
Normal file
157
RobotApp/Services/HighPrecisionTimer.cs
Normal 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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -203,14 +203,14 @@ public class RobotOrderController(INavigation NavigationManager,
|
||||||
}
|
}
|
||||||
else OrderActions.Add(order.Nodes[i].NodeId, order.Edges[i].Actions);
|
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 (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];
|
if (order.Nodes[i].Released) CurrentBaseNode = order.Nodes[i];
|
||||||
}
|
}
|
||||||
SafetyManager.OnSafetySpeedChanged += OnSafetySpeedChanged;
|
SafetyManager.OnSafetySpeedChanged += OnSafetySpeedChanged;
|
||||||
|
|
||||||
if (OrderActions.TryGetValue(order.Nodes[^1].NodeId, out Action[]? finalactions) && finalactions is not null && finalactions.Length > 0) FinalAction = [.. finalactions];
|
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 (OrderActions.Count > 0) ActionManager.AddOrderActions([.. OrderActions.Values.SelectMany(a => a)]);
|
||||||
|
|
||||||
if (order.Nodes.Length > 1 && order.Edges.Length >= 0)
|
if (order.Nodes.Length > 1 && order.Edges.Length >= 0)
|
||||||
|
|
@ -314,11 +314,7 @@ public class RobotOrderController(INavigation NavigationManager,
|
||||||
{
|
{
|
||||||
var robotAction = ActionManager[ActionHard.ActionId];
|
var robotAction = ActionManager[ActionHard.ActionId];
|
||||||
if (robotAction is null) return;
|
if (robotAction is null) return;
|
||||||
if (robotAction is not null && robotAction.IsCompleted)
|
if (robotAction is not null && robotAction.IsCompleted) ActionHard = null;
|
||||||
{
|
|
||||||
NavigationManager.Resume();
|
|
||||||
ActionHard = null;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
|
|
@ -345,6 +341,11 @@ public class RobotOrderController(INavigation NavigationManager,
|
||||||
if (IsWaitingPaused) IsWaitingPaused = false;
|
if (IsWaitingPaused) IsWaitingPaused = false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (ActionHard == null && ActionWaitingRunning.IsEmpty)
|
||||||
|
{
|
||||||
|
NavigationManager.Resume();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
private void OrderHandler()
|
private void OrderHandler()
|
||||||
|
|
|
||||||
|
|
@ -151,41 +151,48 @@ public class FuzzyLogic
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <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>
|
/// </summary>
|
||||||
/// <param name="velocity">Vận tốc tuyến tính mong muốn (0.0 - 2.0 m/s)</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-13)</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)
|
private static void FuzzifyVelocity(double velocity, double[] membershipValues)
|
||||||
{
|
{
|
||||||
// Phân chia 9 tập mờ đều cho dải 0.0 - 2.0
|
// 11 tập mờ phân bố đều từ 0.0 đến 3.0
|
||||||
// Mỗi tập mờ cách nhau 0.25, overlap 50%
|
// 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]
|
// 1. VVS (Very Very Slow): [-∞, -∞, 0.0, 0.3]
|
||||||
membershipValues[5] = Fuzzy_trapmf(velocity, -1.0E+9, -1.0E+9, 0.0, 0.25);
|
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]
|
// 2. VS (Very Slow): [0.0, 0.3, 0.6]
|
||||||
membershipValues[6] = Fuzzy_trimf(velocity, 0.0, 0.25, 0.5);
|
membershipValues[6] = Fuzzy_trimf(velocity, 0.0, 0.3, 0.6);
|
||||||
|
|
||||||
// 3. S (Slow): [0.25, 0.5, 0.75]
|
// 3. S- (Slow Low): [0.3, 0.6, 0.9]
|
||||||
membershipValues[7] = Fuzzy_trimf(velocity, 0.25, 0.5, 0.75);
|
membershipValues[7] = Fuzzy_trimf(velocity, 0.3, 0.6, 0.9);
|
||||||
|
|
||||||
// 4. SM (Slow-Medium): [0.5, 0.75, 1.0]
|
// 4. S (Slow): [0.6, 0.9, 1.2]
|
||||||
membershipValues[8] = Fuzzy_trimf(velocity, 0.5, 0.75, 1.0);
|
membershipValues[8] = Fuzzy_trimf(velocity, 0.6, 0.9, 1.2);
|
||||||
|
|
||||||
// 5. M (Medium): [0.75, 1.0, 1.25]
|
// 5. S+ (Slow High): [0.9, 1.2, 1.5]
|
||||||
membershipValues[9] = Fuzzy_trimf(velocity, 0.75, 1.0, 1.25);
|
membershipValues[9] = Fuzzy_trimf(velocity, 0.9, 1.2, 1.5);
|
||||||
|
|
||||||
// 6. MF (Medium-Fast): [1.0, 1.25, 1.5]
|
// 6. M (Medium): [1.2, 1.5, 1.8]
|
||||||
membershipValues[10] = Fuzzy_trimf(velocity, 1.0, 1.25, 1.5);
|
membershipValues[10] = Fuzzy_trimf(velocity, 1.2, 1.5, 1.8);
|
||||||
|
|
||||||
// 7. F (Fast): [1.25, 1.5, 1.75]
|
// 7. F- (Fast Low): [1.5, 1.8, 2.1]
|
||||||
membershipValues[11] = Fuzzy_trimf(velocity, 1.25, 1.5, 1.75);
|
membershipValues[11] = Fuzzy_trimf(velocity, 1.5, 1.8, 2.1);
|
||||||
|
|
||||||
// 8. VF (Very Fast): [1.5, 1.75, 2.0]
|
// 8. F (Fast): [1.8, 2.1, 2.4]
|
||||||
membershipValues[12] = Fuzzy_trimf(velocity, 1.5, 1.75, 2.0);
|
membershipValues[12] = Fuzzy_trimf(velocity, 1.8, 2.1, 2.4);
|
||||||
|
|
||||||
// 9. VVF (Very Very Fast): [1.75, 2.0, +∞, +∞]
|
// 9. F+ (Fast High): [2.1, 2.4, 2.7]
|
||||||
membershipValues[13] = Fuzzy_trapmf(velocity, 1.75, 2.0, 1.0E+9, 1.0E+9);
|
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>
|
/// <summary>
|
||||||
/// Đánh giá luật mờ cho một bộ điều khiển
|
/// Đánh giá luật mờ cho một bộ điều khiển
|
||||||
/// </summary>
|
/// </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="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="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="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)
|
// 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;
|
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;
|
int velocityMembershipIndex = ruleAntecedentIndices[ruleIndex + numRules] + VELOCITY_OFFSET - 1;
|
||||||
|
|
||||||
// Tính độ kích hoạt của luật (AND operator = phép nhân)
|
// Tính độ kích hoạt của luật (AND operator = phép nhân)
|
||||||
double ruleActivation = inputMembershipValues[piMembershipIndex]
|
double ruleActivation = inputMembershipValues[piMembershipIndex]
|
||||||
* inputMembershipValues[velocityMembershipIndex];
|
* 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;
|
int outputIndex = ruleConsequentIndices[ruleIndex] - 1;
|
||||||
double outputValue = outputSingletons[outputIndex];
|
double outputValue = outputSingletons[outputIndex];
|
||||||
|
|
||||||
|
|
@ -269,120 +276,133 @@ public class FuzzyLogic
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Bảng chỉ số tiền đề luật cho bánh phải (Rule Antecedent Indices - Right Wheel)
|
/// 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
|
/// 110 phần tử: 55 cho PI + 55 cho Velocity
|
||||||
/// Hệ thống: 5 tập mờ PI × 9 tập mờ V = 45 luật
|
/// Hệ thống: 5 tập mờ PI × 11 tập mờ V = 55 luật
|
||||||
/// Giá trị từ 1-5 cho PI, 1-9 cho V
|
/// Giá trị từ 1-5 cho PI, 1-11 cho V
|
||||||
/// </summary>
|
/// </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)
|
// 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 9 lần (cho 9 mức vận tốc)
|
// 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, // Luật 1-9: PI = NB (tập 1)
|
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, // Luật 10-18: PI = Z (tập 2)
|
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, // Luật 19-27: PI = PB (tập 3)
|
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, // Luật 28-36: PI = NM (tập 4)
|
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, // Luật 37-45: PI = PM (tập 5)
|
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)
|
// 55 phần tử sau: Chỉ số tập mờ của Velocity (1-11)
|
||||||
// Lặp lại theo pattern: VVS, VS, S, SM, M, MF, F, VF, VVF
|
// 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, // Luật 1-9: V = VVS đến 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, // Luật 10-18: 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, // Luật 19-27: 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, // Luật 28-36: 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 // Luật 37-45: V = VVS đến VVF
|
1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11 // Luật 45-55: V = VVS đến VVF
|
||||||
];
|
];
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Bảng chỉ số hệ quả cho bánh phải (Rule Consequent Indices - Right Wheel)
|
/// 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
|
/// 55 phần tử tương ứng với 55 luật
|
||||||
/// Giá trị từ 1-9 tương ứng với 9 mức tốc độ đầu ra (0.0 - 2.0)
|
/// Giá trị từ 1-11 tương ứng với 11 mức tốc độ đầu ra (0.0 - 3.0)
|
||||||
///
|
///
|
||||||
/// Logic:
|
/// Logic bánh phải:
|
||||||
/// - Khi PI âm (NB, NM): Bánh phải chậm hơn (robot rẽ trái)
|
/// - 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
|
/// - PI = 0 (Z): Bánh phải theo vận tốc V → Robot đi thẳng
|
||||||
/// - Khi PI dương (PB, PM): Bánh phải nhanh hơn (robot rẽ phải)
|
/// - PI dương (PB, PM): Bánh phải nhanh hơn → Robot rẽ phải
|
||||||
/// </summary>
|
/// </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
|
// PI = NB (Negative Big) - Bánh phải RẤT CHẬM (rẽ trái mạnh)
|
||||||
1, 1, 2, 2, 3, 3, 4, 5, 6, // Luật 1-9: V thấp->cao → Output 1-6
|
// 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
|
// PI = Z (Zero) - Bánh phải THEO VẬN TỐC (đi thẳng)
|
||||||
2, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 10-18: V thấp->cao → Output 2-9
|
// 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
|
// PI = PB (Positive Big) - Bánh phải RẤT NHANH (rẽ phải mạnh)
|
||||||
4, 5, 6, 7, 8, 9, 9, 9, 9, // Luật 19-27: V thấp->cao → Output 4-9
|
// 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
|
// PI = NM (Negative Medium) - Bánh phải HƠI CHẬM (rẽ trái nhẹ)
|
||||||
1, 2, 2, 3, 4, 4, 5, 6, 7, // Luật 28-36: V thấp->cao → Output 1-7
|
// 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
|
// PI = PM (Positive Medium) - Bánh phải HƠI NHANH (rẽ phải nhẹ)
|
||||||
3, 4, 5, 6, 7, 8, 8, 9, 9 // Luật 37-45: V thấp->cao → Output 3-9
|
// 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>
|
/// <summary>
|
||||||
/// Bảng chỉ số tiền đề luật cho bánh trái (Rule Antecedent Indices - Left Wheel)
|
/// 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>
|
/// </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)
|
// 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, // Luật 1-9: PI = NB (tập 1)
|
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, // Luật 10-18: PI = Z (tập 2)
|
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, // Luật 19-27: PI = PB (tập 3)
|
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, // Luật 28-36: PI = NM (tập 4)
|
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, // Luật 37-45: PI = PM (tập 5)
|
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)
|
// 55 phần tử sau: Chỉ số tập mờ của Velocity (1-11)
|
||||||
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, 10, 11, // Luật 1-11
|
||||||
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, 10, 11, // Luật 12-22
|
||||||
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, 10, 11, // Luật 23-33
|
||||||
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, 10, 11, // Luật 34-44
|
||||||
1, 2, 3, 4, 5, 6, 7, 8, 9 // Luật 37-45: V = VVS đến VVF
|
1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11 // Luật 45-55
|
||||||
];
|
];
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Bảng chỉ số hệ quả cho bánh trái (Rule Consequent Indices - Left Wheel)
|
/// 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
|
/// Logic bánh trái: 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)
|
/// - 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
|
/// - PI = 0 (Z): Bánh trái theo vận tốc V → Robot đi thẳng
|
||||||
/// - Khi PI dương (PB, PM): Bánh trái chậm hơn (robot rẽ phải)
|
/// - PI dương (PB, PM): Bánh trái chậm hơn → Robot rẽ phải
|
||||||
/// </summary>
|
/// </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
|
// PI = NB (Negative Big) - Bánh trái RẤT NHANH (rẽ trái mạnh)
|
||||||
4, 5, 6, 7, 8, 9, 9, 9, 9, // Luật 1-9: V thấp->cao → Output 4-9
|
// 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
|
// PI = Z (Zero) - Bánh trái THEO VẬN TỐC (đi thẳng)
|
||||||
2, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 10-18: V thấp->cao → Output 2-9
|
// 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
|
// PI = PB (Positive Big) - Bánh trái RẤT CHẬM (rẽ phải mạnh)
|
||||||
1, 1, 2, 2, 3, 3, 4, 5, 6, // Luật 19-27: V thấp->cao → Output 1-6
|
// 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
|
// PI = NM (Negative Medium) - Bánh trái HƠI NHANH (rẽ trái nhẹ)
|
||||||
3, 4, 5, 6, 7, 8, 8, 9, 9, // Luật 28-36: V thấp->cao → Output 3-9
|
// 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
|
// PI = PM (Positive Medium) - Bánh trái HƠI CHẬM (rẽ phải nhẹ)
|
||||||
1, 2, 2, 3, 4, 4, 5, 6, 7 // Luật 37-45: V thấp->cao → Output 1-7
|
// 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>
|
/// <summary>
|
||||||
/// Các mức đầu ra singleton (Output Singletons)
|
/// 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>
|
/// </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.0, // Mức 1: Dừng hoàn toàn (0%)
|
||||||
0.25, // Mức 2: Rất chậm (12.5%)
|
0.3, // Mức 2: Rất chậm (10%)
|
||||||
0.5, // Mức 3: Chậm (25%)
|
0.6, // Mức 3: Chậm (20%)
|
||||||
0.75, // Mức 4: Hơi chậm (37.5%)
|
0.9, // Mức 4: Hơi chậm (30%)
|
||||||
1.0, // Mức 5: Trung bình (50%) - Điểm chuẩn cũ
|
1.2, // Mức 5: Chậm vừa (40%)
|
||||||
1.25, // Mức 6: Hơi nhanh (62.5%)
|
1.5, // Mức 6: Trung bình (50%) - Điểm chuẩn
|
||||||
1.5, // Mức 7: Nhanh (75%)
|
1.8, // Mức 7: Hơi nhanh (60%)
|
||||||
1.75, // Mức 8: Rất nhanh (87.5%)
|
2.1, // Mức 8: Nhanh (70%)
|
||||||
2.0 // Mức 9: Tối đa (100%)
|
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
|
// 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
|
// Hệ số bộ điều khiển PI
|
||||||
private double proportionalGain = 1.0; // Hệ số tỷ lệ (Kp)
|
private static double proportionalGain = 1.0; // Hệ số tỷ lệ (Kp)
|
||||||
private double integralGain = 0.5; // Hệ số tích phân (Ki)
|
private static double integralGain = 0.1; // Hệ số tích phân (Ki)
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Hàm điều khiển chính - Tính toán tốc độ bánh trái và phải
|
/// 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ờ
|
/// 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)
|
/// - 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)
|
/// - Velocity: 11 tập mờ (VVS, VS, S-, S, S+, M, F-, F, F+, VF, VVF)
|
||||||
/// - Tổng số luật: 5 × 9 = 45 luật cho mỗi bánh
|
/// - Tổng số luật: 5 × 11 = 55 luật cho mỗi bánh
|
||||||
/// - Dải đầu ra: 0.0 - 2.0 (gấp đôi phiên bản cũ)
|
/// - 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>
|
/// </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="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>
|
/// <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, 2.0]</returns>
|
/// <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(
|
public (double leftWheelSpeed, double rightWheelSpeed) Fuzzy_step(
|
||||||
double desiredVelocity,
|
double desiredVelocity,
|
||||||
double desiredAngularVelocity,
|
double desiredAngularVelocity,
|
||||||
double samplingTime)
|
double samplingTime)
|
||||||
{
|
{
|
||||||
const int NUM_INPUT_MEMBERSHIPS = 14; // 5 cho PI + 9 cho Velocity
|
const int NUM_INPUT_MEMBERSHIPS = 16; // 5 cho PI + 11 cho Velocity
|
||||||
const int NUM_OUTPUT_LEVELS = 9; // 9 mức đầu ra (0.0 - 2.0)
|
const int NUM_OUTPUT_LEVELS = 11; // 11 mức đầu ra (0.0 - 3.0)
|
||||||
const int NUM_RULES = 45; // 5 × 9 = 45 luật
|
const int NUM_RULES = 55; // 5 × 11 = 55 luật
|
||||||
|
|
||||||
// Khởi tạo mảng lưu độ thuộc đầu vào
|
// Khởi tạo mảng lưu độ thuộc đầu vào
|
||||||
double[] inputMembershipValues = new double[NUM_INPUT_MEMBERSHIPS];
|
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
|
// Cập nhật trạng thái tích phân: I(t) = I(t-1) + Ki * error * dt
|
||||||
integratorState += integralGain * desiredAngularVelocity * samplingTime;
|
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)
|
// Tính tín hiệu điều khiển PI: u(t) = Kp * error + I(t)
|
||||||
double piControlSignal = proportionalGain * desiredAngularVelocity + integratorState;
|
double piControlSignal = proportionalGain * desiredAngularVelocity + integratorState;
|
||||||
|
|
||||||
|
|
@ -446,7 +470,7 @@ public class FuzzyLogic
|
||||||
NUM_RULES
|
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 ==========
|
// ========== BƯỚC 4: TÍNH TOÁN BÁNH TRÁI ==========
|
||||||
var (weightedSum_Left, totalWeight_Left) = EvaluateRules(
|
var (weightedSum_Left, totalWeight_Left) = EvaluateRules(
|
||||||
|
|
@ -457,9 +481,14 @@ public class FuzzyLogic
|
||||||
NUM_RULES
|
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);
|
return (leftWheelSpeed, rightWheelSpeed);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -22,7 +22,9 @@ public class SimulationNavigation : INavigation, IDisposable
|
||||||
protected bool NavDriving = false;
|
protected bool NavDriving = false;
|
||||||
|
|
||||||
protected const int CycleHandlerMilliseconds = 50;
|
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 double TargetAngle = 0;
|
||||||
protected PID? RotatePID;
|
protected PID? RotatePID;
|
||||||
|
|
@ -62,7 +64,7 @@ public class SimulationNavigation : INavigation, IDisposable
|
||||||
|
|
||||||
protected void HandleNavigationStart()
|
protected void HandleNavigationStart()
|
||||||
{
|
{
|
||||||
NavigationTimer = new(CycleHandlerMilliseconds, NavigationHandler, Logger);
|
NavigationTimer = new((int)(CycleHandlerMilliseconds / Scale), NavigationHandler, Logger);
|
||||||
NavigationTimer.Start();
|
NavigationTimer.Start();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -91,8 +93,10 @@ public class SimulationNavigation : INavigation, IDisposable
|
||||||
NavigationPath = PathPlanner.PathSplit(pathDirection, edges);
|
NavigationPath = PathPlanner.PathSplit(pathDirection, edges);
|
||||||
|
|
||||||
MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
|
MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
|
||||||
MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
|
MoveFuzzy = new FuzzyLogic();
|
||||||
MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath([.. NavigationPath]);
|
MovePurePursuit = new PurePursuit()
|
||||||
|
.WithLookheadDistance(0.35)
|
||||||
|
.WithPath([.. NavigationPath]);
|
||||||
|
|
||||||
(NavigationNode node, int index) = MovePurePursuit.GetOnNode(Visualization.X, Visualization.Y);
|
(NavigationNode node, int index) = MovePurePursuit.GetOnNode(Visualization.X, Visualization.Y);
|
||||||
if(index >= NavigationPath.Length - 1) return;
|
if(index >= NavigationPath.Length - 1) return;
|
||||||
|
|
@ -140,9 +144,13 @@ public class SimulationNavigation : INavigation, IDisposable
|
||||||
StartNodeId = currentRobotNode.NodeId,
|
StartNodeId = currentRobotNode.NodeId,
|
||||||
EndNodeId = goalNode.NodeId,
|
EndNodeId = goalNode.NodeId,
|
||||||
}]);
|
}]);
|
||||||
MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
|
|
||||||
MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
|
MovePID = new PID().WithKp(1.5).WithKi(0.0001).WithKd(0.8);
|
||||||
MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath(NavigationPath);
|
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);
|
double Angle = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X);
|
||||||
Rotate(Angle * 180 / Math.PI);
|
Rotate(Angle * 180 / Math.PI);
|
||||||
UpdateOrder(goalNode.NodeId);
|
UpdateOrder(goalNode.NodeId);
|
||||||
|
|
|
||||||
|
|
@ -5,31 +5,54 @@ namespace RobotApp.Services;
|
||||||
public class WatchTimer<T>(int Interval, Action Callback, Logger<T>? Logger) : IDisposable where T : class
|
public class WatchTimer<T>(int Interval, Action Callback, Logger<T>? Logger) : IDisposable where T : class
|
||||||
{
|
{
|
||||||
private Timer? Timer;
|
private Timer? Timer;
|
||||||
private readonly Stopwatch Watch = new();
|
|
||||||
public bool Disposed;
|
public bool Disposed;
|
||||||
|
|
||||||
|
private long NextDueTime;
|
||||||
|
private readonly Lock Lock = new();
|
||||||
|
|
||||||
private void Handler(object? state)
|
private void Handler(object? state)
|
||||||
{
|
{
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
Watch.Restart();
|
bool shouldRun = false;
|
||||||
|
lock (Lock)
|
||||||
Callback.Invoke();
|
|
||||||
|
|
||||||
Watch.Stop();
|
|
||||||
if (Watch.ElapsedMilliseconds >= Interval || Interval - Watch.ElapsedMilliseconds <= 50)
|
|
||||||
{
|
{
|
||||||
if(Watch.ElapsedMilliseconds > Interval) Logger?.Warning($"WatchTimer Warning: Elapsed time {Watch.ElapsedMilliseconds}ms exceeds interval {Interval}ms.");
|
if (Disposed) return;
|
||||||
Timer?.Change(Interval, Timeout.Infinite);
|
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)
|
catch (Exception ex)
|
||||||
{
|
{
|
||||||
Logger?.Error($"WatchTimer Error: {ex}");
|
Logger?.Error($"WatchTimerAsync Error: {ex}");
|
||||||
Timer?.Change(Interval, Timeout.Infinite);
|
Timer?.Change(Interval, Timeout.Infinite);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
@ -38,8 +61,12 @@ public class WatchTimer<T>(int Interval, Action Callback, Logger<T>? Logger) : I
|
||||||
{
|
{
|
||||||
if (!Disposed)
|
if (!Disposed)
|
||||||
{
|
{
|
||||||
Timer = new Timer(Handler, null, Timeout.Infinite, Timeout.Infinite);
|
lock (Lock)
|
||||||
Timer.Change(Interval, 0);
|
{
|
||||||
|
NextDueTime = GetCurrentTimeMs() + Interval;
|
||||||
|
Timer = new Timer(Handler, null, Timeout.Infinite, Timeout.Infinite);
|
||||||
|
Timer.Change(Interval, Timeout.Infinite);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
else throw new ObjectDisposedException(nameof(WatchTimer<T>));
|
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()
|
public void Stop()
|
||||||
{
|
{
|
||||||
if (Disposed) return;
|
if (Disposed) return;
|
||||||
|
|
||||||
if (Timer != null)
|
if (Timer != null)
|
||||||
{
|
{
|
||||||
Timer.Change(Timeout.Infinite, Timeout.Infinite);
|
lock (Lock)
|
||||||
Timer.Dispose();
|
{
|
||||||
Timer = null;
|
Timer.Change(Timeout.Infinite, Timeout.Infinite);
|
||||||
|
Timer.Dispose();
|
||||||
|
Timer = null;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
private static long GetCurrentTimeMs()
|
||||||
|
{
|
||||||
|
return Stopwatch.GetTimestamp() * 1000 / Stopwatch.Frequency;
|
||||||
|
}
|
||||||
|
|
||||||
public void Dispose()
|
public void Dispose()
|
||||||
{
|
{
|
||||||
Dispose(true);
|
Dispose(true);
|
||||||
|
|
|
||||||
|
|
@ -5,25 +5,49 @@ namespace RobotApp.Services;
|
||||||
public class WatchTimerAsync<T>(int Interval, Func<Task> Callback, Logger<T>? Logger) : IDisposable where T : class
|
public class WatchTimerAsync<T>(int Interval, Func<Task> Callback, Logger<T>? Logger) : IDisposable where T : class
|
||||||
{
|
{
|
||||||
private Timer? Timer;
|
private Timer? Timer;
|
||||||
private readonly Stopwatch Watch = new();
|
|
||||||
public bool Disposed;
|
public bool Disposed;
|
||||||
|
|
||||||
|
private long NextDueTime;
|
||||||
|
private readonly Lock Lock = new();
|
||||||
|
|
||||||
private async void Handler(object? state)
|
private async void Handler(object? state)
|
||||||
{
|
{
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
Watch.Restart();
|
bool shouldRun = false;
|
||||||
|
lock (Lock)
|
||||||
await Callback.Invoke();
|
|
||||||
|
|
||||||
Watch.Stop();
|
|
||||||
if (Watch.ElapsedMilliseconds >= Interval || Interval - Watch.ElapsedMilliseconds <= 50)
|
|
||||||
{
|
{
|
||||||
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)
|
catch (Exception ex)
|
||||||
|
|
@ -37,23 +61,36 @@ public class WatchTimerAsync<T>(int Interval, Func<Task> Callback, Logger<T>? Lo
|
||||||
{
|
{
|
||||||
if (!Disposed)
|
if (!Disposed)
|
||||||
{
|
{
|
||||||
Timer = new Timer(Handler, null, Timeout.Infinite, Timeout.Infinite);
|
lock (Lock)
|
||||||
Timer.Change(Interval, 0);
|
{
|
||||||
|
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()
|
public void Stop()
|
||||||
{
|
{
|
||||||
if (Disposed) return;
|
if (Disposed) return;
|
||||||
|
|
||||||
if (Timer != null)
|
if (Timer != null)
|
||||||
{
|
{
|
||||||
Timer.Change(Timeout.Infinite, Timeout.Infinite);
|
lock (Lock)
|
||||||
Timer.Dispose();
|
{
|
||||||
Timer = null;
|
Timer.Change(Timeout.Infinite, Timeout.Infinite);
|
||||||
|
Timer.Dispose();
|
||||||
|
Timer = null;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
private static long GetCurrentTimeMs()
|
||||||
|
{
|
||||||
|
return Stopwatch.GetTimestamp() * 1000 / Stopwatch.Frequency;
|
||||||
|
}
|
||||||
|
|
||||||
public void Dispose()
|
public void Dispose()
|
||||||
{
|
{
|
||||||
Dispose(true);
|
Dispose(true);
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue
Block a user