update

RobotApp.Client/Pages/Components/Mapping/MapView.razor.css

@@ -5,7 +5,7 @@
     display: flex;
     flex-direction: column;
     position: relative;
-    background-color: var(--mud-palette-surface);
+    background-color: #808080;
     border-radius: var(--mud-default-borderradius);
     transition: box-shadow 300ms cubic-bezier(0.4, 0, 0.2, 1) 0ms;
     box-shadow: var(--mud-elevation-10);

RobotApp.Common.Shares/Enums/StateType.cs

@@ -2,22 +2,29 @@
 
 public enum RootStateType
 {
-    Booting,
-    Operational,
+    System,
+    Auto,
+    Manual,
+    Service,
+    Stop,
+    Fault,
 }
 
-public enum OperationalStateType
+public enum SystemStateType
 {
-
+    Initializing,
+    Standby,
+    Shutting_Down,
 }
 
-public enum AutomationStateType
+public enum AutoStateType
 {
     Idle,
     Executing,
     Paused,
-    Charging,
-    Error,
+    Holding,
+    Canceling,
+    Recovering,
     Remote_Override,
 }
 
@@ -27,13 +34,60 @@ public enum ManualStateType
     Active,
 }
 
-public enum SafetyStateType
+public enum ServiceStateType
 {
-    Init,
-    Run_Ok,
-    SS1,
-    STO,
-    PDS,
-    SLS,
-    Error,
+    Idle,
+    Active,
+}
+
+public enum StopStateType
+{
+    EMC,
+    Protective,
+    Manual,
+}
+
+public enum FaultStateType
+{
+    Navigation,
+    Localization,
+    Shielf,
+    Battery,
+    Driver,
+    Peripherals,
+    Safety,
+    Communication,
+}
+
+public enum ExecutingStateType
+{
+    Planning,
+    Moving,
+    ACT,
+}
+
+public enum ACTStateType
+{
+    Docking,
+    Docked,
+    Charging,
+    Undocking,
+    Loading,
+    Unloading,
+    TechAction,
+}
+
+public enum MoveStateType
+{
+    Navigation,
+    Avoidance,
+    Approach,
+    Tracking,
+    Repositioning,
+}
+
+public enum PlanStateType
+{
+    Task,
+    Path,
 }

RobotApp.Common.Shares/MathExtensions.cs

@@ -4,6 +4,14 @@ namespace RobotApp.Common.Shares;
 
 public static class MathExtensions
 {
+    public static double NormalizeAngle(double angle)
+    {
+        angle = angle % 360;
+        if (angle > 180)  angle -= 360;
+        else if (angle < -180) angle += 360;
+        return angle;
+    }
+
     public static (double x, double y) CurveDegreeTwo(double t, double x1, double y1, double controlPointX, double controlPointY, double x2, double y2)
     {
         var x = (1 - t) * (1 - t) * x1 + 2 * t * (1 - t) * controlPointX + t * t * x2;
@@ -77,4 +85,21 @@ public static class MathExtensions
         }
         return distance;
     }
+
+    public static double GetVectorAngle(double originNodeX, double originNodeY, double vector1X, double vector1Y, double vector2X, double vector2Y)
+    {
+        double BA_x = vector1X - originNodeX;
+        double BA_y = vector1Y - originNodeY;
+        double BC_x = vector2X - originNodeX;
+        double BC_y = vector2Y - originNodeY;
+        // Tính độ dài của các vector AB và BC
+        double lengthAB = Math.Sqrt(BA_x * BA_x + BA_y * BA_y);
+        double lengthBC = Math.Sqrt(BC_x * BC_x + BC_y * BC_y);
+        // Tính tích vô hướng của AB và BC
+        double dotProduct = BA_x * BC_x + BA_y * BC_y;
+        if (lengthAB * lengthBC == 0) return 0;
+        if (dotProduct / (lengthAB * lengthBC) > 1) return 0;
+        if (dotProduct / (lengthAB * lengthBC) < -1) return 180;
+        return Math.Acos(dotProduct / (lengthAB * lengthBC)) * (180.0 / Math.PI);
+    }
 }

RobotApp/Interfaces/IBattery.cs

@@ -1,5 +1,31 @@
 namespace RobotApp.Interfaces;
 
+public enum BatteryStatus
+{
+    Unknown,
+    Charging,
+    Discharging,
+    Full,
+    NotCharging,
+    Fault,
+    Standby
+}
+
 public interface IBattery
 {
+    bool IsReady { get; }
+    double Voltage { get; }
+    double Current { get; }
+    double SOC { get; }
+    double SOH { get; }
+    BatteryStatus Status { get; }
+    int ChargeTime { get; }
+    int DischargeTime { get; }
+    double Temperature { get; }
+    double RemainingCapacity { get; }
+    double RemainingEnergy { get; }
+    int DischargeCycles { get; }
+    int ChargeCycles { get; }
+    bool IsCharging { get; }
+    string[] ErrorStatus { get; }
 }

RobotApp/Interfaces/IError.cs

@@ -6,4 +6,6 @@ public interface IError
 {
     bool HasFatalError { get; }
     void AddError(Error error, TimeSpan? clearAfter = null);
+    void DeleteError(string errorType);
+    void ClearAllErrors();
 }

RobotApp/Interfaces/IInfomation.cs

@@ -2,4 +2,5 @@
 
 public interface IInfomation
 {
+
 }

RobotApp/Interfaces/IInstantActions.cs

@@ -3,7 +3,7 @@ using Action = RobotApp.VDA5050.InstantAction.Action;
 
 namespace RobotApp.Interfaces;
 
-public interface IInstanceActions
+public interface IInstantActions
 {
     ActionState[] ActionStates { get; }
     bool HasActionRunning { get; }

RobotApp/Interfaces/ILocalization.cs

@@ -1,7 +1,14 @@
-namespace RobotApp.Interfaces;
+using RobotApp.Common.Shares;
+
+namespace RobotApp.Interfaces;
 
 public interface ILocalization
 {
+    /// <summary>
+    /// Trạng thái sẵn sàng của hệ thống định vị.
+    /// </summary>
+    bool IsReady { get; }
+
     /// <summary>
     /// Vị trí hiện tại của robot trong hệ tọa độ toàn cục theo trục X (đơn vị: mét).
     /// </summary>
@@ -13,10 +20,36 @@ public interface ILocalization
     double Y { get; }
 
     /// <summary>
-    /// Hướng hiện tại của robot trong hệ tọa độ toàn cục (đơn vị: độ).
+    /// Hướng hiện tại của robot trong hệ tọa độ toàn cục (đơn vị: radian).
     /// </summary>
     double Theta { get; }
 
+    /// <summary>
+    /// Trang thái SLAM hiện tại của robot.
+    /// </summary>
+    string SlamState { get; }
+
+    /// <summary>
+    /// Thông tin chi tiết về trạng thái SLAM hiện tại của robot.
+    /// </summary>
+    string SlamStateDetail { get; }
+
+    /// <summary>
+    /// Bản đồ đang được sử dụng để định vị hiện tại.
+    /// </summary>
+    string CurrentActiveMap { get; }
+
+
+    /// <summary>
+    /// Độ tin cậy của vị trí định vị hiện tại (0.0 - 100.0).
+    /// </summary>
+    double Reliability { get; }
+
+    /// <summary>
+    /// Độ phù hợp của vị trí định vị hiện tại so với bản đồ (0.0 - 100.0).
+    /// </summary>
+    double MatchingScore { get; }   
+
     /// <summary>
     /// Khởi tạo vị trí của robot trong hệ tọa độ toàn cục.
     /// </summary>
@@ -24,5 +57,57 @@ public interface ILocalization
     /// <param name="y">đơn vị: mét</param>
     /// <param name="theta">đơn vị: độ</param>
     /// <returns></returns>
-    bool InitializePosition(double x, double y, double theta);
+    MessageResult SetInitializePosition(double x, double y, double theta);
+
+    /// <summary>
+    /// Bắt đầu quá trình lập bản đồ với độ phân giải chỉ định.
+    /// </summary>
+    /// <param name="resolution">đơn vị mét/px</param>
+    /// <returns></returns>
+    MessageResult StartMapping(double resolution);
+
+    /// <summary>
+    /// Dừng quá trình lập bản đồ hiện tại và lưu bản đồ với tên chỉ định.
+    /// </summary>
+    /// <param name="mapName"></param>
+    /// <returns></returns>
+    MessageResult StopMapping(string mapName);
+
+    /// <summary>
+    /// Bắt đầu quá trình định vị sử dụng bản đồ đã lưu.
+    /// </summary>
+    /// <returns></returns>
+    MessageResult StartLocalization();
+
+    /// <summary>
+    /// Dừng quá trình định vị hiện tại.
+    /// </summary>
+    /// <returns></returns>
+    MessageResult StopLocalization();
+
+    /// <summary>
+    /// Kích hoạt bản đồ đã lưu để sử dụng trong quá trình định vị.
+    /// </summary>
+    /// <param name="mapName"></param>
+    /// <returns></returns>
+    MessageResult ActivateMap(string mapName);
+
+    /// <summary>
+    /// Chuyển sang bản đồ đã lưu và khởi tạo vị trí của robot trên bản đồ đó.
+    /// </summary>
+    /// <param name="mapName"></param>
+    /// <param name="x"></param>
+    /// <param name="y"></param>
+    /// <param name="theta"></param>
+    /// <returns></returns>
+    MessageResult SwitchMap(string mapName, double x, double y, double theta);
+
+    /// <summary>
+    /// Thay đổi gốc tọa độ của bản đồ hiện tại.
+    /// </summary>
+    /// <param name="x"></param>
+    /// <param name="y"></param>
+    /// <param name="theta"></param>
+    /// <returns></returns>
+    MessageResult ChangeMapOrigin(double x, double y, double theta);
 }

RobotApp/Interfaces/INavigation.cs

@@ -5,11 +5,12 @@ namespace RobotApp.Interfaces;
 public enum NavigationState
 {
     None,
-    Initializing,
-    Initialized,
     Idle,
+    Initializing,
+    Waiting,
     Moving,
     Rotating,
+    Canceled,
     Paused,
     Error
 }
@@ -25,13 +26,19 @@ public enum NavigationProccess
 
 public interface INavigation
 {
+    bool IsReady { get; }
     bool Driving { get; }
+    double VelocityX { get; }
+    double VelocityY { get; }
+    double Omega { get; }
     NavigationState State { get; }
     void Move(Node[] nodes, Edge[] edges);
     void MoveStraight(double x, double y);
     void Rotate(double angle);
     void Paused();
     void Resume();
-    void UpdateOrder(int lastBaseSequence);
+    void UpdateOrder(string lastBaseNodeId);
+    void RefreshOrder(Node[] nodes, Edge[] edges);
+    void Refresh();
     void CancelMovement();
 }

RobotApp/Interfaces/IPeripheral.cs

@@ -1,14 +1,35 @@
 namespace RobotApp.Interfaces;
 
-public enum OperatingMode
+public enum PeripheralMode
 {
     AUTOMATIC,
     MANUAL,
-    SEMIAUTOMATIC,
-    TEACHIN,
     SERVICE,
 }
 public interface IPeripheral
 {
-    OperatingMode OperatingMode { get; }
+    bool IsReady { get; }
+    PeripheralMode PeripheralMode { get; }
+    bool Emergency { get; }
+    bool Bumper { get; }
+    bool LiftedUp { get; }
+    bool LiftedDown { get; }
+    bool LiftHome { get; }
+    bool LeftMotorReady { get; }
+    bool RightMotorReady { get; }
+    bool LiftMotorReady { get; }
+    bool SwitchLock { get; }
+    bool SwitchManual { get; }
+    bool SwitchAuto { get; }
+    bool ButtonStart { get; }
+    bool ButtonStop { get; }
+    bool ButtonReset { get; }
+    bool HasLoad { get; }
+    bool MutedBase { get; }
+    bool MutedLoad { get; }
+    bool StartedCharging { get; }
+
+    bool SetSytemState();
+    bool SetProccessState();
+    bool SetOnCharging();
 }

RobotApp/Interfaces/IRFHandler.cs

@@ -1,6 +1,29 @@
-namespace RobotApp.Interfaces
+namespace RobotApp.Interfaces;
+
+public enum RFHandlerMode
 {
-    public class IRFHandler
+    Default,    
+    Maintenance,
+    Override,
+    Unknown
+}
+
+public interface IRFHandler
 {
-    }
+    bool IsConnected { get; }
+    bool IsEnabled { get; }
+    bool IsLocked { get; }
+    bool EStop { get; }
+    bool LiftUp { get; }
+    bool LiftDown { get; }
+    int Speed { get; }
+    bool Forward { get; }   
+    bool Backward { get; }
+    bool RotateLeft { get; }
+    bool RotateRight { get; }
+    bool Left { get; }
+    bool Right { get; }
+    RFHandlerMode Mode { get; }
+    event Action<RFHandlerMode> OnStateChanged;
+    event Action<bool>? OnConnectionChanged;
 }

