update

RobotApp.Common.Shares/MathExtensions.cs

@@ -102,4 +102,9 @@ public static class MathExtensions
         if (dotProduct / (lengthAB * lengthBC) < -1) return 180;
         return Math.Acos(dotProduct / (lengthAB * lengthBC)) * (180.0 / Math.PI);
     }
+
+    public static double Distance(double x1, double y1, double x2, double y2)
+    {
+        return Math.Sqrt(Math.Pow(x2 - x1, 2) + Math.Pow(y2 - y1, 2));
+    }
 }

RobotApp.VDA5050/Type/InformationType.cs

@@ -0,0 +1,12 @@
+namespace RobotApp.VDA5050.Type;
+
+public enum InformationType
+{
+    robot_general
+}
+
+
+public enum InformationReferencesKey
+{
+    robot_state,
+}

RobotApp/Interfaces/IError.cs

@@ -4,6 +4,7 @@ namespace RobotApp.Interfaces;
 
 public interface IError
 {
+    Error[] ErrorsState { get; }
     event Action? OnNewFatalError;
     bool HasFatalError { get; }
     void AddError(Error error, TimeSpan? clearAfter = null);

RobotApp/Interfaces/IInfomation.cs

@@ -4,6 +4,7 @@ namespace RobotApp.Interfaces;
 
 public interface IInfomation
 {
+    Information[] InformationState { get; }
     void AddInfo(Information infor);
     void DeleteInfoType(string infoType);
     void ClearAllInfos();

RobotApp/Interfaces/IInstantActions.cs

@@ -8,7 +8,7 @@ public interface IInstantActions
     ActionState[] ActionStates { get; }
     bool HasActionRunning { get; }
     void AddOrderActions(Action[] actions);
-    void AddInstantAction(Action action);
+    void AddInstantAction(Action[] action);
     void StartOrderAction(string actionId, bool wait);
     void ClearInstantActions();
     void PauseActions();

RobotApp/Interfaces/ILoad.cs

@@ -4,5 +4,5 @@ namespace RobotApp.Interfaces;
 
 public interface ILoad
 {
-    Load? Load { get; }
+    Load[] Load { get; }
 }

RobotApp/Interfaces/ILocalization.cs

@@ -128,4 +128,12 @@ public interface ILocalization
     /// </summary>
     /// <returns></returns>
     MessageResult ResetSlamError();
+
+    /// <summary>
+    /// Khoảng cách từ vị trí hiện tại đến tọa độ (x, y) trong hệ tọa độ toàn cục.
+    /// </summary>
+    /// <param name="x"></param>
+    /// <param name="y"></param>
+    /// <returns></returns>
+    double DistanceTo(double x, double y);  
 }

RobotApp/Interfaces/INavigation.cs

@@ -26,6 +26,8 @@ public enum NavigationProccess
 
 public interface INavigation
 {
+    event Action? OnNavigationStateChanged;
+    event Action? OnNavigationFinished;  
     bool IsReady { get; }
     bool Driving { get; }
     double VelocityX { get; }
@@ -35,7 +37,7 @@ public interface INavigation
     void Move(Node[] nodes, Edge[] edges);
     void MoveStraight(double x, double y);
     void Rotate(double angle);
-    void Paused();
+    void Pause();
     void Resume();
     void UpdateOrder(string lastBaseNodeId);
     void RefreshOrder(Node[] nodes, Edge[] edges);

RobotApp/Interfaces/IOrder.cs

@@ -13,8 +13,7 @@ public interface IOrder
     NodeState[] NodeStates { get; }
     EdgeState[] EdgeStates { get; }
 
-    void StartOrder(string orderId, Node[] nodes, Edge[] edges);
-    void UpdateOrder(int orderUpdateId, Node[] nodes, Edge[] edges);
+    void UpdateOrder(OrderMsg order);
     void StopOrder();
     void PauseOrder();
     void ResumeOrder();

RobotApp/Interfaces/IPeripheral.cs

@@ -4,7 +4,7 @@ namespace RobotApp.Interfaces;
 
 public enum PeripheralMode
 {
-    AUTO,
+    AUTOMATIC,
     MANUAL,
     SERVICE,
 }

RobotApp/Services/Exceptions/ActionException.cs

@@ -2,7 +2,7 @@
 
 namespace RobotApp.Services.Exceptions;
 
-public class ActionException : Exception
+public class ActionException : RobotExeption
 {
     public ActionException(string message) : base(message) { }
     public ActionException(string message, Exception inner) : base(message, inner) { }
@@ -11,5 +11,4 @@ public class ActionException : Exception
     {
         Error = error;
     }
-    public Error? Error { get; set; }
 }

RobotApp/Services/Exceptions/ModbusException.cs

@@ -2,7 +2,7 @@
 
 namespace RobotApp.Services.Exceptions;
 
-public class ModbusException : Exception
+public class ModbusException : RobotExeption
 {
     public ModbusException(string message) : base(message) { }
     public ModbusException(string message, Exception inner) : base(message, inner) { }
@@ -11,5 +11,4 @@ public class ModbusException : Exception
     {
         Error = error;
     }
-    public Error? Error { get; set; }
 }

RobotApp/Services/Exceptions/OrderException.cs

@@ -2,7 +2,7 @@
 
 namespace RobotApp.Services.Exceptions;
 
-public class OrderException : Exception
+public class OrderException : RobotExeption
 {
     public OrderException(string message) : base(message) { }
     public OrderException(string message, Exception inner) : base(message, inner) { }
@@ -11,5 +11,4 @@ public class OrderException : Exception
     {
         Error = error;
     }
-    public Error? Error { get; set; }
 }

RobotApp/Services/Exceptions/PathPlannerException.cs

@@ -2,7 +2,7 @@
 
 namespace RobotApp.Services.Exceptions;
 
-public class PathPlannerException : Exception
+public class PathPlannerException : RobotExeption
 {
     public PathPlannerException(string message) : base(message) { }
     public PathPlannerException(string message, Exception inner) : base(message, inner) { }
@@ -11,5 +11,4 @@ public class PathPlannerException : Exception
     {
         Error = error;
     }
-    public Error? Error { get; set; }
 }

RobotApp/Services/Exceptions/RobotExeption.cs

@@ -0,0 +1,15 @@
+using RobotApp.VDA5050.State;
+
+namespace RobotApp.Services.Exceptions;
+
+public class RobotExeption : Exception
+{
+    public RobotExeption(string message) : base(message) { }
+    public RobotExeption(string message, Exception inner) : base(message, inner) { }
+    public RobotExeption() : base() { }
+    public RobotExeption(Error error) : base()
+    {
+        Error = error;
+    }
+    public Error? Error { get; set; }
+}

RobotApp/Services/Exceptions/SimulationException.cs

@@ -2,7 +2,7 @@
 
 namespace RobotApp.Services.Exceptions;
 
-public class SimulationException : Exception
+public class SimulationException : RobotExeption
 {
     public SimulationException(string message) : base(message) { }
     public SimulationException(string message, Exception inner) : base(message, inner) { }
@@ -11,5 +11,4 @@ public class SimulationException : Exception
     {
         Error = error;
     }
-    public Error? Error { get; set; }
 }

RobotApp/Services/Robot/Actions/RobotAction.cs

@@ -1,4 +1,5 @@
 using Grpc.Core;
+using RobotApp.Services.Exceptions;
 using RobotApp.VDA5050.Factsheet;
 using RobotApp.VDA5050.InstantAction;
 using RobotApp.VDA5050.State;
@@ -34,7 +35,11 @@ public abstract class RobotAction(IServiceProvider serviceProvider) : IAsyncDisp
         Status = ActionStatus.WAITING;
         ActionScope = actionScope;
         Action = action;
-        return Initialize();
+        Initialize();
+        Id = Action.ActionId;
+        Description = Action.ActionDescription;
+        Status = ActionStatus.WAITING;
+        return true;
     }
 
     public void Start()
@@ -99,25 +104,22 @@ public abstract class RobotAction(IServiceProvider serviceProvider) : IAsyncDisp
         return Task.CompletedTask;
     }
 
-    protected virtual bool Initialize()
+    protected virtual void Initialize()
     {
-        if (Action is null) return false;
+        if (Action is null) throw new ActionException("Khởi tạo Action không tồn tại");
         Scope ??= ServiceProvider.CreateScope();
         var FactsheetService = Scope.ServiceProvider.GetRequiredService<RobotFactsheet>();
 
         if (Enum.TryParse(Action.ActionType, out ActionType type)) Type = type;
-        else return false;
+        else throw new ActionException($"ActionType {Action.ActionType} không hợp lệ.");
 
-        var actionStore = FactsheetService.GetAction(Type);
-        if (actionStore is null) return false;
+        var actionStore = FactsheetService.GetAction(Type) ?? throw new ActionException($"ActionType {Action.ActionType} không được thiết kế.");
 
         if (Enum.TryParse(Action.BlockingType, out BlockingType blockingType)) BlockingType = blockingType;
-        else return false;
+        else throw new ActionException($"BlockingType {Action.BlockingType} không hợp lệ.");
 
-        if (!actionStore.BlockingTypes.Any(bt => bt == BlockingType.ToString()) || !actionStore.ActionScopes.Any(sp => sp == ActionScope.ToString()))
-        {
-            return false;
-        }
+        if (!actionStore.BlockingTypes.Any(bt => bt == BlockingType.ToString())) throw new ActionException($"BlockingType {BlockingType} không được hỗ trợ cho action {Type}.");
+        if (!actionStore.ActionScopes.Any(sp => sp == ActionScope.ToString())) throw new ActionException($"ActionScope {ActionScope} không được hỗ trợ cho action {Type}.");
 
         if (actionStore.ActionParameters.Length > 0)
         {
@@ -125,20 +127,12 @@ public abstract class RobotAction(IServiceProvider serviceProvider) : IAsyncDisp
             {
                 if (!parameterStore.IsOptional)
                 {
-                    if (!Action.ActionParameters.Any(a => a.Key == parameterStore.Key))
-                    {
-                        return false;
-                    }
+                    if (!Action.ActionParameters.Any(a => a.Key == parameterStore.Key)) throw new ActionException($"Thiếu tham số bắt buộc '{parameterStore.Key}' cho action {Type}.");
                 }
             }
             Parameters = Action.ActionParameters;
         }
-
-        Id = Action.ActionId;
-        Description = Action.ActionDescription;
         AgvAction = actionStore;
-        Status = ActionStatus.WAITING;
-        return true;
     }
 
     private async Task ActionHandler()
@@ -180,6 +174,7 @@ public abstract class RobotAction(IServiceProvider serviceProvider) : IAsyncDisp
     private Task Dispose()
     {
         ActionTimer?.Dispose();
+        ActionTimer = null;
         return Task.CompletedTask;
     }
 