RobotApp/Interfaces/ISafety.cs

@@ -2,4 +2,17 @@
 
 public interface ISafety
 {
+    bool SpeedVerySlow { get; }
+    bool SpeedSlow { get; }
+    bool SpeedNormal { get; }
+    bool SpeedMedium { get; }
+    bool SpeedOptimal { get; }
+    bool SpeedFast { get; }
+    bool SpeedVeryFast { get; }
+    bool LidarFrontProtectField { get; }
+    bool LidarBackProtectField { get; }
+    bool LidarFrontTimProtectField { get; }
+    bool SetMutedLoad(bool muted);
+    bool SetMutedBase(bool muted);
+    bool SetHorizontalLoad();
 }

RobotApp/Interfaces/ISensorIMU.cs

@@ -5,6 +5,11 @@
 /// </summary>
 public interface ISensorIMU
 {
+    /// <summary>
+    /// Trạng thái sẵn sàng của cảm biến IMU
+    /// </summary>
+    bool IsReady { get; }
+
     /// <summary>
     /// Góc xoay của robot (đơn vị độ)
     /// </summary>

RobotApp/Program.cs

@@ -5,7 +5,9 @@ using MudBlazor.Services;
 using RobotApp.Components;
 using RobotApp.Components.Account;
 using RobotApp.Data;
+using RobotApp.Services;
 using RobotApp.Services.Robot;
+using RobotApp.Services.Robot.Simulation;
 
 var builder = WebApplication.CreateBuilder(args);
 
@@ -46,7 +48,9 @@ builder.Services.AddSingleton<IEmailSender<ApplicationUser>, IdentityNoOpEmailSe
 
 builder.Services.AddSingleton(typeof(RobotApp.Services.Logger<>));
 builder.Services.AddSingleton<RobotConnection>();
-builder.Services.AddHostedService(sp => sp.GetRequiredService<RobotConnection>());
+
+builder.Services.AddRobotSimulation();
+builder.Services.AddRobot();
 
 var app = builder.Build();
 await app.Services.SeedApplicationDbAsync();

RobotApp/RobotApp.csproj

@@ -29,8 +29,4 @@
     <PackageReference Include="MQTTnet" Version="5.0.1.1416" />
   </ItemGroup>
 
-  <ItemGroup>
-    <Folder Include="Services\Robot\Simulation\" />
-  </ItemGroup>
-
 </Project>

RobotApp/Services/Exceptions/ActionException.cs

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

RobotApp/Services/Exceptions/ModbusException.cs

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

RobotApp/Services/Exceptions/PathPlannerException.cs

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

RobotApp/Services/Exceptions/SimulationException.cs

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

RobotApp/Services/ModbusTcpClient.cs

@@ -13,11 +13,12 @@ public class ModbusTcpClient(string IpAddress, int Port, byte ClientId) : IDispo
     private NetworkStream? stream;
     private bool disposed = false;
 
+    private readonly Lock LockObject = new();
     private uint transactionIdentifierInternal = 0;
     private const int connectTimeout = 3000;
     private const int readTimeout = 500;
     private const int writeTimeout = 500;
-    private int numberOfRetries { get; set; } = 3;
+    private const int numberOfRetries = 3;
 
     ~ModbusTcpClient()
     {
@@ -101,9 +102,28 @@ public class ModbusTcpClient(string IpAddress, int Port, byte ClientId) : IDispo
         }
     }
     
+    private uint GetNextTransactionId()
+    {
+        lock (LockObject)
+        {
+            return ++transactionIdentifierInternal;
+        }
+    }
+
+    private static bool ShouldRetry(Exception ex)
+    {
+        return ex is ConnectionAbortedException ||
+               ex is TimeoutException ||
+               ex is SocketException ||
+               ex is InvalidOperationException ||
+              (ex is IOException && ex.InnerException is SocketException);
+    }
+
     private byte[] Write(byte functionCode, ushort startingAddress, ushort quantity, CancellationToken? cancellationToken = null)
         => Write(functionCode, startingAddress, quantity, [], cancellationToken);
     private byte[] Write(byte functionCode, ushort startingAddress, ushort quantity, byte[] multipleData, CancellationToken? cancellationToken = null)
+        => Write(functionCode, startingAddress, quantity, multipleData, 0, cancellationToken);
+    private byte[] Write(byte functionCode, ushort startingAddress, ushort quantity, byte[] multipleData, int retryAttempt, CancellationToken? cancellationToken = null)
     {
         try
         {
@@ -112,11 +132,11 @@ public class ModbusTcpClient(string IpAddress, int Port, byte ClientId) : IDispo
                 if (!Connect() || !IsConnected || stream is null) throw new ConnectionAbortedException();
             }
 
-            transactionIdentifierInternal++;
+            var transactionId = GetNextTransactionId();
             byte[] writeData = new byte[12 + multipleData.Length];
             var dataLength = multipleData.Length + 6;
-            writeData[0] = (byte)(transactionIdentifierInternal >> 8);
-            writeData[1] = (byte)(transactionIdentifierInternal & 0xFF);
+            writeData[0] = (byte)(transactionId >> 8);
+            writeData[1] = (byte)(transactionId & 0xFF);
             writeData[2] = 0x00;
             writeData[3] = 0x00;
             writeData[4] = (byte)(dataLength >> 8);
@@ -171,6 +191,11 @@ public class ModbusTcpClient(string IpAddress, int Port, byte ClientId) : IDispo
             Array.Copy(readData, 9, receiveData, 0, receiveData.Length);
             return receiveData;
         }
+        catch (Exception ex) when (ShouldRetry(ex) && retryAttempt >= numberOfRetries)
+        {
+            Dispose(true);
+            return Write(functionCode, startingAddress, quantity, multipleData, retryAttempt + 1, cancellationToken);
+        }
         catch (Exception ex) when (!(ex is ModbusException || ex is TimeoutException))
         {
             throw new ModbusException("Communication error", ex);

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

@@ -1,156 +0,0 @@
-//namespace RobotApp.Services.Navigation.Algorithm;
-//public class PurePursuit
-//{
-//    private double MaxAngularVelocity = 1.5;
-//    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)
-//    {
-//        LookaheadDistance = distance;
-//        return this;
-//    }
-
-//    public PurePursuit WithMaxAngularVelocity(double vel)
-//    {
-//        MaxAngularVelocity = vel;
-//        return this;
-//    }
-
-//    public PurePursuit WithPath(NavigationNode[] path)
-//    {
-//        Waypoints_Value = [.. path];
-//        return this;
-//    }
-
-//    public void UpdatePath(NavigationNode[] path)
-//    {
-//        Waypoints_Value = [.. path];
-//    }
-
-//    public void UpdateGoal(NavigationNode goal)
-//    {
-//        Goal = goal;
-//        GoalIndex = Waypoints_Value.IndexOf(Goal);
-//    }
-
-//    public (NavigationNode node, int index) GetOnNode(double x, double y)
-//    {
-//        double minDistance = double.MaxValue;
-//        NavigationNode onNode = Waypoints_Value[0];
-//        int index = 0;
-//        for (int i = 1; i < Waypoints_Value.Count; i++)
-//        {
-//            var distance = Math.Sqrt(Math.Pow(x - Waypoints_Value[i].X, 2) + Math.Pow(y - Waypoints_Value[i].Y, 2));
-//            if (distance < minDistance)
-//            {
-//                onNode = Waypoints_Value[i];
-//                minDistance = distance;
-//                index = i;
-//            }
-//        }
-//        return (onNode, index);
-//    }
-
-//    private (NavigationNode? node, int index) OnNode(double x, double y)
-//    {
-//        if (Waypoints_Value is null || Waypoints_Value.Count == 0) return (null, 0);
-//        double minDistance = double.MaxValue;
-//        int index = 0;
-//        NavigationNode? onNode = null;
-//        if (Goal is null) return (onNode, index);
-//        for (int i = OnNodeIndex; i < Waypoints_Value.IndexOf(Goal); i++)
-//        {
-//            var distance = Math.Sqrt(Math.Pow(x - Waypoints_Value[i].X, 2) + Math.Pow(y - Waypoints_Value[i].Y, 2));
-//            if (distance < minDistance)
-//            {
-//                onNode = Waypoints_Value[i];
-//                minDistance = distance;
-//                index = i;
-//            }
-//        }
-//        return (onNode, index);
-//    }
-
-//    public double PurePursuit_step(double X_Ref, double Y_Ref, double Angle_Ref)
-//    {
-//        if (Waypoints_Value is null || Waypoints_Value.Count < 2) return 0;
-//        NavigationNode? lookaheadStartPt = null;
-//        var (onNode, index) = OnNode(X_Ref, Y_Ref);
-//        if (onNode is null || Goal is null) return 0;
-//        OnNodeIndex = index;
-//        double lookDistance = 0;
-//        for (int i = OnNodeIndex + 1; i < Waypoints_Value.IndexOf(Goal); i++)
-//        {
-//            lookDistance += Math.Sqrt(Math.Pow(Waypoints_Value[i - 1].X - Waypoints_Value[i].X, 2) + Math.Pow(Waypoints_Value[i - 1].Y - Waypoints_Value[i].Y, 2));
-//            if (lookDistance >= LookaheadDistance || Waypoints_Value[i].Direction != onNode.Direction)
-//            {
-//                lookaheadStartPt = Waypoints_Value[i];
-//                break;
-//            }
-//        }
-//        lookaheadStartPt ??= Goal;
-//        if (onNode.Direction == RobotDirection.BACKWARD)
-//        {
-//            if (Angle_Ref > Math.PI) Angle_Ref -= Math.PI * 2;
-//            else if (Angle_Ref < -Math.PI) Angle_Ref += Math.PI * 2;
-//            Angle_Ref += Math.PI;
-//            if (Angle_Ref > Math.PI) Angle_Ref -= Math.PI * 2;
-//        }
-//        var distance = Math.Atan2(lookaheadStartPt.Y - Y_Ref, lookaheadStartPt.X - X_Ref) - Angle_Ref;
-
-//        if (Math.Abs(distance) > Math.PI)
-//        {
-//            double minDistance;
-//            if (distance + Math.PI == 0.0) minDistance = 0.0;
-//            else
-//            {
-//                double data = (distance + Math.PI) / (2 * Math.PI);
-//                if (data < 0) data = Math.Round(data + 0.5);
-//                else data = Math.Round(data - 0.5);
-//                minDistance = distance + Math.PI - data * (2 * Math.PI);
-//                double checker = 0;
-//                if (minDistance != 0.0)
-//                {
-//                    checker = Math.Abs((distance + Math.PI) / (2 * Math.PI));
-//                }
-//                if (!(Math.Abs(checker - Math.Floor(checker + 0.5)) > 2.2204460492503131E-16 * checker))
-//                {
-//                    minDistance = 0.0;
-//                }
-//                else if (distance + Math.PI < 0.0)
-//                {
-//                    minDistance += Math.PI * 2;
-//                }
-//            }
-//            if (minDistance == 0.0 && distance + Math.PI > 0.0)
-//            {
-//                minDistance = Math.PI * 2;
-//            }
-//            distance = minDistance - Math.PI;
-//        }
-
-//        var AngularVelocity = 2.0 * 0.5 * Math.Sin(distance) / LookaheadDistance;
-//        if (Math.Abs(AngularVelocity) > MaxAngularVelocity)
-//        {
-//            if (AngularVelocity < 0.0)
-//            {
-//                AngularVelocity = -1.0;
-//            }
-//            else if (AngularVelocity > 0.0)
-//            {
-//                AngularVelocity = 1.0;
-//            }
-//            else if (AngularVelocity == 0.0)
-//            {
-//                AngularVelocity = 0.0;
-//            }
-//            AngularVelocity *= MaxAngularVelocity;
-//        }
-//        return AngularVelocity;
-//    }
-
-//}

RobotApp/Services/Robot/Navigation/DifferentialNavigation.cs

@@ -1,22 +0,0 @@
-using RobotApp.Interfaces;
-
-namespace RobotApp.Services.Robot.Navigation;
-
-public class DifferentialNavigation(Logger<NavigationController> navLogger,
-                                    Logger<DifferentialNavigation> Logger,
-                                    IDriver Driver,
-                                    ISafety Safety,
-                                    ISensorIMU SensorIMU) : NavigationController(navLogger)
-{
-    protected override void NavigationHandler()
-    {
-        try
-        {
-            // Implement differential drive navigation logic here
-        }
-        catch (Exception ex)
-        {
-            Logger.Write($"Error in DifferentialNavigation: {ex.Message}", LogLevel.Error);
-        }
-    }
-}

RobotApp/Services/Robot/Navigation/NavigationController.cs

@@ -1,61 +0,0 @@
-using RobotApp.Interfaces;
-using RobotApp.VDA5050.Order;
-
-namespace RobotApp.Services.Robot.Navigation;
-
-public class NavigationController(Logger<NavigationController> Logger) : INavigation
-{
-    public NavigationState State { get; private set; } = NavigationState.None;
-    public bool Driving { get; private set; }
-
-    protected const int CycleHandlerMilliseconds = 20;
-    private WatchTimer<NavigationController>? NavigationTimer;
-
-    protected Node[] Nodes = [];
-    protected Edge[] Edges = [];
-
-    protected void HandleNavigationStart()
-    {
-        NavigationTimer = new(CycleHandlerMilliseconds, NavigationHandler, Logger);
-        NavigationTimer.Start();
-    }
-
-    protected void HandleNavigationStop()
-    {
-        NavigationTimer?.Dispose();
-    }
-
-    protected virtual void NavigationHandler() { }
-
-    public void CancelMovement()
-    {
-    }
-
-    public void Move(Node[] nodes, Edge[] edges)
-    {
-        Nodes = nodes;
-        Edges = edges;
-        State = NavigationState.Initializing;
-    }
-
-    public void MoveStraight(double x, double y)
-    {
-    }
-
-    public void Paused()
-    {
-    }
-
-    public void Resume()
-    {
-    }
-
-    public void Rotate(double angle)
-    {
-    }
-
-    public void UpdateOrder(int lastBaseSequence)
-    {
-        State = NavigationState.Initialized;
-    }
-}

RobotApp/Services/Robot/Navigation/NavigationManager.cs

@@ -1,6 +0,0 @@
-namespace RobotApp.Services.Robot.Navigation
-{
-    public class NavigationManager
-    {
-    }
-}

RobotApp/Services/Robot/RobotAction.cs

@@ -3,7 +3,7 @@ using RobotApp.VDA5050.State;
 
 namespace RobotApp.Services.Robot;
 
-public class RobotAction : IInstanceActions
+public class RobotAction : IInstantActions
 {
     public ActionState[] ActionStates { get; private set; } = [];
 

RobotApp/Services/Robot/RobotBattery.cs

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

RobotApp/Services/Robot/RobotConfiguration.cs

@@ -6,4 +6,17 @@ public class RobotConfiguration
 {
     public static string SerialNumber { get; set; } = "Robot-Demo";
     public static NavigationType NavigationType { get; set; } = NavigationType.Differential;
+    public static VDA5050.VDA5050Setting VDA5050Setting { get; set; } = new()
+    {
+        HostServer = "172.20.235.170",
+        Port = 1885,
+        UserName = "robotics",
+        Password = "robotics",
+        Manufacturer = "PhenikaaX",
+        Version = "0.0.1",
+        PublishRepeat = 2,
+    };
+    public static string PLCAddress { get; set; } = "127.0.0.1";
+    public static int PLCPort { get; set; } = 502;
+    public static int PLCUnitId { get; set; } = 1;
 }

RobotApp/Services/Robot/RobotConnection.cs

@@ -6,17 +6,13 @@ using System.Text.Json;
 
 namespace RobotApp.Services.Robot;
 
-public class RobotConnection(Logger<RobotConnection> Logger, Logger<MQTTClient> MQTTClientLogger) : BackgroundService
+public class RobotConnection(Logger<RobotConnection> Logger, Logger<MQTTClient> MQTTClientLogger)
 {
-    private readonly VDA5050Setting VDA5050Setting = new()
-    {
-        HostServer = "172.20.235.170",
-        Port = 1886,
-    };
+    private readonly VDA5050Setting VDA5050Setting = RobotConfiguration.VDA5050Setting;
     private MQTTClient? MqttClient;
 
     public bool IsConnected => MqttClient is not null && MqttClient.IsConnected;
-    public readonly static string SerialNumber = "RobotDemo";
+    public readonly static string SerialNumber = RobotConfiguration.SerialNumber;
 
     private void OrderChanged(string data)
     {
@@ -50,20 +46,17 @@ public class RobotConnection(Logger<RobotConnection> Logger, Logger<MQTTClient>
         return new(false, "Chưa có kết nối tới broker");
     }
 
-    protected override async Task ExecuteAsync(CancellationToken stoppingToken)
+    public async Task StartConnection(CancellationToken cancellationToken)
     {
-        await Task.Yield();
-
         MqttClient = new MQTTClient(SerialNumber, VDA5050Setting, MQTTClientLogger);
         MqttClient.OrderChanged += OrderChanged;
         MqttClient.InstanceActionsChanged += InstanceActionsChanged;
-        await MqttClient.ConnectAsync(stoppingToken);
-        await MqttClient.SubscribeAsync(stoppingToken);
+        await MqttClient.ConnectAsync(cancellationToken);
+        await MqttClient.SubscribeAsync(cancellationToken);
     }
 
-    public override async Task StopAsync(CancellationToken cancellationToken)
+    public async Task StopConnection()
     {
         if (MqttClient is not null) await MqttClient.DisposeAsync();
-        await base.StopAsync(cancellationToken);
     }
 }

RobotApp/Services/Robot/RobotController.cs

@@ -1,13 +1,14 @@
 using RobotApp.Interfaces;
 using RobotApp.Services.Exceptions;
+using RobotApp.Services.State;
+using RobotApp.VDA5050.InstantAction;
 using RobotApp.VDA5050.Order;
-using RobotApp.VDA5050.State;
 
 namespace RobotApp.Services.Robot;
 
 public class RobotController(IOrder OrderManager,
                              INavigation NavigationManager,
-                             IInstanceActions ActionManager,
+                             IInstantActions ActionManager,
                              IBattery BatteryManager,
                              ILocalization Localization,
                              IPeripheral PeripheralManager,
@@ -15,7 +16,8 @@ public class RobotController(IOrder OrderManager,
                              IError ErrorManager,
                              IInfomation InfomationManager,
                              Logger<RobotController> Logger,
-                             RobotConnection ConnectionManager) : BackgroundService
+                             RobotConnection ConnectionManager,
+                             RobotStateMachine StateManager) : BackgroundService
 {
     private readonly Mutex NewOrderMutex = new();
     private readonly Mutex NewInstanceMutex = new();
@@ -25,11 +27,25 @@ public class RobotController(IOrder OrderManager,
 
     protected override Task ExecuteAsync(CancellationToken stoppingToken)
     {
+        InitializationingHandler();
+
         UpdateStateTimer = new(UpdateStateInterval, UpdateStateHandler, Logger);
         UpdateStateTimer.Start();
         return Task.CompletedTask;
     }
 
+    private void InitializationingHandler()
+    {
+        try
+        {
+            StateManager.InitializeHierarchyStates();
+        }
+        catch
+        {
+
+        }
+    }
+
     private void UpdateStateHandler()
     {
         // xử lý cập nhật trạng thái robot và gửi thông tin qua kết nối
@@ -43,13 +59,13 @@ public class RobotController(IOrder OrderManager,
             {
                 if (!string.IsNullOrEmpty(OrderManager.OrderId))
                 {
-                    if (order.OrderId != OrderManager.OrderId) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1001", ErrorLevel.WARNING, $"Có order đang được thực hiện. OrderId: {OrderManager.OrderId}, OrderId mới: {order.OrderId}"));
+                    if (order.OrderId != OrderManager.OrderId) throw new OrderException(RobotErrors.Error1001(OrderManager.OrderId, order.OrderId));
                     OrderManager.UpdateOrder(order.OrderUpdateId, order.Nodes, order.Edges);
                 }
-                else if (PeripheralManager.OperatingMode != Interfaces.OperatingMode.AUTOMATIC) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1006", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi chế độ vận hành không phải là TỰ ĐỘNG. Chế độ hiện tại: {PeripheralManager.OperatingMode}"));
-                else if(ActionManager.HasActionRunning) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1007", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi có action đang thực hiện. Vui lòng chờ hoàn thành các action hiện tại."));
-                else if(ErrorManager.HasFatalError) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1008", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi có lỗi nghiêm trọng. Vui lòng kiểm tra và xử lý lỗi."));
-                else if(NavigationManager.Driving) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1009", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi robot đang di chuyển. Vui lòng dừng robot."));
+                else if (PeripheralManager.PeripheralMode != PeripheralMode.AUTOMATIC) throw new OrderException(RobotErrors.Error1006(PeripheralManager.PeripheralMode));
+                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);
             }
             catch (OrderException orEx)
@@ -71,7 +87,7 @@ public class RobotController(IOrder OrderManager,
         }
     }
 
-    public void NewInstanceActionUpdated()
+    public void NewInstanceActionUpdated(InstantActionsMsg action)
     {
         if (NewInstanceMutex.WaitOne(2000))
         {

RobotApp/Services/Robot/RobotErrors.cs

@@ -1,5 +1,6 @@
-using RobotApp.VDA5050.State;
-using RobotApp.VDA5050.Type;
+using Microsoft.AspNetCore.Components;
+using RobotApp.Interfaces;
+using RobotApp.VDA5050.State;
 
 namespace RobotApp.Services.Robot;
 
@@ -38,4 +39,35 @@ public class RobotErrors
             ErrorReferences = []
         };
     }