RobotApp/Services/Robot/Actions/RobotCancelOrderAction.cs

@@ -1,9 +1,15 @@
-namespace RobotApp.Services.Robot.Actions;
+using RobotApp.Interfaces;
+
+namespace RobotApp.Services.Robot.Actions;
 
 public class RobotCancelOrderAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
 {
     protected override Task StartAction()
     {
+        Scope ??= ServiceProvider.CreateAsyncScope();
+        var RobotOrder = Scope.ServiceProvider.GetRequiredService<IOrder>();
+        RobotOrder.StopOrder();
+        Status = VDA5050.State.ActionStatus.FINISHED;
         return base.StartAction();
     }
 

RobotApp/Services/Robot/Actions/RobotFactsheetRequestAction.cs

@@ -1,10 +1,15 @@
-namespace RobotApp.Services.Robot.Actions;
+using RobotApp.Interfaces;
+
+namespace RobotApp.Services.Robot.Actions;
 
 public class RobotFactsheetRequestAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
 {
-    protected override Task StartAction()
+    protected override async Task StartAction()
     {
-        return base.StartAction();
+        Scope ??= ServiceProvider.CreateAsyncScope();
+        var RobotFactsheet = Scope.ServiceProvider.GetRequiredService<RobotFactsheet>();
+        await RobotFactsheet.PubFactsheet();
+        Status = VDA5050.State.ActionStatus.FINISHED;
     }
 
     protected override Task ExecuteAction()

RobotApp/Services/Robot/Actions/RobotInitPositionAction.cs

@@ -1,9 +1,29 @@
-namespace RobotApp.Services.Robot.Actions;
+using RobotApp.Interfaces;
+using RobotApp.Services.Exceptions;
+
+namespace RobotApp.Services.Robot.Actions;
 
 public class RobotInitPositionAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
 {
+    double X = 0;
+    double Y = 0;
+    double Theta = 0;
+
     protected override Task StartAction()
     {
+        Scope ??= ServiceProvider.CreateAsyncScope();
+        var Localization = Scope.ServiceProvider.GetRequiredService<ILocalization>();
+        var initPose = Localization.SetInitializePosition(X, Y, Theta);
+        if(initPose.IsSuccess)
+        {
+            Status = VDA5050.State.ActionStatus.FINISHED;
+            ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription;
+        }
+        else
+        {
+            Status = VDA5050.State.ActionStatus.FAILED;
+            ResultDescription = initPose.Message;
+        }
         return base.StartAction();
     }
 
@@ -11,4 +31,24 @@ public class RobotInitPositionAction(IServiceProvider ServiceProvider) : RobotAc
     {
         return base.ExecuteAction();
     }
+
+    protected override void Initialize()
+    {
+        base.Initialize();
+
+        var xPara = Parameters.FirstOrDefault(p => p.Key == "x") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'x'");
+        var yPara = Parameters.FirstOrDefault(p => p.Key == "y") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'y'");
+        var thetaPara = Parameters.FirstOrDefault(p => p.Key == "theta") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'theta'");
+
+        var xParse = double.TryParse(xPara.Value, out double xData);
+        var yParse = double.TryParse(xPara.Value, out double yData);
+        var thetaParse = double.TryParse(xPara.Value, out double thetaData);
+
+        if (!xParse) throw new ActionException($"Action {Type} có parameter 'x' không đúng kiểu dữ liệu");
+        if (!yParse) throw new ActionException($"Action {Type} có parameter 'y' không đúng kiểu dữ liệu");
+        if (!thetaParse) throw new ActionException($"Action {Type} có parameter 'theta' không đúng kiểu dữ liệu");
+        X = xData;
+        Y = yData;
+        Theta = thetaData;
+    }
 }

RobotApp/Services/Robot/Actions/RobotLiftDownAction.cs

@@ -4,6 +4,8 @@ public class RobotLiftDownAction(IServiceProvider ServiceProvider) : RobotAction
 {
     protected override Task StartAction()
     {
+        Status = VDA5050.State.ActionStatus.FINISHED;
+        ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription;
         return base.StartAction();
     }
 

RobotApp/Services/Robot/Actions/RobotLiftRotateAction.cs

@@ -4,6 +4,8 @@ public class RobotLiftRotateAction(IServiceProvider ServiceProvider) : RobotActi
 {
     protected override Task StartAction()
     {
+        Status = VDA5050.State.ActionStatus.FINISHED;
+        ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription;
         return base.StartAction();
     }
 

RobotApp/Services/Robot/Actions/RobotLiftUpAction.cs

@@ -4,6 +4,8 @@ public class RobotLiftUpAction(IServiceProvider ServiceProvider) : RobotAction(S
 {
     protected override Task StartAction()
     {
+        Status = VDA5050.State.ActionStatus.FINISHED;
+        ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription;
         return base.StartAction();
     }
 

RobotApp/Services/Robot/Actions/RobotMutedBaseOffAction.cs

@@ -1,9 +1,14 @@
-namespace RobotApp.Services.Robot.Actions;
+using RobotApp.Interfaces;
+
+namespace RobotApp.Services.Robot.Actions;
 
 public class RobotMutedBaseOffAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
 {
     protected override Task StartAction()
     {
+        Scope ??= ServiceProvider.CreateAsyncScope();
+        var RobotSafety = Scope.ServiceProvider.GetRequiredService<ISafety>();
+        RobotSafety.SetMutedBase(false);
         return base.StartAction();
     }
 

RobotApp/Services/Robot/Actions/RobotMutedBaseOnAction.cs

@@ -1,9 +1,14 @@
-namespace RobotApp.Services.Robot.Actions;
+using RobotApp.Interfaces;
+
+namespace RobotApp.Services.Robot.Actions;
 
 public class RobotMutedBaseOnAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
 {
     protected override Task StartAction()
     {
+        Scope ??= ServiceProvider.CreateAsyncScope();
+        var RobotSafety = Scope.ServiceProvider.GetRequiredService<ISafety>();
+        RobotSafety.SetMutedBase(true);
         return base.StartAction();
     }
 

RobotApp/Services/Robot/Actions/RobotMutedLoadOffAction.cs

@@ -1,9 +1,14 @@
-namespace RobotApp.Services.Robot.Actions;
+using RobotApp.Interfaces;
+
+namespace RobotApp.Services.Robot.Actions;
 
 public class RobotMutedLoadOffAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
 {
     protected override Task StartAction()
     {
+        Scope ??= ServiceProvider.CreateAsyncScope();
+        var RobotSafety = Scope.ServiceProvider.GetRequiredService<ISafety>();
+        RobotSafety.SetMutedLoad(false);
         return base.StartAction();
     }
 

RobotApp/Services/Robot/Actions/RobotMutedLoadOnAction.cs

@@ -1,9 +1,14 @@
-namespace RobotApp.Services.Robot.Actions;
+using RobotApp.Interfaces;
+
+namespace RobotApp.Services.Robot.Actions;
 
 public class RobotMutedLoadOnAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
 {
     protected override Task StartAction()
     {
+        Scope ??= ServiceProvider.CreateAsyncScope();
+        var RobotSafety = Scope.ServiceProvider.GetRequiredService<ISafety>();
+        RobotSafety.SetMutedLoad(true);
         return base.StartAction();
     }
 

RobotApp/Services/Robot/Actions/RobotRotateAction.cs

@@ -1,14 +1,44 @@
-namespace RobotApp.Services.Robot.Actions;
+using RobotApp.Interfaces;
+using RobotApp.Services.Exceptions;
+
+namespace RobotApp.Services.Robot.Actions;
 
 public class RobotRotateAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
 {
+    double Angle = 0;
+    INavigation? RobotNavigation;
     protected override Task StartAction()
     {
+        Scope ??= ServiceProvider.CreateScope();
+        RobotNavigation = Scope.ServiceProvider.GetRequiredService<INavigation>();
+        RobotNavigation.Rotate(Angle);
         return base.StartAction();
     }
 
     protected override Task ExecuteAction()
     {
+        if(RobotNavigation is null)
+        {
+            Status = VDA5050.State.ActionStatus.FAILED;
+            ResultDescription = "Không tìm thấy module quản lí Navigation"; 
+        }
+        else if(RobotNavigation.State == NavigationState.Idle)
+        {
+            Status = VDA5050.State.ActionStatus.FINISHED;
+            ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription;
+        }
         return base.ExecuteAction();
     }
+
+    protected override void Initialize()
+    {
+        base.Initialize();
+
+        var anglePara = Parameters.FirstOrDefault(p => p.Key == "angle") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'angle'");
+
+        var angleParse = double.TryParse(anglePara.Value, out double angleData);
+
+        if (!angleParse) throw new ActionException($"Action {Type} có parameter 'angle' không đúng kiểu dữ liệu");
+        Angle = angleData;
+    }
 }

RobotApp/Services/Robot/Actions/RobotRotateKeepLift.cs

@@ -1,9 +1,14 @@
-namespace RobotApp.Services.Robot.Actions;
+using RobotApp.Services.Exceptions;
+
+namespace RobotApp.Services.Robot.Actions;
 
 public class RobotRotateKeepLift(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
 {
+    double Angle = 0;
     protected override Task StartAction()
     {
+        Status = VDA5050.State.ActionStatus.FINISHED;
+        ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription;
         return base.StartAction();
     }
 
@@ -11,4 +16,16 @@ public class RobotRotateKeepLift(IServiceProvider ServiceProvider) : RobotAction
     {
         return base.ExecuteAction();
     }
+
+    protected override void Initialize()
+    {
+        base.Initialize();
+
+        var anglePara = Parameters.FirstOrDefault(p => p.Key == "angle") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'angle'");
+
+        var angleParse = double.TryParse(anglePara.Value, out double angleData);
+
+        if (!angleParse) throw new ActionException($"Action {Type} có parameter 'angle' không đúng kiểu dữ liệu");
+        Angle = angleData;
+    }
 }

RobotApp/Services/Robot/Actions/RobotStartChargingAction.cs

@@ -4,6 +4,8 @@ public class RobotStartChargingAction(IServiceProvider ServiceProvider) : RobotA
 {
     protected override Task StartAction()
     {
+        Status = VDA5050.State.ActionStatus.FINISHED;
+        ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription;
         return base.StartAction();
     }
 

RobotApp/Services/Robot/Actions/RobotStartPauseAction.cs

@@ -7,8 +7,8 @@ public class RobotStartPauseAction(IServiceProvider ServiceProvider) : RobotActi
     protected override Task StartAction()
     {
         Scope ??= ServiceProvider.CreateScope();
-        var robotController = Scope.ServiceProvider.GetRequiredService<RobotController>();
-        robotController.Pause();
+        var RobotController = Scope.ServiceProvider.GetRequiredService<RobotController>();
+        RobotController.Pause();
         Status = ActionStatus.FINISHED;
         ResultDescription = AgvAction is not null ? AgvAction.ResultDescription : ResultDescription;
         return base.StartAction();

RobotApp/Services/Robot/Actions/RobotStateRequestAction.cs

@@ -2,9 +2,12 @@
 
 public class RobotStateRequestAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
 {
-    protected override Task StartAction()
+    protected override async Task StartAction()
     {
-        return base.StartAction();
+        Scope ??= ServiceProvider.CreateAsyncScope();
+        var RobotStates = Scope.ServiceProvider.GetRequiredService<RobotStates>();
+        await RobotStates.PubState();
+        Status = VDA5050.State.ActionStatus.FINISHED;
     }
 
     protected override Task ExecuteAction()

RobotApp/Services/Robot/Actions/RobotStopChargingAction.cs

@@ -4,6 +4,8 @@ public class RobotStopChargingAction(IServiceProvider ServiceProvider) : RobotAc
 {
     protected override Task StartAction()
     {
+        Status = VDA5050.State.ActionStatus.FINISHED;
+        ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription;
         return base.StartAction();
     }
 

RobotApp/Services/Robot/Actions/RobotStopPauseAction.cs

@@ -1,9 +1,16 @@
-namespace RobotApp.Services.Robot.Actions;
+using RobotApp.VDA5050.State;
+
+namespace RobotApp.Services.Robot.Actions;
 
 public class RobotStopPauseAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
 {
     protected override Task StartAction()
     {
+        Scope ??= ServiceProvider.CreateScope();
+        var RobotController = Scope.ServiceProvider.GetRequiredService<RobotController>();
+        RobotController.Resume();
+        Status = ActionStatus.FINISHED;
+        ResultDescription = AgvAction is not null ? AgvAction.ResultDescription : ResultDescription;
         return base.StartAction();
     }
 

RobotApp/Services/Robot/RobotActionController.cs

@@ -1,4 +1,5 @@
 using RobotApp.Interfaces;
+using RobotApp.Services.Exceptions;
 using RobotApp.Services.Robot.Actions;
 using RobotApp.VDA5050.Factsheet;
 using RobotApp.VDA5050.State;
@@ -7,7 +8,7 @@ using System.Collections.Concurrent;
 
 namespace RobotApp.Services.Robot;
 
-public class RobotActionController(Logger<RobotActionController> Logger, RobotActionStorage ActionStorage) : BackgroundService, IInstantActions
+public class RobotActionController(Logger<RobotActionController> Logger, RobotActionStorage ActionStorage, IError ErrorManager) : BackgroundService, IInstantActions
 {
     public ActionState[] ActionStates => [.. Actions.Values.Select(a => new ActionState
         {
@@ -25,10 +26,13 @@ public class RobotActionController(Logger<RobotActionController> Logger, RobotAc
     private WatchTimer<RobotActionController>? HandlerTimer;
     private const int HandlerInterval = 200;
 
-    public void AddInstantAction(VDA5050.InstantAction.Action action)
+    public void AddInstantAction(VDA5050.InstantAction.Action[] actions)
+    {
+        foreach (var action in actions)
         {
             ActionQueue.Enqueue((ActionScopes.INSTANT, action));
         }
+    }
 
     public void AddOrderActions(VDA5050.InstantAction.Action[] actions)
     {
@@ -91,9 +95,18 @@ public class RobotActionController(Logger<RobotActionController> Logger, RobotAc
                 }
             }
         }
+        catch (RobotExeption orEx)
+        {
+            if (orEx.Error is not null)
+            {
+                ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10));
+                Logger.Warning($"Lỗi khi xử lí Action: {orEx.Error.ErrorDescription}");
+            }
+            else Logger.Warning($"Lỗi khi xử lí Action: {orEx.Message}");
+        }
         catch (Exception ex)
         {
-            Logger?.Error($"Khởi tạo action xảy ra lỗi: {ex.Message}");
+            Logger?.Error($"Lỗi khi xử lí Action: {ex.Message}");
         }
     }
 
@@ -113,6 +126,7 @@ public class RobotActionController(Logger<RobotActionController> Logger, RobotAc
     public override Task StopAsync(CancellationToken cancellationToken)
     {
         HandlerTimer?.Dispose();
+        HandlerTimer = null;
         return base.StopAsync(cancellationToken);
     }
 }

RobotApp/Services/Robot/RobotBattery.cs

@@ -2,35 +2,35 @@
 
 namespace RobotApp.Services.Robot;
 
-public class RobotBattery : IBattery
+public class RobotBattery(RobotConfiguration RobotConfiguration) : IBattery
 {
-    public bool IsReady { get; private set; } = true;
+    public bool IsReady { get; private set; } = RobotConfiguration.IsSimulation;
 
-    public double Voltage => throw new NotImplementedException();
+    public double Voltage { get; private set; }
 
-    public double Current => throw new NotImplementedException();
+    public double Current { get; private set; }
 
-    public double SOC => throw new NotImplementedException();
+    public double SOC { get; private set; }
 
-    public double SOH => throw new NotImplementedException();
+    public double SOH { get; private set; }
 
-    public BatteryStatus Status => throw new NotImplementedException();
+    public BatteryStatus Status { get; private set; }
 
-    public int ChargeTime => throw new NotImplementedException();
+    public int ChargeTime { get; private set; }
 
-    public int DischargeTime => throw new NotImplementedException();
+    public int DischargeTime { get; private set; }
 
-    public double Temperature => throw new NotImplementedException();
+    public double Temperature { get; private set; }
 
-    public double RemainingCapacity => throw new NotImplementedException();
+    public double RemainingCapacity { get; private set; }
 
-    public double RemainingEnergy => throw new NotImplementedException();
+    public double RemainingEnergy { get; private set; }
 
-    public int DischargeCycles => throw new NotImplementedException();
+    public int DischargeCycles { get; private set; }
 
-    public int ChargeCycles => throw new NotImplementedException();
+    public int ChargeCycles { get; private set; }
 
-    public bool IsCharging => throw new NotImplementedException();
+    public bool IsCharging { get; private set; }
 
-    public string[] ErrorStatus => throw new NotImplementedException();
+    public string[] ErrorStatus { get; private set; } = [];
 }

RobotApp/Services/Robot/RobotConfiguration.cs

@@ -23,5 +23,5 @@ public class RobotConfiguration
     public bool IsSimulation { get; set; } = true;
     public SimulationModel SimulationModel { get; set; } = new();
 
-    public string XlocAddress { get; set; } = "";
+    public string XlocAddress { get; set; } = "http://192.168.195.56:50050";
 }

RobotApp/Services/Robot/RobotConnection.cs

@@ -22,7 +22,7 @@ public class RobotConnection(RobotConfiguration RobotConfiguration,
     {
         try
         {
-            var msg = JsonSerializer.Deserialize<OrderMsg>(data);
+            var msg = JsonSerializer.Deserialize<OrderMsg>(data, JsonOptionExtends.Read);
             if (msg is null || string.IsNullOrEmpty(msg.SerialNumber) || msg.SerialNumber != RobotConfiguration.SerialNumber) return;
             OrderUpdated?.Invoke(msg);
         }
@@ -36,7 +36,7 @@ public class RobotConnection(RobotConfiguration RobotConfiguration,
     {
         try
         {
-            var msg = JsonSerializer.Deserialize<InstantActionsMsg>(data);
+            var msg = JsonSerializer.Deserialize<InstantActionsMsg>(data, JsonOptionExtends.Read);
             if (msg is null || string.IsNullOrEmpty(msg.SerialNumber) || msg.SerialNumber != RobotConfiguration.SerialNumber) return;
             ActionUpdated?.Invoke(msg);
         }

RobotApp/Services/Robot/RobotController.cs

@@ -16,7 +16,8 @@ public partial class RobotController(IOrder OrderManager,
                              IError ErrorManager,
                              Logger<RobotController> Logger,
                              RobotConnection ConnectionManager,
-                             RobotStateMachine StateManager) : BackgroundService
+                             RobotStateMachine StateManager,
+                             RobotConfiguration RobotConfiguration) : BackgroundService
 {
     private readonly Mutex NewOrderMutex = new();
     private readonly Mutex NewInstanceMutex = new();
@@ -39,23 +40,23 @@ public partial class RobotController(IOrder OrderManager,
             try
             {
                 if (StateManager.RootStateName != RootStateType.Auto.ToString()) throw new OrderException(RobotErrors.Error1013(StateManager.RootStateName));
-                if (!string.IsNullOrEmpty(OrderManager.OrderId))
+                if (string.IsNullOrEmpty(OrderManager.OrderId))
                 {
-                    if (order.OrderId != OrderManager.OrderId) throw new OrderException(RobotErrors.Error1001(OrderManager.OrderId, order.OrderId));
-                    OrderManager.UpdateOrder(order.OrderUpdateId, order.Nodes, order.Edges);
+                    if (ActionManager.HasActionRunning) throw new OrderException(RobotErrors.Error1007());
+                    if (ErrorManager.HasFatalError) throw new OrderException(RobotErrors.Error1008());
+                    if (NavigationManager.Driving) throw new OrderException(RobotErrors.Error1009());
                 }
-                else if (ActionManager.HasActionRunning) throw new OrderException(RobotErrors.Error1007());
-                else if (ErrorManager.HasFatalError) throw new OrderException(RobotErrors.Error1008());
-                else if (NavigationManager.Driving) throw new OrderException(RobotErrors.Error1009());
-                else OrderManager.StartOrder(order.OrderId, order.Nodes, order.Edges);
+                else if (order.OrderId != OrderManager.OrderId) throw new OrderException(RobotErrors.Error1001(OrderManager.OrderId, order.OrderId));
+                OrderManager.UpdateOrder(order);
             }
-            catch (OrderException orEx)
+            catch (RobotExeption orEx)
             {
                 if (orEx.Error is not null)
                 {
                     ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10));
-                    Logger.Warning($"Lỗi khi xử lí Order: {orEx.Error.ErrorDescription}");
+                    Logger.Warning($"Order mới có lỗi: {orEx.Error.ErrorDescription}");
                 }
+                else Logger.Warning($"Order mới có lỗi: {orEx.Message}");
             }
             catch (Exception ex)
             {
@@ -68,21 +69,26 @@ public partial class RobotController(IOrder OrderManager,
         }
     }
 
-    public void NewInstanceActionUpdated(InstantActionsMsg action)
+    public void NewInstantActionUpdated(InstantActionsMsg action)
     {
         if (NewInstanceMutex.WaitOne(2000))
         {
             try
             {
-
+                ActionManager.AddInstantAction(action.Actions);
             }
-            catch (ActionException acEx)
+            catch (RobotExeption acEx)
             {
-
+                if (acEx.Error is not null)
+                {
+                    ErrorManager.AddError(acEx.Error, TimeSpan.FromSeconds(10));
+                    Logger.Warning($"InstantAction có lỗi: {acEx.Error.ErrorDescription}");
+                }
+                else Logger.Warning($"InstantAction có lỗi: {acEx.Message}");
             }
             catch (Exception ex)
             {
-
+                Logger.Warning($"Lỗi khi xử lí InstantAction: {ex.Message}");
             }
             finally
             {
@@ -93,7 +99,11 @@ public partial class RobotController(IOrder OrderManager,
     
     public void Pause()
     {
-
+        if(StateManager.HasState(AutoStateType.Executing.ToString()))
+        {
+            if(StateManager.HasState(ExecutingStateType.Moving.ToString())) OrderManager.PauseOrder();
+            else if(StateManager.HasState(ExecutingStateType.ACT.ToString())) ActionManager.PauseActions();
+        }
     }
 
     public void Resume()

RobotApp/Services/Robot/RobotControllerInitialize.cs

@@ -8,6 +8,7 @@ public partial class RobotController
     private readonly AutoResetEvent StartButtonPressedEvent = new(false);
     private readonly AutoResetEvent StopButtonPressedEvent = new(false);
     private readonly AutoResetEvent ResetButtonPressedEvent = new(false);
+
     private async Task InitializationingHandler(CancellationToken cancellationToken)
     {
         try
@@ -23,9 +24,9 @@ public partial class RobotController
                 if (StateManager.CurrentStateName == Enum.GetName(SystemStateType.Initializing))
                 {
                     if (PeripheralManager.IsReady &&
-                        //PeripheralManager.LiftMotorReady &&
-                        //PeripheralManager.LeftMotorReady &&
-                        //PeripheralManager.RightMotorReady &&
+                        PeripheralManager.LiftMotorReady &&
+                        PeripheralManager.LeftMotorReady &&
+                        PeripheralManager.RightMotorReady &&
                         BatteryManager.IsReady && 
                         Localization.IsReady && 
                         NavigationManager.IsReady) break;
@@ -35,11 +36,13 @@ public partial class RobotController
             Logger.Info("Robot đã khởi tạo xong. Đang kết nối tới Fleet Manager.");
             
             ConnectionManager.OrderUpdated += NewOrderUpdated;
-            ConnectionManager.ActionUpdated += NewInstanceActionUpdated;
+            ConnectionManager.ActionUpdated += NewInstantActionUpdated;
             await ConnectionManager.StartConnection(cancellationToken);
             Logger.Info("Robot đã kết nối tới Fleet Manager.");
             StateManager.TransitionTo(SystemStateType.Standby);
 
+            if (!RobotConfiguration.IsSimulation)
+            {
                 StartButtonPressedEvent.Reset();
                 Logger.Info("Chờ nút Start được nhấn để vào chế độ hoạt động...");
                 if (StartButtonPressedEvent.WaitOne())
@@ -48,6 +51,8 @@ public partial class RobotController
                     PeripheralManager.SetSytemState(SystemState.IDLE);
                 }
             }
+            else StateManager.TransitionTo(RootStateType.Auto);
+        }
         catch (Exception ex)
         {
             Logger.Error($"Khởi tạo RobotController xảy ra lỗi: {ex.Message} - {ex.StackTrace}");
@@ -56,6 +61,8 @@ public partial class RobotController
 
     private void StopHandler()
     {
+        var stopConnection = ConnectionManager.StopConnection();
+        stopConnection.Wait();
         PeripheralManager.OnPeripheralModeChanged -= SwichModeChanged;
         PeripheralManager.OnButtonPressed -= OnButtonPressed;
         PeripheralManager.OnStop -= OnStop;
@@ -65,7 +72,7 @@ public partial class RobotController
     {
         switch (mode)
         {
-            case PeripheralMode.AUTO:
+            case PeripheralMode.AUTOMATIC:
                 if (StartButtonPressedEvent.WaitOne())
                 {
                     StateManager.TransitionTo(RootStateType.Auto);

RobotApp/Services/Robot/RobotDriver.cs

@@ -4,11 +4,11 @@ namespace RobotApp.Services.Robot;
 
 public class RobotDriver : IDriver
 {
-    public bool IsReady => throw new NotImplementedException();
+    public bool IsReady { get; private set; }
 
-    public double LinearVelocity => throw new NotImplementedException();
+    public double LinearVelocity { get; private set; }
 
-    public double AngularVelocity => throw new NotImplementedException();
+    public double AngularVelocity { get; private set; }
 
     public bool ControlVelocity(double left, double right)
     {

RobotApp/Services/Robot/RobotErrors.cs

@@ -5,11 +5,12 @@ namespace RobotApp.Services.Robot;
 
 public class RobotErrors : IError
 {
-    private readonly List<Error> Errors = [];
+    public Error[] ErrorsState => [.. Errors];
     public bool HasFatalError => Errors.Any(e => e.ErrorLevel == ErrorLevel.FATAL.ToString());
-
     public event Action? OnNewFatalError;
 
+    private readonly List<Error> Errors = [];
+
     public void AddError(Error error, TimeSpan? clearAfter = null)
     {
         if (Errors.Any(e => e.ErrorType == error.ErrorType && e.ErrorHint == error.ErrorHint)) return;

RobotApp/Services/Robot/RobotFactsheet.cs

@@ -69,7 +69,7 @@ public class RobotFactsheet(RobotConnection RobotConnection, RobotConfiguration
         ActionScopes = [ActionScopes.INSTANT.ToString()],
         ActionParameters = [],
         ResultDescription = "Robot đã tạm dừng.",
-        BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString()],
+        BlockingTypes = [BlockingType.NONE.ToString()],
     };
 
     public readonly static AgvAction StopPause = new()
@@ -79,7 +79,7 @@ public class RobotFactsheet(RobotConnection RobotConnection, RobotConfiguration
         ActionScopes = [ActionScopes.INSTANT.ToString()],
         ActionParameters = [],
         ResultDescription = "Robot đã tiếp tục hoạt động.",
-        BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString(), BlockingType.HARD.ToString()],
+        BlockingTypes = [BlockingType.NONE.ToString()],
     };
 
     public readonly static AgvAction StartCharging = new()

RobotApp/Services/Robot/RobotInfomations.cs

@@ -1,10 +1,13 @@
 using RobotApp.Interfaces;
+using RobotApp.Services.State;
 using RobotApp.VDA5050.State;
+using RobotApp.VDA5050.Type;
 
 namespace RobotApp.Services.Robot;
 
-public class RobotInfomations : IInfomation
+public class RobotInfomations() : IInfomation
 {
+    public Information[] InformationState => [.. Infors];
     private readonly List<Information> Infors = [];
     public void AddInfo(Information infor)
     {

RobotApp/Services/Robot/RobotLoads.cs

@@ -5,7 +5,7 @@ namespace RobotApp.Services.Robot;
 
 public class RobotLoads(IPeripheral PeriperalManager) : ILoad
 {
-    public Load? Load => PeriperalManager.HasLoad ? GetLoad() : null;
+    public Load[] Load => PeriperalManager.HasLoad ? [GetLoad()] : [];
     private static Load GetLoad()
     {
         return new()

RobotApp/Services/Robot/RobotLocalization.cs

@@ -430,9 +430,15 @@ public class RobotLocalization(RobotConfiguration RobotConfiguration, Simulation
         XlocData.IsReady = true;
     }
 
+    public double DistanceTo(double x, double y)
+    {
+        return MathExtensions.Distance(this.X, this.Y, x, y);
+    }
+
     public override Task StopAsync(CancellationToken cancellationToken)
     {
         ReaderTimer?.Dispose();
+        ReaderTimer = null;
         return base.StopAsync(cancellationToken);
     }
 }

RobotApp/Services/Robot/RobotNavigation.cs

@@ -4,60 +4,65 @@ using RobotApp.VDA5050.Order;
 
 namespace RobotApp.Services.Robot;
 
-public class RobotNavigation : INavigation
+public class RobotNavigation(RobotConfiguration RobotConfiguration, IServiceProvider ServiceProvider) : INavigation
 {
     public bool IsReady => IsSimulation;
     public bool Driving => IsSimulation ? SimNavigation is not null ? SimNavigation.Driving : false : false;
     public double VelocityX => IsSimulation ? SimNavigation is not null ? SimNavigation.VelocityX : 0 : 0;
     public double VelocityY => IsSimulation ? SimNavigation is not null ? SimNavigation.VelocityY : 0 : 0;
     public double Omega => IsSimulation ? SimNavigation is not null ? SimNavigation.Omega : 0 : 0;
-    public NavigationState State => IsSimulation ? SimNavigation is not null ? SimNavigation.State : NavigationState.None : NavigationState.None;
+    public NavigationState State => IsSimulation ? SimNavigation is not null ? SimNavigation.State : NavigationState.Idle : NavigationState.None;
 
-    private readonly SimulationNavigation? SimNavigation;
-    private readonly bool IsSimulation;
+    private SimulationNavigation? SimNavigation;
+    private readonly bool IsSimulation = RobotConfiguration.IsSimulation;
 
-    public RobotNavigation(RobotConfiguration RobotConfiguration, IServiceProvider ServiceProvider)
-    {
-        IsSimulation = RobotConfiguration.IsSimulation;
-        if (IsSimulation)
-        {
-            SimNavigation = SimulationNavigationManager.GetNavigation(RobotConfiguration.SimulationModel.NavigationType, ServiceProvider);
-        }
-    }
+    public event Action? OnNavigationStateChanged;
+    public event Action? OnNavigationFinished;
 
     public void CancelMovement()
     {
-        throw new NotImplementedException();
+        if (IsSimulation)
+        {
+            SimNavigation?.CancelMovement();
+        }
     }
 
     public void Move(Node[] nodes, Edge[] edges)
     {
-        throw new NotImplementedException();
+        if (IsSimulation)
+        {
+            SimNavigation = SimulationNavigationManager.GetNavigation(RobotConfiguration.SimulationModel.NavigationType, ServiceProvider);
+            SimNavigation.OnNavigationFinished += NavigationFinished;
+            SimNavigation.Move(nodes, edges);
+        }
     }
 
     public void MoveStraight(double x, double y)
     {
-        throw new NotImplementedException();
+        if (IsSimulation)
+        {
+            SimNavigation = SimulationNavigationManager.GetNavigation(RobotConfiguration.SimulationModel.NavigationType, ServiceProvider);
+            SimNavigation.MoveStraight(x, y);
+        }
     }
 
-    public void Paused()
+    public void Pause()
     {
-        throw new NotImplementedException();
+        if (IsSimulation) SimNavigation?.Pause();
     }
 
     public void Resume()
     {
-        throw new NotImplementedException();
+        if (IsSimulation) SimNavigation?.Resume();
     }
 
     public void Rotate(double angle)
     {
-        throw new NotImplementedException();
-    }
-
-    public void UpdateOrder(int lastBaseSequence)
+        if (IsSimulation)
         {
-        throw new NotImplementedException();
+            SimNavigation = SimulationNavigationManager.GetNavigation(RobotConfiguration.SimulationModel.NavigationType, ServiceProvider);
+            SimNavigation.Rotate(angle * 180 / Math.PI);
+        }
     }
 
     public void RefreshOrder(Node[] nodes, Edge[] edges)
@@ -67,11 +72,17 @@ public class RobotNavigation : INavigation
 
     public void UpdateOrder(string lastBaseNodeId)
     {
-        throw new NotImplementedException();
+        if (IsSimulation) SimNavigation?.UpdateOrder(lastBaseNodeId);
     }
 
     public void Refresh()
     {
         throw new NotImplementedException();
     }
+
+    private void NavigationFinished()
+    {
+        OnNavigationFinished?.Invoke();
+        if(SimNavigation is not null) SimNavigation.OnNavigationFinished -= NavigationFinished;
+    }
 }

RobotApp/Services/Robot/RobotOrderController.cs

@@ -1,64 +1,24 @@
-using RobotApp.Interfaces;
+using Microsoft.AspNetCore.Components;
+using RobotApp.Common.Shares.Enums;
+using RobotApp.Interfaces;
 using RobotApp.Services.Exceptions;
+using RobotApp.Services.State;
 using RobotApp.VDA5050.Order;
 using RobotApp.VDA5050.State;
 using Action = RobotApp.VDA5050.InstantAction.Action;
 
 namespace RobotApp.Services.Robot;
 
-public class RobotOrderController(INavigation NavigationManager, IInstantActions ActionManager, IError ErrorManager, Logger<RobotOrderController> Logger) : IOrder
+public class RobotOrderController(INavigation NavigationManager, 
+                                  ILocalization Localization, 
+                                  IInstantActions ActionManager, 
+                                  IError ErrorManager, 
+                                  RobotStateMachine StateManager,
+                                  Logger<RobotOrderController> Logger) : IOrder
 {
     public string OrderId { get; private set; } = string.Empty;
     public int OrderUpdateId { get; private set; }
-    public NodeState[] NodeStates { get; private set; } = [];
-    public EdgeState[] EdgeStates { get; private set; } = [];
-    public string LastNodeId { get; private set; } = string.Empty;
-    public int LastNodeSequenceId { get; private set; }
-
-    private readonly Dictionary<string, Action[]> OrderActions = [];
-    private readonly Mutex OrderMutex = new();
-
-    protected const int CycleHandlerMilliseconds = 100;
-    private WatchTimer<RobotOrderController>? OrderTimer;
-
-    private int BaseSequenceId = 0;
-
-    public void StartOrder(string orderId, Node[] nodes, Edge[] edges)
-    {
-        if (OrderMutex.WaitOne(2000))
-        {
-            try
-            {
-                if (!string.IsNullOrEmpty(OrderId) && orderId != OrderId) throw new OrderException(RobotErrors.Error1001(OrderId, orderId));
-                if (nodes.Length < 2) throw new OrderException(RobotErrors.Error1002(nodes.Length));
-                if (edges.Length < 1) throw new OrderException(RobotErrors.Error1003(edges.Length));
-                if (edges.Length != nodes.Length - 1) throw new OrderException(RobotErrors.Error1004(nodes.Length, edges.Length));
-                if (NavigationManager.State != NavigationState.Idle) throw new OrderException(RobotErrors.Error1012(NavigationManager.State));
-
-                for (int i = 0; i < nodes.Length; i++)
-                {
-                    if (nodes[i].Actions is not null && nodes[i].Actions.Length > 0)
-                    {
-                        foreach (var item in nodes[i].Actions)
-                        {
-                            item.ActionDescription += $"\n NodeId: {nodes[i].NodeId}";
-                        }
-                        OrderActions.Add(nodes[i].NodeId, nodes[i].Actions);
-                    }
-                    if (i < nodes.Length - 1 && edges[i] is not null && edges[i].Length > 0)
-                    {
-                        foreach (var item in edges[i].Actions)
-                        {
-                            item.ActionDescription += $"\n NodeId: {nodes[i].NodeId}";
-                        }
-                        OrderActions.TryAdd(nodes[i].NodeId, edges[i].Actions);
-                    }
-                    if (nodes[i].SequenceId != i) throw new OrderException(RobotErrors.Error1010(nodes[i].NodeId, nodes[i].SequenceId, i));
-                    if (i < nodes.Length - 1 && edges[i].SequenceId != i) throw new OrderException(RobotErrors.Error1011(edges[i].EdgeId, edges[i].SequenceId, i));
-                    if (nodes[i].Released) BaseSequenceId = nodes[i].SequenceId;
-                }
-
-                NodeStates = [.. nodes.Select(n => new NodeState
+    public NodeState[] NodeStates => [.. Nodes.Select(n => new NodeState
                     {
                         NodeId = n.NodeId,
                         Released = n.Released,
@@ -72,7 +32,7 @@ public class RobotOrderController(INavigation NavigationManager, IInstantActions
                             MapId = n.NodePosition.MapId
                         }
                     })];
-                EdgeStates = [.. edges.Select(e => new EdgeState
+    public EdgeState[] EdgeStates => [.. Edges.Select(e => new EdgeState
                     {
                         EdgeId = e.EdgeId,
                         Released = e.Released,
@@ -80,114 +40,194 @@ public class RobotOrderController(INavigation NavigationManager, IInstantActions
                         SequenceId = e.SequenceId,
                         Trajectory = e.Trajectory
                     })];
+    public string LastNodeId => LastNode is null ? "" : LastNode.NodeId;
+    public int LastNodeSequenceId => LastNode is null ? 0 : LastNode.SequenceId;
 
-                OrderId = orderId;
-                ActionManager.AddOrderActions([.. OrderActions.Values.SelectMany(a => a)]);
-                NavigationManager.Move(nodes, edges);
-                HandleNavigationStart();
-            }
-            catch (OrderException orEx)
-            {
-                if (orEx.Error is not null)
-                {
-                    ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10));
-                    Logger.Warning($"Lỗi khi khởi tạo Order: {orEx.Error.ErrorDescription}");
-                }
-            }
-            catch (Exception ex)
-            {
-                Logger.Warning($"Lỗi khi khởi tạo Order: {ex.Message}");
-            }
-            finally
-            {
-                OrderMutex.ReleaseMutex();
-            }
-        }
-        Logger.Warning($"Lỗi khi khởi tạo Order: có order đang được khởi tạo chưa xong");
-    }
+    private const int CycleHandlerMilliseconds = 100;
+    private WatchTimer<RobotOrderController>? OrderTimer;
 
-    public void UpdateOrder(int orderUpdateId,Node[] nodes, Edge[] edges)
+    private readonly Dictionary<string, Action[]> OrderActions = [];
+
+    private Node[] Nodes = [];
+    private Edge[] Edges = [];
+    private Node? CurrentBaseNode;
+    private Node? LastNode;
+    private readonly Lock LockObject = new();
+    private OrderMsg? NewOrder; 
+
+    public void UpdateOrder(OrderMsg order)
     {
-        if (OrderMutex.WaitOne(2000))
+        lock (LockObject)
         {
-            try
-            {
-                if (string.IsNullOrEmpty(OrderId)) throw new OrderException(RobotErrors.Error1005());
-                if (orderUpdateId > OrderUpdateId)
-                {
-                    OrderUpdateId = orderUpdateId;
-                    // Check Order Update hợp lệ
-                    // Check Order update Hoziron hay không
+            NewOrder = order;
         }
-            }
-            catch (OrderException orEx)
-            {
-                if (orEx.Error is not null)
-                {
-                    ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10));
-                    Logger.Warning($"Lỗi khi cập nhật Order: {orEx.Error.ErrorDescription}");
-                }
-            }
-            catch (Exception ex)
-            {
-                Logger.Warning($"Lỗi khi cập nhật Order: {ex.Message}");
-            }
-            finally
-            {
-                OrderMutex.ReleaseMutex();
-            }
-        }
-        Logger.Warning($"Lỗi khi cập nhật Order: có order đang được cập nhật chưa xong");
+        if (OrderTimer is null) HandleOrderStart();
     }
 
     public void StopOrder()
     {
-        HandleNavigationStop();
         NavigationManager.CancelMovement();
-
-        OrderId = string.Empty;
-        OrderActions.Clear();
-        OrderUpdateId = 0;
-        BaseSequenceId = 0;
-        LastNodeSequenceId = 0;
-        NodeStates = [];
-        EdgeStates = [];
+        NavigationFinished();
     }
 
     public void PauseOrder()
     {
-        throw new NotImplementedException();
+        NavigationManager.Pause();
     }
 
     public void ResumeOrder()
     {
-        throw new NotImplementedException();
+        NavigationManager.Resume();
     }
 
-    private void HandleNavigationStart()
+    private void HandleOrderStart()
     {
         OrderTimer = new(CycleHandlerMilliseconds, OrderHandler, Logger);
         OrderTimer.Start();
     }
 
-    private void HandleNavigationStop()
+    private void HandleOrderStop()
     {
         OrderTimer?.Dispose();
+        OrderTimer = null;
+    }
+
+    private Node? GetCurrentNode()
+    {
+        Node? inNode = null;
+        double minDistance = double.MaxValue;   
+        foreach (var node in Nodes)
+        {
+            var distance = Localization.DistanceTo(node.NodePosition.X, node.NodePosition.Y);
+            if(distance <= node.NodePosition.AllowedDeviationXY)
+            {
+                if(distance < minDistance)
+                {
+                    minDistance = distance;
+                    inNode = node;
+                }
+            }
+        }
+        return inNode;
+    }
+
+    private void NavigationFinished()
+    {
+        HandleOrderStop();
+        OrderId = string.Empty;
+        OrderUpdateId = 0;
+        OrderActions.Clear();
+        CurrentBaseNode = null;
+        Nodes = [];
+        Edges = [];
+        StateManager.TransitionTo(AutoStateType.Idle);
+    }
+
+    private void HanleNewOrder(OrderMsg order)
+    {
+        if (NavigationManager.State != NavigationState.Idle) throw new OrderException(RobotErrors.Error1012(NavigationManager.State));
+
+        for (int i = 0; i < order.Nodes.Length; i++)
+        {
+            if (order.Nodes[i].Actions is not null && order.Nodes[i].Actions.Length > 0)
+            {
+                foreach (var item in order.Nodes[i].Actions)
+                {
+                    item.ActionDescription += $"\n NodeId: {order.Nodes[i].NodeId}";
+                }
+                OrderActions.Add(order.Nodes[i].NodeId, order.Nodes[i].Actions);
+            }
+            if (i < order.Nodes.Length - 1 && order.Edges[i] is not null && order.Edges[i].Length > 0)
+            {
+                foreach (var item in order.Edges[i].Actions)
+                {
+                    item.ActionDescription += $"\n NodeId: {order.Nodes[i].NodeId}";
+                }
+                OrderActions.TryAdd(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 < 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];
+        }
+
+        ActionManager.ClearInstantActions();
+        if (OrderActions.Count > 0) ActionManager.AddOrderActions([.. OrderActions.Values.SelectMany(a => a)]);
+        NavigationManager.Move(order.Nodes, order.Edges);
+        NavigationManager.OnNavigationFinished += NavigationFinished;
+        OrderId = order.OrderId;
+        OrderUpdateId = order.OrderUpdateId;
+        Nodes = order.Nodes;
+        Edges = order.Edges;
+        if (CurrentBaseNode is not null) NavigationManager.UpdateOrder(CurrentBaseNode.NodeId);
+        if(StateManager.CurrentStateName != AutoStateType.Executing.ToString()) StateManager.TransitionTo(AutoStateType.Executing);
+    }
+
+    private bool IsNewPath()
+    {
+        return true;
+    }
+
+    private void HandleUpdateOrder(OrderMsg order)
+    {
+        if (order.OrderId != OrderId) throw new OrderException(RobotErrors.Error1001(OrderId, order.OrderId));
+        if (order.OrderUpdateId <= OrderUpdateId) return;
+
+        var lastBastNode = order.Nodes.Last(n => n.Released);
+        if (lastBastNode is not null && lastBastNode.NodeId != CurrentBaseNode?.NodeId)
+        {
+            CurrentBaseNode = lastBastNode;
+            NavigationManager.UpdateOrder(CurrentBaseNode.NodeId);
+        }
+        Nodes = order.Nodes;
+        Edges = order.Edges;
+        OrderUpdateId = order.OrderUpdateId;
+    }
+
+    private void HandleOrder()
+    {
+        var currentNode = GetCurrentNode();
+        if (currentNode is not null && currentNode.NodeId != LastNode?.NodeId)
+        {
+            LastNode = currentNode;
+            // xuử lí nếu gặp node mới
+        }
     }
 
     private void OrderHandler()
     {
-        if(!string.IsNullOrEmpty(OrderId) && BaseSequenceId > 0)
+        try
         {
-            if(NavigationManager.State == NavigationState.Initializing)
+            if (NewOrder is not null)
             {
-                // khởi tạo Order
+                Console.WriteLine("Has new Order");
+                OrderMsg NewOrderHandler;
+                lock (LockObject)
+                {
+                    NewOrderHandler = NewOrder;
+                    NewOrder = null;
+                }
 
+                if (NewOrderHandler.Nodes.Length < 2) throw new OrderException(RobotErrors.Error1002(NewOrderHandler.Nodes.Length));
+                if (NewOrderHandler.Edges.Length < 1) throw new OrderException(RobotErrors.Error1003(NewOrderHandler.Edges.Length));
+                if (NewOrderHandler.Edges.Length != NewOrderHandler.Nodes.Length - 1) throw new OrderException(RobotErrors.Error1004(NewOrderHandler.Nodes.Length, NewOrderHandler.Edges.Length));
+
+                if (string.IsNullOrEmpty(OrderId)) HanleNewOrder(NewOrderHandler);
+                else HandleUpdateOrder(NewOrderHandler);
             }
-            else
+            HandleOrder();
+        }
+        catch (RobotExeption orEx )
         {
-                // xử lí khi có Order
+            if (orEx.Error is not null)
+            {
+                ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10));
+                Logger.Warning($"Lỗi khi xử lí Order: {orEx.Error.ErrorDescription}");
             }
+            else Logger.Warning($"Lỗi khi xử lí Order: {orEx.Message}");
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"Lỗi khi xử lí Order: {ex.Message}");
         }
     }
 }

RobotApp/Services/Robot/RobotPathPlanner.cs

@@ -4,21 +4,100 @@ using RobotApp.Common.Shares.Model;
 using RobotApp.Services.Exceptions;
 using RobotApp.Services.Robot.Simulation.Navigation;
 using RobotApp.VDA5050.Order;
-using RobotApp.VDA5050.State;
 
 namespace RobotApp.Services.Robot;
 
 public class RobotPathPlanner(IConfiguration Configuration)
 {
     private readonly double ResolutionSplit = Configuration.GetValue("PathPlanning:ResolutionSplit", 0.1);
+    private readonly double ReverseDirectionAngleDegree = Configuration.GetValue("PathPlanning:ReverseDirectionAngleDegree", 89);
+    private readonly double Ratio = Configuration.GetValue("PathPlanning:Ratio", 0.1);
+
+    public RobotDirection GetDirectionInNode(double currentTheta,Node inNode, Node futureNode, Edge edge)
+    {
+        (double futurex, double futurey) = MathExtensions.Curve(0.1, new()
+        {
+            X1 = inNode.NodePosition.X,
+            Y1 = inNode.NodePosition.Y,
+            X2 = futureNode.NodePosition.X,
+            Y2 = futureNode.NodePosition.Y,
+            ControlPoint1X = edge.Trajectory.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[1].X : 0,
+            ControlPoint1Y = edge.Trajectory.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[1].Y : 0,
+            ControlPoint2X = edge.Trajectory.ControlPoints.Length > 3 ? edge.Trajectory.ControlPoints[2].X : 0,
+            ControlPoint2Y = edge.Trajectory.ControlPoints.Length > 3 ? edge.Trajectory.ControlPoints[2].Y : 0,
+            TrajectoryDegree = edge.Trajectory.Degree == 1 ? TrajectoryDegree.One : edge.Trajectory.Degree == 2 ? TrajectoryDegree.Two : TrajectoryDegree.Three,
+        }); 
+        (double robotx, double roboty) = 
+        (
+            inNode.NodePosition.X + Math.Cos(currentTheta * Math.PI / 180),
+            inNode.NodePosition.Y + Math.Sin(currentTheta * Math.PI / 180)
+        );
+
+        var angle = MathExtensions.GetVectorAngle(
+                inNode.NodePosition.X,
+                inNode.NodePosition.Y,
+                robotx,
+                roboty,
+                futurex,
+                futurey);
+        return angle > ReverseDirectionAngleDegree ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
+    }
 
     public NavigationNode[] GetNavigationNode(double currentTheta, Node[] nodes, Edge[] edges)
     {
         if (nodes.Length < 2) throw new PathPlannerException(RobotErrors.Error1002(nodes.Length));
         if (edges.Length < 1) throw new PathPlannerException(RobotErrors.Error1003(edges.Length));
         if (edges.Length != nodes.Length - 1) throw new PathPlannerException(RobotErrors.Error1004(nodes.Length, edges.Length));
+        NavigationNode[] navigationNodes = [.. nodes.Select(n => new NavigationNode()
+        {
+            NodeId = n.NodeId,
+            X = n.NodePosition.X,
+            Y = n.NodePosition.Y,
+            Theta = n.NodePosition.Theta,
+            AllowedDeviationXY = n.NodePosition.AllowedDeviationXY,
+            AllowedDeviationTheta = n.NodePosition.AllowedDeviationTheta,
+            Description = n.NodeDescription
+        })];
+        navigationNodes[0].Direction = GetDirectionInNode(currentTheta, nodes[0], nodes[1], edges[0]);
+        for (int i = 1; i < nodes.Length - 1; i++)
+        {
+            (double lastx, double lasty) = MathExtensions.Curve(0.1, new()
+            {
+                X1 = nodes[i - 1].NodePosition.X,
+                Y1 = nodes[i - 1].NodePosition.Y,
+                X2 = nodes[i].NodePosition.X,
+                Y2 = nodes[i].NodePosition.Y,
+                ControlPoint1X = edges[i - 1].Trajectory.ControlPoints.Length > 2 ? edges[i - 1].Trajectory.ControlPoints[1].X : 0,
+                ControlPoint1Y = edges[i - 1].Trajectory.ControlPoints.Length > 2 ? edges[i - 1].Trajectory.ControlPoints[1].Y : 0,
+                ControlPoint2X = edges[i - 1].Trajectory.ControlPoints.Length > 3 ? edges[i - 1].Trajectory.ControlPoints[2].X : 0,
+                ControlPoint2Y = edges[i - 1].Trajectory.ControlPoints.Length > 3 ? edges[i - 1].Trajectory.ControlPoints[2].Y : 0,
+                TrajectoryDegree = edges[i - 1].Trajectory.Degree == 1 ? TrajectoryDegree.One : edges[i - 1].Trajectory.Degree == 2 ? TrajectoryDegree.Two : TrajectoryDegree.Three,
+            }); 
+            (double futurex, double futurey) = MathExtensions.Curve(0.1, new()
+            {
+                X1 = nodes[i].NodePosition.X,
+                Y1 = nodes[i].NodePosition.Y,
+                X2 = nodes[i + 1].NodePosition.X,
+                Y2 = nodes[i + 1].NodePosition.Y,
+                ControlPoint1X = edges[i].Trajectory.ControlPoints.Length > 2 ? edges[i].Trajectory.ControlPoints[1].X : 0,
+                ControlPoint1Y = edges[i].Trajectory.ControlPoints.Length > 2 ? edges[i].Trajectory.ControlPoints[1].Y : 0,
+                ControlPoint2X = edges[i].Trajectory.ControlPoints.Length > 3 ? edges[i].Trajectory.ControlPoints[2].X : 0,
+                ControlPoint2Y = edges[i].Trajectory.ControlPoints.Length > 3 ? edges[i].Trajectory.ControlPoints[2].Y : 0,
+                TrajectoryDegree = edges[i].Trajectory.Degree == 1 ? TrajectoryDegree.One : edges[i].Trajectory.Degree == 2 ? TrajectoryDegree.Two : TrajectoryDegree.Three,
+            });
+            var angle = MathExtensions.GetVectorAngle(
+                nodes[i].NodePosition.X,
+                nodes[i].NodePosition.Y,
+                lastx,
+                lasty,
+                futurex,
+                futurey);
 
-        return [];
+            if (angle < ReverseDirectionAngleDegree) navigationNodes[i].Direction = navigationNodes[i - 1].Direction == RobotDirection.FORWARD ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
+            else navigationNodes[i].Direction = navigationNodes[i - 1].Direction;
+        }
+        navigationNodes[^1].Direction = navigationNodes[^2].Direction;
+        return navigationNodes;
     }
 
     public NavigationNode[] PathSplit(NavigationNode[] nodes, Edge[] edges)
@@ -27,7 +106,7 @@ public class RobotPathPlanner(IConfiguration Configuration)
         if (edges.Length < 1) throw new PathPlannerException(RobotErrors.Error1003(edges.Length));
         if (edges.Length != nodes.Length - 1) throw new PathPlannerException(RobotErrors.Error1004(nodes.Length, edges.Length));
 
-        List <NavigationNode> navigationNode = [new()
+        List<NavigationNode> navigationNode = [new()
         {
             NodeId = nodes[0].NodeId,
             X = nodes[0].X,
@@ -49,11 +128,11 @@ public class RobotPathPlanner(IConfiguration Configuration)
                 Y1 = startNode.Y,
                 X2 = endNode.X,
                 Y2 = endNode.Y,
-                ControlPoint1X = edge.Trajectory.ControlPoints.Length > 0 ? edge.Trajectory.ControlPoints[0].X : 0,
-                ControlPoint1Y = edge.Trajectory.ControlPoints.Length > 0 ? edge.Trajectory.ControlPoints[0].Y : 0,
-                ControlPoint2X = edge.Trajectory.ControlPoints.Length > 1 ? edge.Trajectory.ControlPoints[1].X : 0,
-                ControlPoint2Y = edge.Trajectory.ControlPoints.Length > 1 ? edge.Trajectory.ControlPoints[1].Y : 0,
-                TrajectoryDegree = edge.Trajectory.Degree == 1 ? TrajectoryDegree.One : edge.Trajectory.Degree == 2 ? TrajectoryDegree.Two :TrajectoryDegree.Three
+                ControlPoint1X = edge.Trajectory.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[1].X : 0,
+                ControlPoint1Y = edge.Trajectory.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[1].Y : 0,
+                ControlPoint2X = edge.Trajectory.ControlPoints.Length > 3 ? edge.Trajectory.ControlPoints[2].X : 0,
+                ControlPoint2Y = edge.Trajectory.ControlPoints.Length > 3 ? edge.Trajectory.ControlPoints[2].Y : 0,
+                TrajectoryDegree = edge.Trajectory.Degree == 1 ? TrajectoryDegree.One : edge.Trajectory.Degree == 2 ? TrajectoryDegree.Two : TrajectoryDegree.Three
             };
 
             double length = EdgeCalculatorModel.GetEdgeLength();
@@ -83,6 +162,6 @@ public class RobotPathPlanner(IConfiguration Configuration)
                 Description = nodes[0].Description
             });
         }
-        return [..navigationNode];
+        return [.. navigationNode];
     }
 }

RobotApp/Services/Robot/RobotPeripheral.cs

@@ -5,7 +5,7 @@ namespace RobotApp.Services.Robot;
 
 public partial class RobotPeripheral(RobotConfiguration RobotConfiguration, IError ErrorManager, Logger<RobotPeripheral> Logger) : BackgroundService, IPeripheral, ISafety
 {
-    public bool IsReady { get; private set; } = false;
+    public bool IsReady { get; private set; } = RobotConfiguration.IsSimulation;
 
     public event Action<SafetySpeed>? OnSafetySpeedChanged;
     public event Action<PeripheralMode>? OnPeripheralModeChanged;
@@ -17,26 +17,31 @@ public partial class RobotPeripheral(RobotConfiguration RobotConfiguration, IErr
 
     public void SetHorizontalLoad(bool value)
     {
+        if(RobotConfiguration.IsSimulation) return;
         WritePLC(client => client.WriteSingleCoil(SetHorizontalLoadAddress, value));
     }
 
     public void SetMutedBase(bool muted)
     {
+        if (RobotConfiguration.IsSimulation) return;
         WritePLC(client => client.WriteSingleCoil(SetMutedBaseAddress, muted));
     }
 
     public void SetMutedLoad(bool muted)
     {
+        if (RobotConfiguration.IsSimulation) return;
         WritePLC(client => client.WriteSingleCoil(SetMutedLoadAddress, muted));
     }
 
     public void SetOnCharging(bool value)
     {
+        if (RobotConfiguration.IsSimulation) return;
         WritePLC(client => client.WriteSingleCoil(EnableChargingAddress, value));
     }
 
     public void SetProccessState(ProccessingState state)
     {
+        if (RobotConfiguration.IsSimulation) return;
         WritePLC(client =>
         {
             switch (state)
@@ -61,6 +66,7 @@ public partial class RobotPeripheral(RobotConfiguration RobotConfiguration, IErr
 
     public void SetSytemState(SystemState state)
     {
+        if (RobotConfiguration.IsSimulation) return;
         WritePLC(client =>
         {
             if (state != SystemState.ERROR) client.WriteSingleCoil(RobotStateErrorAddress, false);
@@ -139,9 +145,9 @@ public partial class RobotPeripheral(RobotConfiguration RobotConfiguration, IErr
             {
                 PeripheralMode = PeripheralMode.SERVICE;
             }
-            else if (switchs[1] && PeripheralMode != PeripheralMode.AUTO)
+            else if (switchs[1] && PeripheralMode != PeripheralMode.AUTOMATIC)
             {
-                PeripheralMode = PeripheralMode.AUTO;
+                PeripheralMode = PeripheralMode.AUTOMATIC;
             }
             else if (switchs[2] && PeripheralMode != PeripheralMode.MANUAL)
             {
@@ -196,6 +202,8 @@ public partial class RobotPeripheral(RobotConfiguration RobotConfiguration, IErr
             LiftedUp = sensors[2];
             LiftedDown = sensors[3];
             LiftHome = sensors[4];
+            if(Emergency) OnStop?.Invoke(StopStateType.EMC);
+            else if (Bumper) OnStop?.Invoke(StopStateType.Bumber);
         }
     }
 
@@ -262,6 +270,7 @@ public partial class RobotPeripheral(RobotConfiguration RobotConfiguration, IErr
     {
         await Task.Yield();
         PeripheralReaderTimer?.Dispose();
+        PeripheralReaderTimer = null;
         IsReady = false;
         _ = base.StopAsync(cancellationToken);
     }

RobotApp/Services/Robot/RobotPeripheralData.cs

@@ -4,7 +4,7 @@ namespace RobotApp.Services.Robot;
 
 public partial class RobotPeripheral
 {
-    public PeripheralMode PeripheralMode { get; private set; } = PeripheralMode.SERVICE;
+    public PeripheralMode PeripheralMode { get; private set; } = RobotConfiguration.IsSimulation ? PeripheralMode.AUTOMATIC : PeripheralMode.SERVICE;
     public bool Emergency { get; private set; }
     public bool Bumper { get; private set; }
     public bool LiftedUp { get; private set; }
@@ -15,11 +15,11 @@ public partial class RobotPeripheral
     public bool LidarBackProtectField { get; private set; }
     public bool LidarFrontTimProtectField { get; private set; }
 
-    public bool LeftMotorReady { get; private set; }
-    public bool RightMotorReady { get; private set; }
-    public bool LiftMotorReady { get; private set; }
+    public bool LeftMotorReady { get; private set; } = RobotConfiguration.IsSimulation;
+    public bool RightMotorReady { get; private set; } = RobotConfiguration.IsSimulation;
+    public bool LiftMotorReady { get; private set; } = RobotConfiguration.IsSimulation;
 
-    public bool ButtonStart { get; private set; }
+    public bool ButtonStart { get; private set; } = RobotConfiguration.IsSimulation;
     public bool ButtonStop { get; private set; }
     public bool ButtonReset { get; private set; }
 

RobotApp/Services/Robot/RobotStates.cs

@@ -1,15 +1,26 @@
 using RobotApp.Common.Shares;
 using RobotApp.Interfaces;
+using RobotApp.Services.State;
 using RobotApp.VDA5050;
 using RobotApp.VDA5050.State;
+using RobotApp.VDA5050.Type;
 using System.Text.Json;
 
 namespace RobotApp.Services.Robot;
 
-public class RobotStates(RobotConfiguration RobotConfiguration, RobotConnection ConnectionManager,
+public class RobotStates(RobotConfiguration RobotConfiguration, 
+                         RobotConnection ConnectionManager,
+                         RobotStateMachine StateManager,
                          Logger<RobotStates> Logger,
                          IOrder OrderManager,
-                         IPeripheral PeripheralManager) : BackgroundService
+                         IInstantActions ActionManager,
+                         IPeripheral PeripheralManager,
+                         IInfomation InfoManager,
+                         IError ErrorManager, 
+                         ILocalization LocalizationManager,
+                         IBattery BatteryManager,
+                         ILoad LoadManager,
+                         IDriver DriverManager) : BackgroundService
 {
     private uint HeaderId = 0;
     private readonly string SerialNumber = RobotConfiguration.SerialNumber;
@@ -43,9 +54,59 @@ public class RobotStates(RobotConfiguration RobotConfiguration, RobotConnection
             NewBaseRequest = false,
             DistanceSinceLastNode = 0,
             OperatingMode = PeripheralManager.PeripheralMode.ToString(),
+            NodeStates = OrderManager.NodeStates,
+            EdgeStates = OrderManager.EdgeStates,
+            ActionStates = ActionManager.ActionStates,
+            Information = [General, ..InfoManager.InformationState],
+            Errors = ErrorManager.ErrorsState,
+            AgvPosition = new()
+            {
+                X = LocalizationManager.X,
+                Y = LocalizationManager.Y,
+                Theta = LocalizationManager.Theta,
+                LocalizationScore = LocalizationManager.MatchingScore,
+                MapId = LocalizationManager.CurrentActiveMap,
+                DeviationRange = LocalizationManager.Reliability,
+                PositionInitialized = LocalizationManager.IsReady,
+            },
+            BatteryState = new()
+            {
+                Charging = BatteryManager.IsCharging,
+                BatteryHealth = BatteryManager.SOH,
+                Reach = BatteryManager.RemainingCapacity,
+                BatteryVoltage = BatteryManager.Voltage,
+                BatteryCharge = BatteryManager.SOC,
+            },
+            Loads = LoadManager.Load,
+            Velocity = new()
+            {
+                Vx = DriverManager.LinearVelocity,
+                Vy = 0,
+                Omega = DriverManager.AngularVelocity,
+            },
+            SafetyState = new()
+            {
+                FieldViolation = PeripheralManager.LidarBackProtectField || PeripheralManager.LidarFrontProtectField || PeripheralManager.LidarFrontTimProtectField,
+                EStop = PeripheralManager.Emergency || PeripheralManager.Bumper ? EStop.AUTOACK.ToString() : EStop.NONE.ToString(),
+            }
         };
     }
 
+    private Information General => new()
+    {
+        InfoType = InformationType.robot_general.ToString(),
+        InfoDescription = "Thông tin chung của robot",
+        InfoLevel = InfoLevel.INFO.ToString(),
+        InfoReferences =
+        [
+            new InfomationReferences
+            {
+                ReferenceKey = InformationReferencesKey.robot_state.ToString(),
+                ReferenceValue = StateManager.CurrentStateName,
+            },
+        ],
+    };
+
     private async Task UpdateStateHandler()
     {
         await PubState();
@@ -69,6 +130,7 @@ public class RobotStates(RobotConfiguration RobotConfiguration, RobotConnection
     public override Task StopAsync(CancellationToken cancellationToken)
     {
         UpdateStateTimer?.Dispose();
+        UpdateStateTimer = null;
         return base.StopAsync(cancellationToken);
     }
 }

RobotApp/Services/Robot/RobotVisualization.cs

@@ -62,6 +62,7 @@ public class RobotVisualization(ILocalization Localization, INavigation Navigati
     public override Task StopAsync(CancellationToken cancellationToken)
     {
         UpdateTimer?.Dispose();
+        UpdateTimer = null;
         return base.StopAsync(cancellationToken);
     }
 }

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

@@ -29,7 +29,7 @@ public class PID
         return this;
     }
 
-    public double PID_step(double error, double min, double max, double timeSample)
+    public double PID_step(double error, double max, double min, double timeSample)
     {
         DateTime CurrentTime = DateTime.Now;
         double P_part = Kp * (error - Pre_Error);

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

@@ -7,7 +7,6 @@ public class PurePursuit
     private double LookaheadDistance = 0.5;
     private List<NavigationNode> Waypoints_Value = [];
     public int OnNodeIndex = 0;
-    public int GoalIndex = 0;
     public NavigationNode? Goal;
 
     public PurePursuit WithLookheadDistance(double distance)
@@ -33,10 +32,10 @@ public class PurePursuit
         Waypoints_Value = [.. path];
     }
 
-    public void UpdateGoal(NavigationNode goal)
+    public void UpdateGoal(string goalId)
     {
-        Goal = goal;
-        GoalIndex = Waypoints_Value.IndexOf(Goal);
+        var goal = Waypoints_Value.FirstOrDefault(n => n.NodeId == goalId);
+        if (goal is not null) Goal = goal;
     }
 
     public (NavigationNode node, int index) GetOnNode(double x, double y)

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

@@ -65,14 +65,13 @@ public class DifferentialNavigation : SimulationNavigation
                             VelocityController.SetSpeed(AngularVelocityLeft, AngularVelocityRight, CycleHandlerMilliseconds / 1000.0);
                         }
                         else if (DistanceToGoal < 0.02) Dispose();
-                        else NavState = NavigationState.Waiting;
                     }
                 }
             }
         }
         catch (Exception ex)
         {
-            Logger.Write($"Error in DifferentialNavigation: {ex.Message}", LogLevel.Error);
+            Logger.Error($"Error in DifferentialNavigation: {ex.Message}");
         }
     }
 }

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

@@ -16,7 +16,10 @@ public class SimulationNavigation : INavigation, IDisposable
     public double VelocityY => Visualization.Vy;
     public double Omega => Visualization.Omega;
 
-    protected NavigationState NavState = NavigationState.None;
+    public event Action? OnNavigationStateChanged;
+    public event Action? OnNavigationFinished;
+
+    protected NavigationState NavState = NavigationState.Idle;
     protected bool NavDriving = false;
 
     protected const int CycleHandlerMilliseconds = 50;
@@ -38,11 +41,13 @@ public class SimulationNavigation : INavigation, IDisposable
     protected readonly SimulationVelocity VelocityController;
     protected readonly RobotConfiguration RobotConfiguration;
     protected readonly RobotPathPlanner PathPlanner;
+
     protected NavigationNode[] NavigationPath = [];
     protected NavigationNode? CurrentBaseNode;
     protected NavigationState ResumeState = NavigationState.None;
 
     private readonly Logger<SimulationNavigation> Logger;
+
     private bool IsIdle => NavState == NavigationState.Idle || NavState == NavigationState.Canceled;
 
     public SimulationNavigation(IServiceProvider ServiceProvider)
@@ -53,6 +58,7 @@ public class SimulationNavigation : INavigation, IDisposable
         RobotConfiguration = scope.ServiceProvider.GetRequiredService<RobotConfiguration>();
         PathPlanner = scope.ServiceProvider.GetRequiredService<RobotPathPlanner>();
         VelocityController = new(Visualization, RobotConfiguration.SimulationModel);
+        AngularVelocity = RobotConfiguration.SimulationModel.MaxAngularVelocity * RobotConfiguration.SimulationModel.Width / 2 / 2 / RobotConfiguration.SimulationModel.RadiusWheel;
     }
 
     protected void HandleNavigationStart()
@@ -64,6 +70,7 @@ public class SimulationNavigation : INavigation, IDisposable
     protected void HandleNavigationStop()
     {
         NavigationTimer?.Dispose();
+        NavigationTimer = null;
     }
 
     protected virtual void NavigationHandler() { }
@@ -71,7 +78,6 @@ public class SimulationNavigation : INavigation, IDisposable
     public void CancelMovement()
     {
         Dispose();
-        NavState = NavigationState.Canceled;
     }
 
     public void Move(Node[] nodes, Edge[] edges)
@@ -140,7 +146,7 @@ public class SimulationNavigation : INavigation, IDisposable
         UpdateOrder(goalNode.NodeId);
     }
 
-    public void Paused()
+    public void Pause()
     {
         ResumeState = State;
         NavState = NavigationState.Paused;
@@ -154,7 +160,7 @@ public class SimulationNavigation : INavigation, IDisposable
 
     public void Rotate(double angle)
     {
-        if (!IsIdle) throw new SimulationException(RobotErrors.Error1012(NavState));
+        if (!IsIdle && NavState!= NavigationState.Initializing) throw new SimulationException(RobotErrors.Error1012(NavState));
         RotatePID = new PID().WithKp(10).WithKi(0.01).WithKd(0.1);
         TargetAngle = MathExtensions.NormalizeAngle(angle);
         NavState = NavigationState.Rotating;
@@ -163,6 +169,12 @@ public class SimulationNavigation : INavigation, IDisposable
 
     public void UpdateOrder(string lastBaseNodeId)
     {
+        var lastBaseNode = SimulationOrderNodes.FirstOrDefault(n => n.NodeId == lastBaseNodeId);
+        if (lastBaseNode is not null && lastBaseNode.NodeId != CurrentBaseNode?.NodeId)
+        {
+            CurrentBaseNode = lastBaseNode;
+            MovePurePursuit?.UpdateGoal(lastBaseNode.NodeId);
+        }
     }
 
     public void RefreshOrder(Node[] nodes, Edge[] edges)
@@ -176,9 +188,12 @@ public class SimulationNavigation : INavigation, IDisposable
         VelocityController.Stop();
         OrderNodes = [];
         OrderEdges = [];
+        CurrentBaseNode = null;
+        NavigationPath = [];
         SimulationOrderNodes = [];
         NavDriving = false;
-        NavState = NavigationState.None;
+        NavState = NavigationState.Idle;
+        OnNavigationFinished?.Invoke();
         GC.SuppressFinalize(this);
     }
 

RobotApp/Services/Robot/Simulation/SimulationModel.cs

@@ -8,7 +8,7 @@ public class SimulationModel
     public readonly double Width = 0.606;
     public readonly double Length = 1.106;
     public readonly double MaxVelocity = 1.5;
-    public readonly double MaxAngularVelocity = 0.3;
+    public readonly double MaxAngularVelocity = 0.5;
     public readonly double Acceleration = 2;
     public readonly double Deceleration = 10;
     public readonly NavigationType NavigationType = NavigationType.Differential;

RobotApp/Services/RobotExtensions.cs

@@ -13,20 +13,23 @@ public static class RobotExtensions
         services.AddSingleton<RobotConfiguration>();
         services.AddSingleton<RobotPathPlanner>();
         services.AddSingleton<RobotConnection>();
-
+        services.AddSingleton<RobotActionStorage>();
         services.AddInterfaceServiceSingleton<IBattery, RobotBattery>();
         services.AddInterfaceServiceSingleton<IDriver, RobotDriver>();
         services.AddInterfaceServiceSingleton<IError, RobotErrors>();
         services.AddInterfaceServiceSingleton<IInfomation, RobotInfomations>();
-        services.AddInterfaceServiceSingleton<IInstantActions, RobotActionController>();
         services.AddInterfaceServiceSingleton<INavigation, RobotNavigation>();
         services.AddInterfaceServiceSingleton<IOrder, RobotOrderController>();
+        services.AddInterfaceServiceSingleton<ILoad, RobotLoads>();
 
         services.AddHostedInterfaceServiceSingleton<IPeripheral, ISafety, RobotPeripheral>();
         services.AddHostedInterfaceServiceSingleton<ILocalization, RobotLocalization>();
+        services.AddHostedInterfaceServiceSingleton<IInstantActions, RobotActionController>();
 
         services.AddHostedServiceSingleton<RobotController>();
         services.AddHostedServiceSingleton<RobotVisualization>();
+        services.AddHostedServiceSingleton<RobotStates>();
+        services.AddHostedServiceSingleton<RobotFactsheet>();
         return services;
     }
 

RobotApp/Services/State/IRobotState.cs

@@ -13,7 +13,6 @@ public interface IRobotState
 
     event Action<IRobotState>? OnEnter;
     event Action<IRobotState>? OnExit;
-    event Action<IRobotState , IRobotState>? OnTransition;
     void Enter();
     void Exit();
     bool CanTransitionTo(IRobotState targetState);

RobotApp/Services/State/RobotStateMachine.cs

@@ -302,7 +302,7 @@ public record RobotStateMachine(Logger<RobotStateMachine> Logger) : IDisposable
     {
         var stopState = GetState(RootStateType.Stop);
         var subStopState = stopState.SubStates.FirstOrDefault(s => s.Name.Equals(stopType));
-        if(subStopState is not null)
+        if (subStopState is not null)
         {
             var transitionToStop = TransitionTo(RootStateType.Stop);
             if (transitionToStop)
@@ -343,6 +343,19 @@ public record RobotStateMachine(Logger<RobotStateMachine> Logger) : IDisposable
         return current.Name.ToString();
     }
 
+    public bool HasState(string stateName)
+    {
+        if (CurrentState == null || string.IsNullOrEmpty(stateName)) return false;
+
+        var current = CurrentState;
+        while (current != null)
+        {
+            if (current.Name.ToString() == stateName) return true;
+            current = current.SuperState;
+        }
+        return false;
+    }
+
     public string GetCurrentStatePath()
     {
         if (CurrentState == null) return "Unknown";

RobotApp/appsettings.json

@@ -10,15 +10,9 @@
     }
   },
   "AllowedHosts": "*",
-  "Simulation": {
-    "Enable": true,
-    "RadiusWheel": 0.1,
-    "Width": 0.606,
-    "Length": 1.106,
-    "MaxVelocity": 1.5,
-    "MaxAngularVelocity": 0.3,
-    "Acceleration": 2,
-    "Deceleration": 10,
-    "NavigationType": "Differential"
+  "PathPlanning": {
+    "ResolutionSplit": 0.1,
+    "ReverseDirectionAngleDegree": 89,
+    "Ratio":  0.1
   }
 }