+
+    public static Error Error1001(string oldOrderId, string newOrderId)
+        => CreateError(ErrorType.INITIALIZE_ORDER, "1001", ErrorLevel.WARNING, $"Có order đang được thực hiện. OrderId: {oldOrderId}, OrderId mới: {newOrderId}");
+    public static Error Error1002(int nodesLength)
+        => CreateError(ErrorType.INITIALIZE_ORDER, "1002", ErrorLevel.WARNING, $"Order Nodes không hợp lệ. Kích thước: {nodesLength}");
+    public static Error Error1003(int edgesLength)
+        => CreateError(ErrorType.INITIALIZE_ORDER, "1003", ErrorLevel.WARNING, $"Order Edges không hợp lệ. Kích thước: {edgesLength}");
+    public static Error Error1004(int nodesLength, int edgesLength)
+        => CreateError(ErrorType.INITIALIZE_ORDER, "1004", ErrorLevel.WARNING, $"Order không hợp lệ do kích thước giữa Nodes và Edges không phù hợp. Kích thước Edges: {edgesLength}, kích thước nodes: {nodesLength}");
+    public static Error Error1005()
+        => CreateError(ErrorType.INITIALIZE_ORDER, "1005", ErrorLevel.WARNING, $"Không có order đang được thực hiện.");
+    public static Error Error1006(PeripheralMode peripheralMode)
+        => CreateError(ErrorType.INITIALIZE_ORDER, "1006", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi chế độ vận hành không phải là TỰ ĐỘNG. Chế độ hiện tại: {peripheralMode}");
+    public static Error Error1007()
+        => CreateError(ErrorType.INITIALIZE_ORDER, "1007", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi có action đang thực hiện. Vui lòng chờ hoàn thành các action hiện tại.");
+    public static Error Error1008()
+        => CreateError(ErrorType.INITIALIZE_ORDER, "1008", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi có lỗi nghiêm trọng. Vui lòng kiểm tra và xử lý lỗi.");
+    public static Error Error1009()
+        => CreateError(ErrorType.INITIALIZE_ORDER, "1009", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi robot đang di chuyển. Vui lòng dừng robot.");
+    public static Error Error1010(string nodeId, int sequenceId, int correctIndex)
+        => CreateError(ErrorType.INITIALIZE_ORDER, "1010", ErrorLevel.WARNING, $"Order Nodes không đúng thứ tự. NodeId: {nodeId}, SequenceId: {sequenceId}, Vị trí đúng: {correctIndex}");
+    public static Error Error1011(string edgeId, int sequenceId, int correctIndex)
+        => CreateError(ErrorType.INITIALIZE_ORDER, "1011", ErrorLevel.WARNING, $"Order Edges không đúng thứ tự. EdgeId: {edgeId}, SequenceId: {sequenceId}, Vị trí đúng: {correctIndex}");
+    public static Error Error1012(NavigationState state)
+        => CreateError(ErrorType.INITIALIZE_ORDER, "1012", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi hệ thống điều hướng không ở trạng thái sẵn sàng. Trạng thái hiện tại: {state}");
+    public static Error Error1013()
+        => new();
+    public static Error Error1014(string edgeId, string nodeId)
+        => CreateError(ErrorType.INITIALIZE_ORDER, "1014", ErrorLevel.WARNING, $"Edge {edgeId} chứa StartNodeId {nodeId} không tồn tại trong Nodes");
+    public static Error Error1015(string edgeId, string nodeId)
+        => CreateError(ErrorType.INITIALIZE_ORDER, "1015", ErrorLevel.WARNING, $"Edge {edgeId} chứa {nodeId} không tồn tại trong Nodes");
 }

RobotApp/Services/Robot/RobotLoads.cs

@@ -1,6 +1,5 @@
-namespace RobotApp.Services.Robot
-{
+namespace RobotApp.Services.Robot;
+
 public class RobotLoads 
 {
 }
-}

RobotApp/Services/Robot/RobotLocalization.cs

@@ -0,0 +1,83 @@
+using RobotApp.Common.Shares;
+using RobotApp.Interfaces;
+using RobotApp.Services.Robot.Simulation;
+
+namespace RobotApp.Services.Robot;
+
+public class Xloc
+{
+    public double X { get; set; }
+    public double Y { get; set; }
+    public double Theta { get; set; }
+    public string SlamState { get; set; } = string.Empty;
+    public string SlamStateDetail { get; set; } = string.Empty;
+    public string CurrentActiveMap { get; set; } = string.Empty;
+    public double Reliability { get; set; }
+    public double MatchingScore { get; set; }
+    public bool IsReady { get; set; }
+}
+
+public class RobotLocalization(IConfiguration Configuration, SimulationVisualization SimVisualization) : ILocalization
+{
+    public double X => IsSimulation ? SimVisualization.X : Xloc.X;
+
+    public double Y => IsSimulation ? SimVisualization.Y : Xloc.Y;
+
+    public double Theta => IsSimulation ? SimVisualization.Theta * Math.PI / 180 : Xloc.Theta;
+
+    public string SlamState => IsSimulation ? "Localization" : Xloc.SlamState;
+
+    public string SlamStateDetail => IsSimulation ? "" : Xloc.SlamStateDetail;
+
+    public string CurrentActiveMap => IsSimulation ? "" : Xloc.CurrentActiveMap;
+
+    public double Reliability => IsSimulation ? 100 : Xloc.Reliability;
+
+    public double MatchingScore => IsSimulation ? 100 : Xloc.MatchingScore;
+
+    public bool IsReady => IsSimulation || Xloc.IsReady;
+
+
+    private readonly Xloc Xloc = new();
+    private readonly bool IsSimulation = Configuration.GetValue<bool>("Robot:Simulation", false);
+
+    public MessageResult ActivateMap(string mapName)
+    {
+        throw new NotImplementedException();
+    }
+
+    public MessageResult ChangeMapOrigin(double x, double y, double theta)
+    {
+        throw new NotImplementedException();
+    }
+
+    public MessageResult SetInitializePosition(double x, double y, double theta)
+    {
+        throw new NotImplementedException();
+    }
+
+    public MessageResult StartLocalization()
+    {
+        throw new NotImplementedException();
+    }
+
+    public MessageResult StartMapping(double resolution)
+    {
+        throw new NotImplementedException();
+    }
+
+    public MessageResult StopLocalization()
+    {
+        throw new NotImplementedException();
+    }
+
+    public MessageResult StopMapping(string mapName)
+    {
+        throw new NotImplementedException();
+    }
+
+    public MessageResult SwitchMap(string mapName, double x, double y, double theta)
+    {
+        throw new NotImplementedException();
+    }
+}

RobotApp/Services/Robot/RobotNavigation.cs

@@ -0,0 +1,71 @@
+using RobotApp.Interfaces;
+using RobotApp.Services.Robot.Simulation;
+using RobotApp.Services.Robot.Simulation.Navigation;
+using RobotApp.VDA5050.Order;
+
+namespace RobotApp.Services.Robot;
+
+public class RobotNavigation(SimulationModel SimModel) : 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;
+
+
+    private SimulationNavigation? SimNavigation;
+    private bool IsSimulation => SimModel.Enable;
+
+
+    public void CancelMovement()
+    {
+        throw new NotImplementedException();
+    }
+
+    public void Move(Node[] nodes, Edge[] edges)
+    {
+        throw new NotImplementedException();
+    }
+
+    public void MoveStraight(double x, double y)
+    {
+        throw new NotImplementedException();
+    }
+
+    public void Paused()
+    {
+        throw new NotImplementedException();
+    }
+
+    public void Resume()
+    {
+        throw new NotImplementedException();
+    }
+
+    public void Rotate(double angle)
+    {
+        throw new NotImplementedException();
+    }
+
+    public void UpdateOrder(int lastBaseSequence)
+    {
+        throw new NotImplementedException();
+    }
+
+    public void RefreshOrder(Node[] nodes, Edge[] edges)
+    {
+        throw new NotImplementedException();
+    }
+
+    public void UpdateOrder(string lastBaseNodeId)
+    {
+        throw new NotImplementedException();
+    }
+
+    public void Refresh()
+    {
+        throw new NotImplementedException();
+    }
+}

RobotApp/Services/Robot/RobotOrderController.cs

@@ -6,7 +6,7 @@ using Action = RobotApp.VDA5050.InstantAction.Action;
 
 namespace RobotApp.Services.Robot;
 
-public class RobotOrderController(INavigation NavigationManager, IInstanceActions ActionManager, IError ErrorManager, Logger<RobotOrderController> Logger) : IOrder
+public class RobotOrderController(INavigation NavigationManager, IInstantActions ActionManager, IError ErrorManager, Logger<RobotOrderController> Logger) : IOrder
 {
     public string OrderId { get; private set; } = string.Empty;
     public int OrderUpdateId { get; private set; }
@@ -22,17 +22,18 @@ public class RobotOrderController(INavigation NavigationManager, IInstanceAction
     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.CreateError(ErrorType.INITIALIZE_ORDER, "1001", ErrorLevel.WARNING, $"Có order đang được thực hiện. OrderId: {OrderId}, OrderId mới: {orderId}"));
-                if (nodes.Length < 2) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1002", ErrorLevel.WARNING, $"Order Nodes không hợp lệ. Kích thước: {nodes.Length}"));
-                if (edges.Length < 1) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1003", ErrorLevel.WARNING, $"Order Edges không hợp lệ. Kích thước: {edges.Length}"));
-                if (edges.Length != nodes.Length - 1) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1004", ErrorLevel.WARNING, $"Order không hợp lệ do kích thước giữa Nodes và Edges không phù hợp. Kích thước Edges: {edges.Length}, kích thước nodes: {nodes.Length}"));
-                if (NavigationManager.State != NavigationState.Idle) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1012", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi hệ thống điều hướng không ở trạng thái sẵn sàng. Trạng thái hiện tại: {NavigationManager.State}"));
+                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++)
                 {
@@ -52,12 +53,12 @@ public class RobotOrderController(INavigation NavigationManager, IInstanceAction
                         }
                         OrderActions.TryAdd(nodes[i].NodeId, edges[i].Actions);
                     }
-                    if (nodes[i].SequenceId != i) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1010", ErrorLevel.WARNING, $"Order Nodes không đúng thứ tự. NodeId: {nodes[i].NodeId}, SequenceId: {nodes[i].SequenceId}, Vị trí đúng: {i}"));
-                    if (i < nodes.Length - 1 &&  edges[i].SequenceId != i) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1011", ErrorLevel.WARNING, $"Order Edges không đúng thứ tự. EdgeId: {edges[i].EdgeId}, SequenceId: {edges[i].SequenceId}, Vị trí đúng: {i}"));
+                    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
+                NodeStates = [.. nodes.Select(n => new NodeState
                 {
                     NodeId = n.NodeId,
                     Released = n.Released,
@@ -70,15 +71,15 @@ public class RobotOrderController(INavigation NavigationManager, IInstanceAction
                         Theta = n.NodePosition.Theta,
                         MapId = n.NodePosition.MapId
                     }
-                }).ToArray();
-                EdgeStates = edges.Select(e => new EdgeState
+                })];
+                EdgeStates = [.. edges.Select(e => new EdgeState
                 {
                     EdgeId = e.EdgeId,
                     Released = e.Released,
                     EdgeDescription = e.EdgeDescription,
                     SequenceId = e.SequenceId,
                     Trajectory = e.Trajectory
-                }).ToArray();
+                })];
 
                 OrderId = orderId;
                 ActionManager.AddOrderActions([.. OrderActions.Values.SelectMany(a => a)]);
@@ -111,7 +112,7 @@ public class RobotOrderController(INavigation NavigationManager, IInstanceAction
         {
             try
             {
-                if (string.IsNullOrEmpty(OrderId)) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1005", ErrorLevel.WARNING, $"Không có order đang được thực hiện."));
+                if (string.IsNullOrEmpty(OrderId)) throw new OrderException(RobotErrors.Error1005());
                 if (orderUpdateId > OrderUpdateId)
                 {
                     OrderUpdateId = orderUpdateId;

RobotApp/Services/Robot/RobotPathPlanner.cs

@@ -2,7 +2,7 @@
 using RobotApp.Common.Shares.Enums;
 using RobotApp.Common.Shares.Model;
 using RobotApp.Services.Exceptions;
-using RobotApp.Services.Robot.Navigation;
+using RobotApp.Services.Robot.Simulation.Navigation;
 using RobotApp.VDA5050.Order;
 using RobotApp.VDA5050.State;
 
@@ -14,18 +14,18 @@ public class RobotPathPlanner(IConfiguration Configuration)
 
     public NavigationNode[] GetNavigationNode(double currentTheta, Node[] nodes, Edge[] edges)
     {
-        if (nodes.Length < 2) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1002", ErrorLevel.WARNING, $"Order Nodes không hợp lệ. Kích thước: {nodes.Length}"));
-        if (edges.Length < 1) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1003", ErrorLevel.WARNING, $"Order Edges không hợp lệ. Kích thước: {edges.Length}"));
-        if (edges.Length != nodes.Length - 1) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1004", ErrorLevel.WARNING, $"Order không hợp lệ do kích thước giữa Nodes và Edges không phù hợp. Kích thước Edges: {edges.Length}, kích thước nodes: {nodes.Length}"));
+        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));
 
         return [];
     }
 
     public NavigationNode[] PathSplit(NavigationNode[] nodes, Edge[] edges)
     {
-        if (nodes.Length < 2) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1002", ErrorLevel.WARNING, $"Order Nodes không hợp lệ. Kích thước: {nodes.Length}"));
-        if (edges.Length < 1) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1003", ErrorLevel.WARNING, $"Order Edges không hợp lệ. Kích thước: {edges.Length}"));
-        if (edges.Length != nodes.Length - 1) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1004", ErrorLevel.WARNING, $"Order không hợp lệ do kích thước giữa Nodes và Edges không phù hợp. Kích thước Edges: {edges.Length}, kích thước nodes: {nodes.Length}"));
+        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));
 
         List <NavigationNode> navigationNode = [new()
         {
@@ -40,8 +40,8 @@ public class RobotPathPlanner(IConfiguration Configuration)
         {
             var startNode = nodes.FirstOrDefault(n => n.NodeId == edge.StartNodeId);
             var endNode = nodes.FirstOrDefault(n => n.NodeId == edge.EndNodeId);
-            if (startNode is null) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1014", ErrorLevel.WARNING, $"Edge chứa StartNodeId không tồn tại trong Nodes . StartNodeId: {edge.StartNodeId}"));
-            if (endNode is null) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1015", ErrorLevel.WARNING, $"Edge chứa EndNodeId không tồn tại trong Nodes . EndNodeId: {edge.EndNodeId}"));
+            if (startNode is null) throw new PathPlannerException(RobotErrors.Error1014(edge.EdgeId, edge.StartNodeId));
+            if (endNode is null) throw new PathPlannerException(RobotErrors.Error1014(edge.EdgeId, edge.EndNodeId));
 
             var EdgeCalculatorModel = new EdgeCalculatorModel()
             {

RobotApp/Services/Robot/RobotPeripheral.cs

@@ -0,0 +1,77 @@
+using RobotApp.Interfaces;
+
+namespace RobotApp.Services.Robot;
+
+public class RobotPeripheral : BackgroundService, IPeripheral, ISafety
+{
+    public PeripheralMode PeripheralMode { get; private set; } = PeripheralMode.SERVICE;
+    public bool IsReady { get; private set; } = true;
+    public bool Emergency => throw new NotImplementedException();
+    public bool Bumper => throw new NotImplementedException();
+    public bool LiftedUp => throw new NotImplementedException();
+    public bool LiftedDown => throw new NotImplementedException();
+    public bool LiftHome => throw new NotImplementedException();
+    public bool LidarFrontProtectField => throw new NotImplementedException();
+    public bool LidarBackProtectField => throw new NotImplementedException();
+    public bool LidarFrontTimProtectField => throw new NotImplementedException();
+    public bool LeftMotorReady => throw new NotImplementedException();
+    public bool RightMotorReady => throw new NotImplementedException();
+    public bool LiftMotorReady => throw new NotImplementedException();
+    public bool SwitchLock => throw new NotImplementedException();
+    public bool SwitchManual => throw new NotImplementedException();
+    public bool SwitchAuto => throw new NotImplementedException();
+    public bool ButtonStart => throw new NotImplementedException();
+    public bool ButtonStop => throw new NotImplementedException();
+    public bool ButtonReset => throw new NotImplementedException();
+    public bool HasLoad => throw new NotImplementedException();
+    public bool MutedBase => throw new NotImplementedException();
+    public bool MutedLoad => throw new NotImplementedException();
+    public bool StartedCharging => throw new NotImplementedException();
+    public bool SpeedVerySlow => throw new NotImplementedException();
+    public bool SpeedSlow => throw new NotImplementedException();
+    public bool SpeedNormal => throw new NotImplementedException();
+    public bool SpeedMedium => throw new NotImplementedException();
+    public bool SpeedOptimal => throw new NotImplementedException();
+    public bool SpeedFast => throw new NotImplementedException();
+    public bool SpeedVeryFast => throw new NotImplementedException();
+
+    public bool SetHorizontalLoad()
+    {
+        throw new NotImplementedException();
+    }
+
+    public bool SetMutedBase(bool muted)
+    {
+        throw new NotImplementedException();
+    }
+
+    public bool SetMutedLoad(bool muted)
+    {
+        throw new NotImplementedException();
+    }
+
+    public bool SetOnCharging()
+    {
+        throw new NotImplementedException();
+    }
+
+    public bool SetProccessState()
+    {
+        throw new NotImplementedException();
+    }
+
+    public bool SetSytemState()
+    {
+        throw new NotImplementedException();
+    }
+
+    protected override Task ExecuteAsync(CancellationToken stoppingToken)
+    {
+        throw new NotImplementedException();
+    }
+
+    public override Task StopAsync(CancellationToken cancellationToken)
+    {
+        return base.StopAsync(cancellationToken);
+    }
+}

RobotApp/Services/Robot/RobotSafety.cs

@@ -1,6 +0,0 @@
-namespace RobotApp.Services.Robot
-{
-    public class RobotSafety
-    {
-    }
-}

RobotApp/Services/Robot/RobotVisualization.cs

@@ -1,6 +1,34 @@
-namespace RobotApp.Services.Robot
+using RobotApp.Interfaces;
+using RobotApp.VDA5050.Visualization;
+
+namespace RobotApp.Services.Robot;
+
+public class RobotVisualization(ILocalization Localization)
 {
-    public class RobotVisualization
+    public VisualizationMsg Visualization => GetVisualizationMsg();
+    public string SerialNumber { get; set; } = string.Empty;
+
+    private uint HeaderId;
+    private VisualizationMsg GetVisualizationMsg()
     {
+        return new VisualizationMsg()
+        {
+            HeaderId = HeaderId++,
+            SerialNumber = SerialNumber,
+            MapId = Localization.CurrentActiveMap,
+            MapDescription = string.Empty,
+            AgvPosition = new AgvPosition()
+            {
+                X = Localization.X,
+                Y = Localization.Y,
+                Theta = Localization.Theta
+            },
+            Velocity = new Velocity()
+            {
+                Vx = 0,
+                Vy = 0,
+                Omega = 0   
+            }
+        };
     }
 }

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

@@ -1,4 +1,4 @@
-namespace RobotApp.Services.Robot.Navigation.Algorithm;
+namespace RobotApp.Services.Robot.Simulation.Navigation.Algorithm;
 
 public class FuzzyLogic
 {

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

@@ -1,4 +1,4 @@
-namespace RobotApp.Services.Robot.Navigation.Algorithm;
+namespace RobotApp.Services.Robot.Simulation.Navigation.Algorithm;
 
 public class PID
 {
@@ -29,13 +29,12 @@ public class PID
         return this;
     }
 
-    public double PID_step(double error, double min, double max)
+    public double PID_step(double error, double min, double max, double timeSample)
     {
         DateTime CurrentTime = DateTime.Now;
-        double TimeSample = (CurrentTime - PreTime).TotalSeconds;
         double P_part = Kp * (error - Pre_Error);
-        double I_part = 0.5 * Ki * TimeSample * (error + Pre_Error);
-        double D_part = Kd / TimeSample * (error - 2 * Pre_Error + Pre_Pre_Error);
+        double I_part = 0.5 * Ki * timeSample * (error + Pre_Error);
+        double D_part = Kd / timeSample * (error - 2 * Pre_Error + Pre_Pre_Error);
         double Out = Pre_Out + P_part + I_part + D_part;
         Pre_Pre_Error = Pre_Error;
         Pre_Error = error;

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

@@ -0,0 +1,158 @@
+using RobotApp.Common.Shares.Enums;
+
+namespace RobotApp.Services.Robot.Simulation.Navigation.Algorithm;
+public class PurePursuit
+{
+    private double MaxAngularVelocity = 1.5;
+    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)
+    {
+        LookaheadDistance = distance;
+        return this;
+    }
+
+    public PurePursuit WithMaxAngularVelocity(double vel)
+    {
+        MaxAngularVelocity = vel;
+        return this;
+    }
+
+    public PurePursuit WithPath(NavigationNode[] path)
+    {
+        Waypoints_Value = [.. path];
+        return this;
+    }
+
+    public void UpdatePath(NavigationNode[] path)
+    {
+        Waypoints_Value = [.. path];
+    }
+
+    public void UpdateGoal(NavigationNode goal)
+    {
+        Goal = goal;
+        GoalIndex = Waypoints_Value.IndexOf(Goal);
+    }
+
+    public (NavigationNode node, int index) GetOnNode(double x, double y)
+    {
+        double minDistance = double.MaxValue;
+        NavigationNode onNode = Waypoints_Value[0];
+        int index = 0;
+        for (int i = 1; i < Waypoints_Value.Count; i++)
+        {
+            var distance = Math.Sqrt(Math.Pow(x - Waypoints_Value[i].X, 2) + Math.Pow(y - Waypoints_Value[i].Y, 2));
+            if (distance < minDistance)
+            {
+                onNode = Waypoints_Value[i];
+                minDistance = distance;
+                index = i;
+            }
+        }
+        return (onNode, index);
+    }
+
+    private (NavigationNode? node, int index) OnNode(double x, double y)
+    {
+        if (Waypoints_Value is null || Waypoints_Value.Count == 0) return (null, 0);
+        double minDistance = double.MaxValue;
+        int index = 0;
+        NavigationNode? onNode = null;
+        if (Goal is null) return (onNode, index);
+        for (int i = OnNodeIndex; i < Waypoints_Value.IndexOf(Goal); i++)
+        {
+            var distance = Math.Sqrt(Math.Pow(x - Waypoints_Value[i].X, 2) + Math.Pow(y - Waypoints_Value[i].Y, 2));
+            if (distance < minDistance)
+            {
+                onNode = Waypoints_Value[i];
+                minDistance = distance;
+                index = i;
+            }
+        }
+        return (onNode, index);
+    }
+
+    public double PurePursuit_step(double X_Ref, double Y_Ref, double Angle_Ref)
+    {
+        if (Waypoints_Value is null || Waypoints_Value.Count < 2) return 0;
+        NavigationNode? lookaheadStartPt = null;
+        var (onNode, index) = OnNode(X_Ref, Y_Ref);
+        if (onNode is null || Goal is null) return 0;
+        OnNodeIndex = index;
+        double lookDistance = 0;
+        for (int i = OnNodeIndex + 1; i < Waypoints_Value.IndexOf(Goal); i++)
+        {
+            lookDistance += Math.Sqrt(Math.Pow(Waypoints_Value[i - 1].X - Waypoints_Value[i].X, 2) + Math.Pow(Waypoints_Value[i - 1].Y - Waypoints_Value[i].Y, 2));
+            if (lookDistance >= LookaheadDistance || Waypoints_Value[i].Direction != onNode.Direction)
+            {
+                lookaheadStartPt = Waypoints_Value[i];
+                break;
+            }
+        }
+        lookaheadStartPt ??= Goal;
+        if (onNode.Direction == RobotDirection.BACKWARD)
+        {
+            if (Angle_Ref > Math.PI) Angle_Ref -= Math.PI * 2;
+            else if (Angle_Ref < -Math.PI) Angle_Ref += Math.PI * 2;
+            Angle_Ref += Math.PI;
+            if (Angle_Ref > Math.PI) Angle_Ref -= Math.PI * 2;
+        }
+        var distance = Math.Atan2(lookaheadStartPt.Y - Y_Ref, lookaheadStartPt.X - X_Ref) - Angle_Ref;
+
+        if (Math.Abs(distance) > Math.PI)
+        {
+            double minDistance;
+            if (distance + Math.PI == 0.0) minDistance = 0.0;
+            else
+            {
+                double data = (distance + Math.PI) / (2 * Math.PI);
+                if (data < 0) data = Math.Round(data + 0.5);
+                else data = Math.Round(data - 0.5);
+                minDistance = distance + Math.PI - data * (2 * Math.PI);
+                double checker = 0;
+                if (minDistance != 0.0)
+                {
+                    checker = Math.Abs((distance + Math.PI) / (2 * Math.PI));
+                }
+                if (!(Math.Abs(checker - Math.Floor(checker + 0.5)) > 2.2204460492503131E-16 * checker))
+                {
+                    minDistance = 0.0;
+                }
+                else if (distance + Math.PI < 0.0)
+                {
+                    minDistance += Math.PI * 2;
+                }
+            }
+            if (minDistance == 0.0 && distance + Math.PI > 0.0)
+            {
+                minDistance = Math.PI * 2;
+            }
+            distance = minDistance - Math.PI;
+        }
+
+        var AngularVelocity = 2.0 * 0.5 * Math.Sin(distance) / LookaheadDistance;
+        if (Math.Abs(AngularVelocity) > MaxAngularVelocity)
+        {
+            if (AngularVelocity < 0.0)
+            {
+                AngularVelocity = -1.0;
+            }
+            else if (AngularVelocity > 0.0)
+            {
+                AngularVelocity = 1.0;
+            }
+            else if (AngularVelocity == 0.0)
+            {
+                AngularVelocity = 0.0;
+            }
+            AngularVelocity *= MaxAngularVelocity;
+        }
+        return AngularVelocity;
+    }
+
+}

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

@@ -0,0 +1,78 @@
+using RobotApp.Common.Shares.Enums;
+using RobotApp.Interfaces;
+
+namespace RobotApp.Services.Robot.Simulation.Navigation;
+
+public class DifferentialNavigation : SimulationNavigation
+{
+    private readonly Logger<DifferentialNavigation> Logger;
+
+    public DifferentialNavigation(IServiceProvider ServiceProvider) : base(ServiceProvider)
+    {
+        using var scope = ServiceProvider.CreateScope();
+        Logger = scope.ServiceProvider.GetRequiredService<Logger<DifferentialNavigation>>();
+    }
+
+    protected override void NavigationHandler()
+    {
+        try
+        {
+            if (NavState == NavigationState.Rotating)
+            {
+                if (RotatePID is not null)
+                {
+                    double Error = Visualization.Theta - TargetAngle;
+                    if (Error > 180) Error -= 360;
+                    else if (Error < -180) Error += 360;
+                    if (Math.Abs(Error) < 1)
+                    {
+                        if (NavigationPath is not null && NavigationPath.Length > 2) NavState = NavigationState.Moving;
+                        else Dispose();
+                    }
+                    else
+                    {
+                        var SpeedCal = RotatePID.PID_step(Error * Math.PI / 180, AngularVelocity, -AngularVelocity, CycleHandlerMilliseconds / 1000.0);
+                        VelocityController.SetSpeed(SpeedCal, SpeedCal, CycleHandlerMilliseconds / 1000.0);
+                    }
+                }
+            }
+            else if (NavState == NavigationState.Moving)
+            {
+                if (NavigationPath is not null && SimulationOrderNodes.Length > 1 && CurrentBaseNode is not null)
+                {
+                    if (MovePID is not null && MoveFuzzy is not null && MovePurePursuit is not null)
+                    {
+                        var DistanceToGoal = Math.Sqrt(Math.Pow(Visualization.X - SimulationOrderNodes[^1].X, 2) + Math.Pow(Visualization.Y - SimulationOrderNodes[^1].Y, 2));
+                        var DistanceToCheckingNode = Math.Sqrt(Math.Pow(Visualization.X - CurrentBaseNode.X, 2) + Math.Pow(Visualization.Y - CurrentBaseNode.Y, 2));
+                        var deviation = CurrentBaseNode.NodeId == SimulationOrderNodes[^1].NodeId ? 0.02 : 0.1;
+                        if (DistanceToCheckingNode > deviation)
+                        {
+                            double SpeedTarget = MovePID.PID_step(DistanceToCheckingNode, SimulationModel.MaxVelocity, 0, CycleHandlerMilliseconds / 1000.0);
+                            double AngularVel = MovePurePursuit.PurePursuit_step(Visualization.X, Visualization.Y, Visualization.Theta * Math.PI / 180);
+                            AngularVel *= NavigationPath[MovePurePursuit.OnNodeIndex].Direction == RobotDirection.FORWARD ? 1 : -1;
+                            (double AngularVelocityLeft, double AngularVelocityRight) = MoveFuzzy.Fuzzy_step(SpeedTarget, AngularVel, CycleHandlerMilliseconds / 1000.0);
+
+                            if (NavigationPath[MovePurePursuit.OnNodeIndex].Direction == RobotDirection.FORWARD)
+                            {
+                                AngularVelocityLeft /= SimulationModel.RadiusWheel;
+                                AngularVelocityRight = AngularVelocityRight / SimulationModel.RadiusWheel * -1;
+                            }
+                            else
+                            {
+                                AngularVelocityLeft = AngularVelocityLeft / SimulationModel.RadiusWheel * -1;
+                                AngularVelocityRight /= SimulationModel.RadiusWheel;
+                            }
+                            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);
+        }
+    }
+}

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

@@ -0,0 +1,23 @@
+namespace RobotApp.Services.Robot.Simulation.Navigation;
+
+public class ForkliftNavigation : SimulationNavigation
+{
+    private readonly Logger<ForkliftNavigation> Logger;
+    public ForkliftNavigation(IServiceProvider ServiceProvider) : base(ServiceProvider)
+    {
+        using var scope = ServiceProvider.CreateScope();
+        Logger = scope.ServiceProvider.GetRequiredService<Logger<ForkliftNavigation>>();
+    }
+
+    protected override void NavigationHandler()
+    {
+        try
+        {
+            // Implement differential drive navigation logic here
+        }
+        catch (Exception ex)
+        {
+            Logger.Write($"Error in ForkliftNavigationSevice: {ex.Message}", LogLevel.Error);
+        }
+    }
+}

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

@@ -1,6 +1,6 @@
 using RobotApp.Common.Shares.Enums;
 
-namespace RobotApp.Services.Robot.Navigation;
+namespace RobotApp.Services.Robot.Simulation.Navigation;
 
 public class NavigationNode
 {
@@ -9,5 +9,7 @@ public class NavigationNode
     public double Y { get; set; }
     public double Theta { get; set; }
     public RobotDirection Direction { get; set; }
+    public double AllowedDeviationXY { get; set; }
+    public double AllowedDeviationTheta { get; set; }
     public string Description { get; set; } = string.Empty;
 }

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

@@ -0,0 +1,192 @@
+using Microsoft.AspNetCore.Components;
+using RobotApp.Common.Shares;
+using RobotApp.Common.Shares.Enums;
+using RobotApp.Interfaces;
+using RobotApp.Services.Exceptions;
+using RobotApp.Services.Robot.Simulation.Navigation.Algorithm;
+using RobotApp.VDA5050.Order;
+using RobotApp.VDA5050.State;
+using System.IO;
+
+namespace RobotApp.Services.Robot.Simulation.Navigation;
+
+public class SimulationNavigation : INavigation, IDisposable
+{
+    public NavigationState State => NavState;
+    public bool Driving => NavDriving;
+    public bool IsReady => true;
+    public double VelocityX => Visualization.Vx;
+    public double VelocityY => Visualization.Vy;
+    public double Omega => Visualization.Omega;
+
+    protected NavigationState NavState = NavigationState.None;
+    protected bool NavDriving = false;
+
+    protected const int CycleHandlerMilliseconds = 50;
+    private WatchTimer<SimulationNavigation>? NavigationTimer;
+
+    protected double TargetAngle = 0;
+    protected PID? RotatePID;
+    protected readonly double AngularVelocity;
+
+    protected PID? MovePID;
+    protected FuzzyLogic? MoveFuzzy;
+    protected PurePursuit? MovePurePursuit;
+
+    protected Node[] OrderNodes = [];
+    protected Edge[] OrderEdges = [];
+    protected NavigationNode[] SimulationOrderNodes = [];
+
+    protected readonly SimulationVisualization Visualization;
+    protected readonly SimulationVelocity VelocityController;
+    protected readonly SimulationModel SimulationModel;
+    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)
+    {
+        using var scope = ServiceProvider.CreateScope();
+        Logger = scope.ServiceProvider.GetRequiredService<Logger<SimulationNavigation>>();
+        Visualization = scope.ServiceProvider.GetRequiredService<SimulationVisualization>();
+        SimulationModel = scope.ServiceProvider.GetRequiredService<SimulationModel>();
+        PathPlanner = scope.ServiceProvider.GetRequiredService<RobotPathPlanner>();
+        VelocityController = new(Visualization, SimulationModel);
+    }
+
+    protected void HandleNavigationStart()
+    {
+        NavigationTimer = new(CycleHandlerMilliseconds, NavigationHandler, Logger);
+        NavigationTimer.Start();
+    }
+
+    protected void HandleNavigationStop()
+    {
+        NavigationTimer?.Dispose();
+    }
+
+    protected virtual void NavigationHandler() { }
+
+    public void CancelMovement()
+    {
+        Dispose();
+        NavState = NavigationState.Canceled;
+    }
+
+    public void Move(Node[] nodes, Edge[] edges)
+    {
+        if (!IsIdle) throw new SimulationException(RobotErrors.Error1012(NavState));
+        NavState = NavigationState.Initializing;
+        if (nodes.Length < 2) throw new SimulationException(RobotErrors.Error1002(nodes.Length));
+        if (edges.Length < 1) throw new SimulationException(RobotErrors.Error1003(edges.Length));
+        if (edges.Length != nodes.Length - 1) throw new SimulationException(RobotErrors.Error1004(nodes.Length, edges.Length));
+
+        var pathDirection = PathPlanner.GetNavigationNode(Visualization.Theta, nodes, edges);
+        NavigationPath = PathPlanner.PathSplit(pathDirection, edges);
+
+        MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
+        MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
+        MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath([.. NavigationPath]);
+
+        double angleFoward = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X) * 180 / Math.PI;
+        double angleBacward = Math.Atan2(NavigationPath[0].Y - NavigationPath[1].Y, NavigationPath[0].X - NavigationPath[1].X) * 180 / Math.PI;
+        OrderNodes = nodes;
+        OrderEdges = edges;
+        SimulationOrderNodes = pathDirection;
+        Rotate(SimulationOrderNodes[0].Direction == RobotDirection.FORWARD ? angleFoward : angleBacward);
+    }
+
+    public void MoveStraight(double x, double y)
+    {
+        if (!IsIdle) throw new SimulationException(RobotErrors.Error1012(NavState));
+        var headRobotNode = new NavigationNode()
+        {
+            X = Visualization.X * Math.Acos(Visualization.Theta * Math.PI / 180),
+            Y = Visualization.Y * Math.Asin(Visualization.Theta * Math.PI / 180),
+        };
+        var goalNode = new NavigationNode()
+        {
+            NodeId = "RobotGoal",
+            X = x,
+            Y = y,
+        };
+        var currentRobotNode = new NavigationNode()
+        {
+            NodeId = "RobotCurrentNode",
+            X = Visualization.X,
+            Y = Visualization.Y,
+        }; 
+        goalNode.Theta = MathExtensions.GetVectorAngle(currentRobotNode.X, currentRobotNode.Y, headRobotNode.X, headRobotNode.Y, goalNode.X, goalNode.Y) > 90 ?
+                            Math.Atan2(currentRobotNode.Y - goalNode.Y, currentRobotNode.X - goalNode.X) :
+                            Math.Atan2(goalNode.Y - currentRobotNode.Y, goalNode.X - currentRobotNode.X);
+        currentRobotNode.Theta = goalNode.Theta;
+        NavigationPath = PathPlanner.PathSplit([currentRobotNode, goalNode], [new Edge()
+        {
+            EdgeId = "Straight edge",
+            Trajectory = new Trajectory()
+            {
+                Degree = 1,
+                ControlPoints = []
+            },
+            StartNodeId = currentRobotNode.NodeId,
+            EndNodeId = goalNode.NodeId,
+        }]);
+        MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
+        MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
+        MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath(NavigationPath);
+        double Angle = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X);
+        Rotate(Angle * 180 / Math.PI);
+        UpdateOrder(goalNode.NodeId);
+    }
+
+    public void Paused()
+    {
+        ResumeState = State;
+        NavState = NavigationState.Paused;
+    }
+
+    public void Resume()
+    {
+        NavState = ResumeState;
+        ResumeState = NavigationState.None;
+    }
+
+    public void Rotate(double angle)
+    {
+        if (!IsIdle) throw new SimulationException(RobotErrors.Error1012(NavState));
+        RotatePID = new PID().WithKp(10).WithKi(0.01).WithKd(0.1);
+        TargetAngle = MathExtensions.NormalizeAngle(angle);
+        NavState = NavigationState.Rotating;
+        HandleNavigationStart();
+    }
+
+    public void UpdateOrder(string lastBaseNodeId)
+    {
+    }
+
+    public void RefreshOrder(Node[] nodes, Edge[] edges)
+    {
+        throw new NotImplementedException();
+    }
+
+    public void Dispose()
+    {
+        HandleNavigationStop();
+        VelocityController.Stop();
+        OrderNodes = [];
+        OrderEdges = [];
+        SimulationOrderNodes = [];
+        NavDriving = false;
+        NavState = NavigationState.None;
+        GC.SuppressFinalize(this);
+    }
+
+    public void Refresh()
+    {
+        NavState = NavigationState.Idle;
+    }
+}

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

@@ -0,0 +1,12 @@
+using RobotApp.Common.Shares.Enums;
+
+namespace RobotApp.Services.Robot.Simulation.Navigation;
+
+public class SimulationNavigationManager
+{
+    public static SimulationNavigation GetNavigation(NavigationType type, IServiceProvider ServiceProvider)
+    {
+        if (type == NavigationType.Forklift) return new ForkliftNavigation(ServiceProvider);
+        return new DifferentialNavigation(ServiceProvider);
+    }
+}

RobotApp/Services/Robot/Simulation/SimulationExtensions.cs

@@ -0,0 +1,13 @@
+using System.Diagnostics.CodeAnalysis;
+
+namespace RobotApp.Services.Robot.Simulation;
+
+public static class SimulationExtensions
+{
+    public static IServiceCollection AddRobotSimulation(this IServiceCollection services)
+    {
+        services.AddSingleton<SimulationVisualization>();
+        services.AddSingleton<SimulationModel>();
+        return services;
+    }
+}

RobotApp/Services/Robot/Simulation/SimulationModel.cs

@@ -0,0 +1,16 @@
+using RobotApp.Common.Shares.Enums;
+
+namespace RobotApp.Services.Robot.Simulation;
+
+public class SimulationModel(IConfiguration Configuration)
+{
+    public readonly bool Enable = Configuration.GetValue("Simulation:Enable", false);
+    public readonly double RadiusWheel = Configuration.GetValue("Simulation:RadiusWheel", 0.1);
+    public readonly double Width = Configuration.GetValue("Simulation:Width", 0.606);
+    public readonly double Length = Configuration.GetValue("Simulation:Length", 1.106);
+    public readonly double MaxVelocity = Configuration.GetValue("Simulation:MaxVelocity", 1.5);
+    public readonly double MaxAngularVelocity = Configuration.GetValue("Simulation:MaxAngularVelocity", 0.3);
+    public readonly double Acceleration = Configuration.GetValue("Simulation:Acceleration", 2);
+    public readonly double Deceleration = Configuration.GetValue("Simulation:Deceleration", 1);
+    public readonly NavigationType NavigationType = Configuration.GetValue("Simulation:NavigationType", NavigationType.Differential);
+}

RobotApp/Services/Robot/Simulation/SimulationVelocity.cs

@@ -0,0 +1,56 @@
+namespace RobotApp.Services.Robot.Simulation;
+
+public class SimulationVelocity(SimulationVisualization Visualization, SimulationModel Model)
+{
+    private readonly double Acceleration = Model.Acceleration;
+    private readonly double Deceleration = Model.Deceleration;
+    private double AngularVelLeft;
+    private double AngularVelRight;
+
+    private (double angularVelLeft, double angularVelRight) AccelerationCalculator(double wL, double wR, double wL_Current, double wR_Current)
+    {
+        var angularVelLeft = wL_Current;
+        var angularVelRight = wR_Current;
+        if (wL_Current == 0 || wL / wL_Current < 0)
+        {
+            if (wL != 0) angularVelLeft += wL / Math.Abs(wL) * Acceleration;
+            else angularVelLeft = wL;
+        }
+        else
+        {
+            if (Math.Abs(wL) - Math.Abs(wL_Current) > Acceleration) angularVelLeft += Acceleration * wL_Current / Math.Abs(wL_Current);
+            else if (Math.Abs(wL_Current) - Math.Abs(wL) > Deceleration) angularVelLeft -= Deceleration * wL_Current / Math.Abs(wL_Current);
+            else angularVelLeft = wL;
+        }
+
+        if (wR_Current == 0 || wR / wR_Current < 0)
+        {
+            if (wR != 0) angularVelRight += wR / Math.Abs(wR) * Acceleration;
+            else angularVelRight = wR;
+        }
+        else
+        {
+            if (Math.Abs(wR) - Math.Abs(wR_Current) > Acceleration) angularVelRight += Acceleration * wR_Current / Math.Abs(wR_Current);
+            else if (Math.Abs(wR_Current) - Math.Abs(wR) > Deceleration) angularVelRight -= Deceleration * wR_Current / Math.Abs(wR_Current);
+            else angularVelRight = wR;
+        }
+
+        if (Math.Abs(angularVelLeft) > Math.Abs(wL)) angularVelLeft = wL;
+        if (Math.Abs(angularVelRight) > Math.Abs(wR)) angularVelRight = wR;
+        return (angularVelLeft, angularVelRight);
+    }
+
+    public bool SetSpeed(double angularVelLeft, double angularVelRight, double sampleTime)
+    {
+        (AngularVelLeft, AngularVelRight) = AccelerationCalculator(angularVelLeft, angularVelRight, AngularVelLeft, AngularVelRight);
+        //Console.WriteLine($"AngularVelLeft = {AngularVelLeft:0.####}, AngularVelRight = {AngularVelRight:0.####}");
+        _ = Visualization.UpdatePosition(AngularVelLeft, AngularVelRight, sampleTime);
+        return true;
+    }
+
+    public void Stop()
+    {
+        (AngularVelLeft, AngularVelRight) = (0, 0);
+        _ = Visualization.UpdatePosition(AngularVelLeft, AngularVelRight, 0.05);
+    }
+}

RobotApp/Services/Robot/Simulation/SimulationVisualization.cs

@@ -0,0 +1,39 @@
+namespace RobotApp.Services.Robot.Simulation;
+
+public class SimulationVisualization(SimulationModel Model)
+{
+    public double X { get; private set; }
+    public double Y { get; private set; }
+    public double Theta { get; private set; }
+    public double Vx { get; private set; }
+    public double Vy { get; private set; }
+    public double Omega { get; private set; }
+
+    private readonly double RadiusWheel = Model.RadiusWheel;
+    private readonly double RadiusRobot = Model.Width / 2;
+
+    public (double x, double y, double angle) UpdatePosition(double wL, double wR, double time)
+    {
+        Theta = (Theta + time * (-wR - wL) * RadiusWheel / RadiusRobot * 180 / Math.PI) % 360;
+        X += time * (-wR + wL) * RadiusWheel * Math.Cos(Theta * Math.PI / 180) / 2;
+        Y += time * (-wR + wL) * RadiusWheel * Math.Sin(Theta * Math.PI / 180) / 2;
+        _ = UpdateVelocity(wL, wR);
+        if (Theta > 180) Theta -= 360;
+        else if (Theta < -180) Theta += 360;
+        return (X, Y, Theta);
+    }
+
+    public (double vx, double vy, double omega) UpdateVelocity(double wL, double wR)
+    {
+        Vx = (-wR + wL) * RadiusWheel / 2;
+        Omega = (-wR - wL) * RadiusWheel / RadiusRobot;
+        return (Vx, 0, Omega);
+    }
+
+    public void LocalizationInitialize(double x, double y, double theta)
+    {
+        X = x;
+        Y = y;
+        Theta = theta;
+    }
+}

RobotApp/Services/RobotExtensions.cs

@@ -0,0 +1,39 @@
+using RobotApp.Interfaces;
+using RobotApp.Services.Robot;
+using RobotApp.Services.State;
+using System.Diagnostics.CodeAnalysis;
+
+namespace RobotApp.Services;
+
+public static class RobotExtensions
+{
+    public static IServiceCollection AddRobot(this IServiceCollection services)
+    {
+        services.AddSingleton<RobotStateMachine>();
+
+        services.AddInterfaceServiceSingleton<ILocalization, RobotLocalization>();
+        return services;
+    }
+
+    public static IServiceCollection AddInterfaceServiceSingleton<TService, [DynamicallyAccessedMembers(DynamicallyAccessedMemberTypes.PublicConstructors)] TImplementation>(this IServiceCollection services) where TService : class where TImplementation : class, TService
+    {
+        services.AddSingleton<TImplementation>();
+        services.AddSingleton<TService>(sp => sp.GetRequiredService<TImplementation>());
+        return services;
+    }
+
+    public static IServiceCollection AddHostedServiceSingleton<[DynamicallyAccessedMembers(DynamicallyAccessedMemberTypes.PublicConstructors)] THostedService>(this IServiceCollection services) where THostedService : class, IHostedService
+    {
+        services.AddSingleton<THostedService>();
+        services.AddHostedService(sp => sp.GetRequiredService<THostedService>());
+        return services;
+    }
+
+    public static IServiceCollection AddHostedInterfaceServiceSingleton<TService, [DynamicallyAccessedMembers(DynamicallyAccessedMemberTypes.PublicConstructors)] THostedService>(this IServiceCollection services) where TService : class where THostedService : class, IHostedService, TService
+    {
+        services.AddSingleton<THostedService>();
+        services.AddSingleton<TService>(sp => sp.GetRequiredService<THostedService>());
+        services.AddHostedService(sp => sp.GetRequiredService<THostedService>());
+        return services;
+    }
+}

RobotApp/Services/State/ACTState.cs

@@ -0,0 +1,7 @@
+using RobotApp.Common.Shares.Enums;
+
+namespace RobotApp.Services.State;
+
+public class ACTState<T>(ACTStateType state, IRobotState? superState = null) : RobotState<T>(state, superState) where T : Enum
+{ 
+}

RobotApp/Services/State/AutoState.cs

@@ -0,0 +1,7 @@
+using RobotApp.Common.Shares.Enums;
+
+namespace RobotApp.Services.State;
+
+public class AutoState<T>(AutoStateType state, IRobotState? superState = null) : RobotState<T>(state, superState) where T : Enum
+{
+}

RobotApp/Services/State/ExecutingState.cs

@@ -0,0 +1,7 @@
+using RobotApp.Common.Shares.Enums;
+
+namespace RobotApp.Services.State;
+
+public class ExecutingState<T>(ExecutingStateType state, IRobotState? superState = null) : RobotState<T>(state, superState) where T : Enum
+{
+}

RobotApp/Services/State/FaultState.cs

@@ -0,0 +1,7 @@
+using RobotApp.Common.Shares.Enums;
+
+namespace RobotApp.Services.State;
+
+public class FaultState<T>(FaultStateType state, IRobotState? superState = null) : RobotState<T>(state, superState) where T : Enum
+{
+}

RobotApp/Services/State/IRobotState.cs

@@ -0,0 +1,21 @@
+namespace RobotApp.Services.State;
+
+public interface IRobotState
+{
+    Enum Name { get; }
+    Type Type { get; }
+    IRobotState? SuperState { get; } 
+    List<IRobotState> SubStates { get; set; }
+    IRobotState? ActiveSubState { get;}
+    IRobotState? HistoryState { get;}
+    bool HasSubStates => SubStates.Count > 0;
+    bool IsActive => ActiveSubState != null;
+
+    event Action<IRobotState>? OnEnter;
+    event Action<IRobotState>? OnExit;
+    event Action<IRobotState , IRobotState>? OnTransition;
+    void Enter();
+    void Exit();
+    bool CanTransitionTo(IRobotState targetState);
+    void Update();
+}

RobotApp/Services/State/ManualState.cs

@@ -0,0 +1,7 @@
+using RobotApp.Common.Shares.Enums;
+
+namespace RobotApp.Services.State;
+
+public class ManualState<T>(ManualStateType state, IRobotState? superState = null) : RobotState<T>(state, superState) where T : Enum
+{ 
+}

RobotApp/Services/State/MoveState.cs

@@ -0,0 +1,7 @@
+using RobotApp.Common.Shares.Enums;
+
+namespace RobotApp.Services.State;
+
+public class MoveState<T>(MoveStateType state, IRobotState? superState = null) : RobotState<T>(state, superState) where T : Enum
+{
+}

RobotApp/Services/State/PlanState.cs

@@ -0,0 +1,7 @@
+using RobotApp.Common.Shares.Enums;
+
+namespace RobotApp.Services.State;
+
+public class PlanState<T>(PlanStateType state, IRobotState? superState = null) : RobotState<T>(state, superState) where T : Enum
+{
+}

RobotApp/Services/State/RobotState.cs

@@ -1,8 +1,38 @@
 namespace RobotApp.Services.State;
 
-public abstract class RobotState<T>(T type) where T : Enum
+public abstract class RobotState<T>(Enum name, IRobotState? superState = null) : IRobotState where T : Enum 
 {
-    public readonly T Type = type;
-    public virtual void Enter() { }
-    public virtual void Exit() { }
+    public Enum Name { get; private set; } = name;
+    public Type Type { get; private set; } = typeof(T);
+    public IRobotState? SuperState { get; private set; } = superState;
+    public List<IRobotState> SubStates { get; set; } = [];
+    public IRobotState? ActiveSubState { get; protected set; }
+    public IRobotState? HistoryState { get; protected set; }
+    public bool HasSubStates => SubStates.Count > 0;
+    public bool IsActive => ActiveSubState != null;
+    public override string ToString() => $"{GetType().Name}({Name})";
+
+    public event Action<IRobotState>? OnEnter;
+    public event Action<IRobotState>? OnExit;
+    public event Action<IRobotState, IRobotState>? OnTransition;
+
+    public virtual void Enter()
+    {
+        OnEnter?.Invoke(this);
+    }
+
+    public virtual void Exit()
+    {
+        OnExit?.Invoke(this);
+    }
+
+    public virtual bool CanTransitionTo(IRobotState targetState)
+    {
+        return true; 
+    }
+
+    public virtual void Update()
+    {
+        ActiveSubState?.Update();
+    }
 }

RobotApp/Services/State/RobotStateMachine.cs

@@ -0,0 +1,352 @@
+using RobotApp.Common.Shares.Enums;
+
+namespace RobotApp.Services.State;
+
+public record RobotStateMachine(Logger<RobotStateMachine> Logger) : IDisposable
+{
+    private readonly Dictionary<Type, Dictionary<Enum, IRobotState>> StateRegistry = [];
+
+    public IRobotState? CurrentState { get; private set; }
+    public event Action<IRobotState?, IRobotState>? OnStateChanged;
+    public bool IsInitialized = false;
+
+    private readonly Lock StateLock = new();
+
+    private void PrintStateAdded()
+    {
+        Console.WriteLine("=== All Registered States ===");
+        var allStates = StateRegistry.Values
+            .SelectMany(dict => dict.Values)
+            .Where(state => state.SuperState == null)
+            .OrderBy(state => state.Type.Name)
+            .ThenBy(state => state.Name.ToString());
+
+        foreach (var rootState in allStates)
+        {
+            PrintStateSimple(rootState, 0);
+        }
+
+        var totalStates = StateRegistry.Values.Sum(dict => dict.Count);
+        Console.WriteLine($"\nTotal: {totalStates} states registered");
+        Console.WriteLine("==============================\n");
+    }
+
+    private void PrintStateSimple(IRobotState state, int depth)
+    {
+        var indent = new string(' ', depth * 4);
+        var marker = state == CurrentState ? " [CURRENT]" : "";
+        var activeMarker = state.SuperState?.ActiveSubState == state ? " [ACTIVE]" : "";
+
+        Console.WriteLine($"{indent}- {state.Name}{marker}{activeMarker}");
+
+        foreach (var subState in state.SubStates.OrderBy(s => s.Name.ToString()))
+        {
+            PrintStateSimple(subState, depth + 1);
+        }
+    }
+
+    public void InitializeHierarchyStates()
+    {
+        if (IsInitialized) return;
+        RegisterState(new RootState<RootStateType>(RootStateType.System));
+        RegisterState(new RootState<RootStateType>(RootStateType.Auto));
+        RegisterState(new RootState<RootStateType>(RootStateType.Manual));
+        RegisterState(new RootState<RootStateType>(RootStateType.Service));
+        RegisterState(new RootState<RootStateType>(RootStateType.Stop));
+        RegisterState(new RootState<RootStateType>(RootStateType.Fault));
+        SetupSystemState();
+        SetupAutoState();
+        SetupManualState();
+        SetupServiceState();
+        SetupStopState();
+        SetupFaultState();
+
+        var systemState = GetState(RootStateType.System);
+        lock (StateLock)
+        {
+            systemState.Enter();
+            CurrentState = systemState;
+        }
+
+        PrintStateAdded();
+        IsInitialized = true;
+        Logger.Info($"Khởi tạo thành công State Machine với State hiện tại: {GetCurrentStatePath()}");
+    }
+
+    private void SetupSystemState()
+    {
+        var systemlState = GetState(RootStateType.System) ?? throw new InvalidOperationException($"Failed to get state RootStateType.Auto");
+        var initializingState = new SystemState<SystemStateType>(SystemStateType.Initializing, systemlState);
+        var standbyState = new SystemState<SystemStateType>(SystemStateType.Standby, systemlState);
+        var shutting_DownState = new SystemState<SystemStateType>(SystemStateType.Shutting_Down, systemlState);
+
+        systemlState.SubStates.AddRange([initializingState, standbyState, shutting_DownState]);
+
+        RegisterState(initializingState);
+        RegisterState(standbyState);
+        RegisterState(shutting_DownState);
+    }
+
+    private void SetupAutoState()
+    {
+        var autoState = GetState(RootStateType.Auto) ?? throw new InvalidOperationException($"Failed to get state RootStateType.Auto");
+        var idleState = new AutoState<AutoStateType>(AutoStateType.Idle, autoState);
+        var executingState = new AutoState<AutoStateType>(AutoStateType.Executing, autoState);
+        var pausedState = new AutoState<AutoStateType>(AutoStateType.Paused, autoState);
+        var holdingState = new AutoState<AutoStateType>(AutoStateType.Holding, autoState);
+        var cancelingState = new AutoState<AutoStateType>(AutoStateType.Canceling, autoState);
+        var recoveringState = new AutoState<AutoStateType>(AutoStateType.Recovering, autoState);
+        var remote_Override_State = new AutoState<AutoStateType>(AutoStateType.Remote_Override, autoState);
+
+        autoState.SubStates.AddRange([ idleState, executingState, pausedState, holdingState,
+                                       cancelingState, recoveringState, remote_Override_State ]);
+
+        RegisterState(idleState);
+        RegisterState(executingState);
+        RegisterState(pausedState);
+        RegisterState(holdingState);
+        RegisterState(cancelingState);
+        RegisterState(recoveringState);
+        RegisterState(remote_Override_State);
+
+        SetupExecutingState();
+    }
+
+    private void SetupManualState()
+    {
+        var manualState = GetState(RootStateType.Manual) ?? throw new InvalidOperationException($"Failed to get state RootStateType.Auto");
+        var idleState = new ManualState<ManualStateType>(ManualStateType.Idle, manualState);
+        var executingState = new ManualState<ManualStateType>(ManualStateType.Active, manualState);
+
+        manualState.SubStates.AddRange([idleState, executingState]);
+
+        RegisterState(idleState);
+        RegisterState(executingState);
+    }
+
+    private void SetupServiceState()
+    {
+        var serviceState = GetState(RootStateType.Service) ?? throw new InvalidOperationException($"Failed to get state RootStateType.Auto");
+        var idleState = new ServiceState<ServiceStateType>(ServiceStateType.Idle, serviceState);
+        var executingState = new ServiceState<ServiceStateType>(ServiceStateType.Active, serviceState);
+
+        serviceState.SubStates.AddRange([idleState, executingState]);
+
+        RegisterState(idleState);
+        RegisterState(executingState);
+    }
+
+    private void SetupStopState()
+    {
+        var stopState = GetState(RootStateType.Stop) ?? throw new InvalidOperationException($"Failed to get state RootStateType.Auto");
+        var emcStopState = new StopState<StopStateType>(StopStateType.EMC, stopState);
+        var protectiveState = new StopState<StopStateType>(StopStateType.Protective, stopState);
+        var manualStopState = new StopState<StopStateType>(StopStateType.Manual, stopState);
+
+        stopState.SubStates.AddRange([emcStopState, protectiveState, manualStopState]);
+
+        RegisterState(emcStopState);
+        RegisterState(protectiveState);
+        RegisterState(manualStopState);
+    }
+
+    private void SetupFaultState()
+    {
+        var faultState = GetState(RootStateType.Fault) ?? throw new InvalidOperationException($"Failed to get state RootStateType.Auto");
+        var navigationFaultState = new FaultState<FaultStateType>(FaultStateType.Navigation, faultState);
+        var localizationFaultState = new FaultState<FaultStateType>(FaultStateType.Localization, faultState);
+        var shelftFaultState = new FaultState<FaultStateType>(FaultStateType.Shielf, faultState);
+        var batteryFaultState = new FaultState<FaultStateType>(FaultStateType.Battery, faultState);
+        var driverFaultState = new FaultState<FaultStateType>(FaultStateType.Driver, faultState);
+        var peripheralsFaultState = new FaultState<FaultStateType>(FaultStateType.Peripherals, faultState);
+        var safetyFaultState = new FaultState<FaultStateType>(FaultStateType.Safety, faultState);
+        var comunicationFaultState = new FaultState<FaultStateType>(FaultStateType.Communication, faultState);
+
+        faultState.SubStates.AddRange([navigationFaultState, localizationFaultState, shelftFaultState,
+                                       batteryFaultState, driverFaultState, peripheralsFaultState, safetyFaultState,
+                                       comunicationFaultState]);
+
+        RegisterState(navigationFaultState);
+        RegisterState(localizationFaultState);
+        RegisterState(shelftFaultState);
+        RegisterState(batteryFaultState);
+        RegisterState(driverFaultState);
+        RegisterState(peripheralsFaultState);
+        RegisterState(safetyFaultState);
+        RegisterState(comunicationFaultState);
+    }
+
+    private void SetupExecutingState()
+    {
+        var executingState = GetState(AutoStateType.Executing) ?? throw new InvalidOperationException($"Failed to get state RootStateType.Auto");
+        var planningState = new ExecutingState<ExecutingStateType>(ExecutingStateType.Planning, executingState);
+        var movingState = new ExecutingState<ExecutingStateType>(ExecutingStateType.Moving, executingState);
+        var actStopState = new ExecutingState<ExecutingStateType>(ExecutingStateType.ACT, executingState);
+
+        executingState.SubStates.AddRange([planningState, movingState, actStopState]);
+
+        RegisterState(planningState);
+        RegisterState(movingState);
+        RegisterState(actStopState);
+
+        SetupACTState();
+        SetupMoveState();
+        SetupPlanState();
+    }
+
+    private void SetupACTState()
+    {
+        var ACTState = GetState(ExecutingStateType.ACT) ?? throw new InvalidOperationException($"Failed to get state RootStateType.Auto");
+        var dockingState = new ACTState<ACTStateType>(ACTStateType.Docking, ACTState);
+        var dockedState = new ACTState<ACTStateType>(ACTStateType.Docked, ACTState);
+        var chargingState = new ACTState<ACTStateType>(ACTStateType.Charging, ACTState);
+        var undockingState = new ACTState<ACTStateType>(ACTStateType.Undocking, ACTState);
+        var loadingState = new ACTState<ACTStateType>(ACTStateType.Loading, ACTState);
+        var unLoadingState = new ACTState<ACTStateType>(ACTStateType.Unloading, ACTState);
+        var techActionState = new ACTState<ACTStateType>(ACTStateType.TechAction, ACTState);
+
+        ACTState.SubStates.AddRange([dockingState, dockedState, chargingState, undockingState,
+                                     loadingState, unLoadingState, techActionState]);
+
+        RegisterState(dockingState);
+        RegisterState(dockedState);
+        RegisterState(chargingState);
+        RegisterState(undockingState);
+        RegisterState(loadingState);
+        RegisterState(unLoadingState);
+        RegisterState(techActionState);
+    }
+
+    private void SetupMoveState()
+    {
+        var moveState = GetState(ExecutingStateType.Moving) ?? throw new InvalidOperationException($"Failed to get state RootStateType.Auto");
+        var navigationState = new MoveState<MoveStateType>(MoveStateType.Navigation, moveState);
+        var avoidabceState = new MoveState<MoveStateType>(MoveStateType.Avoidance, moveState);
+        var approachState = new MoveState<MoveStateType>(MoveStateType.Approach, moveState);
+        var trackingState = new MoveState<MoveStateType>(MoveStateType.Tracking, moveState);
+        var repositioningState = new MoveState<MoveStateType>(MoveStateType.Repositioning, moveState);
+
+        moveState.SubStates.AddRange([navigationState, avoidabceState, approachState, trackingState, repositioningState]);
+
+        RegisterState(navigationState);
+        RegisterState(avoidabceState);
+        RegisterState(approachState);
+        RegisterState(trackingState);
+        RegisterState(repositioningState);
+    }
+
+    private void SetupPlanState()
+    {
+        var planState = GetState(ExecutingStateType.Planning) ?? throw new InvalidOperationException($"Failed to get state RootStateType.Auto");
+        var taskPlanState = new PlanState<PlanStateType>(PlanStateType.Task, planState);
+        var pathPlanState = new PlanState<PlanStateType>(PlanStateType.Path, planState);
+
+        planState.SubStates.AddRange([taskPlanState, pathPlanState]);
+
+        RegisterState(taskPlanState);
+        RegisterState(pathPlanState);
+    }
+
+    private void RegisterState<T>(RobotState<T> state) where T : Enum
+    {
+        var enumType = typeof(T);
+        StateRegistry.TryAdd(enumType, []);
+        StateRegistry[enumType][state.Name] = state;
+    }
+
+    public RobotState<T>? TryGetState<T>(T stateType) where T : Enum
+    {
+        var enumType = typeof(T);
+        if (StateRegistry.TryGetValue(enumType, out var states) && states.TryGetValue(stateType, out var state) && state is RobotState<T> robotState)
+        {
+            return robotState;
+        }
+        return null;
+    }
+
+    public RobotState<T> GetState<T>(T stateType) where T : Enum
+    {
+        return TryGetState(stateType) ?? throw new ArgumentException($"Failed to get state {stateType}");
+    }
+
+    public bool TransitionTo<T>(T stateType) where T : Enum
+    {
+        if (!IsInitialized)
+        {
+            Logger.Warning("State Machine chưa được khởi tạo");
+            return false;
+        }
+        lock (StateLock)
+        {
+            try
+            {
+                var targetState = TryGetState(stateType);
+                if (targetState == null)
+                {
+                    Logger.Warning($"State {stateType} không được tìm thấy");
+                    return false;
+                }
+
+                if (CurrentState != null)
+                {
+                    if (!CurrentState.CanTransitionTo(targetState))
+                    {
+                        Logger.Warning($"Không thể chuyển State từ {CurrentState.Name} thành {targetState.Name}");
+                        return false;
+                    }
+
+                    CurrentState.Exit();
+                }
+
+                var previousState = CurrentState;
+                CurrentState = targetState;
+                CurrentState.Enter();
+
+                Logger.Info($"Chuyển đổi State thành công: {GetStatePath(previousState)} -> {GetStatePath(targetState)}");
+                OnStateChanged?.Invoke(previousState, CurrentState);
+                return true;
+            }
+            catch (Exception ex)
+            {
+                Logger.Error($"Lỗi khi chuyển đổi State: {ex.Message}");
+                return false;
+            }
+        }
+    }
+
+    public void Update()
+    {
+        if (!IsInitialized)
+        {
+            Logger.Warning("State Machine chưa được khởi tạo");
+            return;
+        }
+        lock (StateLock)
+        {
+            CurrentState?.Update();
+        }
+    }
+
+    public string GetCurrentStatePath()
+    {
+        if (CurrentState == null) return "No State";
+        return GetStatePath(CurrentState);
+    }
+
+    private static string GetStatePath(IRobotState? state)
+    {
+        var path = new List<string>();
+        var current = state;
+        while (current != null)
+        {
+            path.Insert(0, current.Name.ToString());
+            current = current.SuperState;
+        }
+        return string.Join(".", path);
+    }
+
+    public void Dispose()
+    {
+        GC.SuppressFinalize(this);
+    }
+}

RobotApp/Services/State/RootState.cs

@@ -0,0 +1,36 @@
+using RobotApp.Common.Shares.Enums;
+
+namespace RobotApp.Services.State;
+
+public class RootState<T>(RootStateType state, IRobotState? superState = null) : RobotState<T>(state, superState) where T : Enum
+{
+    public override void Enter()
+    {
+        switch(Name)
+        {
+            case RootStateType.System:
+                ActiveSubState = SubStates.FirstOrDefault(s => s.Name.Equals(SystemStateType.Initializing));
+                ActiveSubState?.Enter();
+                break;
+            case RootStateType.Auto:
+                if(HistoryState is not null) ActiveSubState = HistoryState;
+                else ActiveSubState = SubStates.FirstOrDefault(s => s.Name.Equals(AutoStateType.Idle));
+                ActiveSubState?.Enter();
+                break;
+        }
+    }
+
+    public override void Exit()
+    {
+    }
+
+    public override bool CanTransitionTo(IRobotState targetState)
+    {
+        return true;
+    }
+
+    public override void Update()
+    {
+        ActiveSubState?.Update();
+    }
+}

RobotApp/Services/State/ServiceState.cs

@@ -0,0 +1,7 @@
+using RobotApp.Common.Shares.Enums;
+
+namespace RobotApp.Services.State;
+
+public class ServiceState<T>(ServiceStateType state, IRobotState? superState = null) : RobotState<T>(state, superState) where T : Enum
+{
+}

RobotApp/Services/State/StopState.cs

@@ -0,0 +1,8 @@
+using RobotApp.Common.Shares.Enums;
+
+namespace RobotApp.Services.State
+{
+    public class StopState<T>(StopStateType state, IRobotState? superState = null) : RobotState<T>(state, superState) where T : Enum
+    {
+    }
+}

RobotApp/Services/State/SystemState.cs

@@ -0,0 +1,21 @@
+using RobotApp.Common.Shares.Enums;
+
+namespace RobotApp.Services.State;
+
+public class SystemState<T>(SystemStateType state, IRobotState? superState = null) : RobotState<T>(state, superState) where T : Enum
+{
+    public override void Enter()
+    {
+    }
+    public override void Exit()
+    {
+    }
+    public override bool CanTransitionTo(IRobotState targetState)
+    {
+        return true;
+    }
+    public override void Update()
+    {
+        ActiveSubState?.Update();
+    }
+}

RobotApp/appsettings.json

@@ -5,8 +5,20 @@
   "Logging": {
     "LogLevel": {
       "Default": "Information",
-      "Microsoft.AspNetCore": "Warning"
+      "Microsoft.AspNetCore": "Warning",
+      "Microsoft.EntityFrameworkCore": "Warning"
     }
   },
-  "AllowedHosts": "*"
+  "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"
+  }
 }