Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/CameraLightOffAction.cs

@@ -0,0 +1,44 @@
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Interfaces;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+/// <summary>
+/// Tắt đèn camera (M918 - Light on OFF).
+/// </summary>
+[RobotAction(ActionType.CAMERA_LIGHT_OFF,
+            [ActionScope.INSTANT, ActionScope.NODE, ActionScope.EDGE],
+            [BlockingType.NONE, BlockingType.SOFT, BlockingType.HARD],
+            "Tắt đèn camera (M918 Light on OFF).",
+            "Đèn camera đã tắt (M918 Light on OFF).")]
+public class CameraLightOffAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    private IPlcController? PlcController;
+
+    private const int MAX_TIMEOUT = 4000;
+    private int CountTimeout = 0;
+
+    protected override Task StartAction()
+    {
+        PlcController = ServiceProvider.GetRequiredService<IPlcController>();
+        PlcController.SetLightOn(false); // M918 Light on OFF
+        return base.StartAction();
+    }
+
+    protected override Task ExecuteAction()
+    {
+        if (PlcController is not null && !PlcController.SetLightOnValue)
+        {
+            SetStatus(ActionEvent.FINISHED);
+            ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
+        }
+        else if (CountTimeout++ > MAX_TIMEOUT / ActionInterval)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = "Timeout action";
+        }
+
+        return base.ExecuteAction();
+    }
+}
+

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/CameraLightOnAction.cs

@@ -0,0 +1,45 @@
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Interfaces;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+/// <summary>
+/// Bật đèn camera (M918 - coil 2966).
+/// action tương tự các action PLC toggle khác: ghi ON, rồi chờ PLC phản hồi đã ON.
+/// </summary>
+[RobotAction(ActionType.CAMERA_LIGHT_ON,
+            [ActionScope.INSTANT, ActionScope.NODE, ActionScope.EDGE],
+            [BlockingType.NONE, BlockingType.SOFT, BlockingType.HARD],
+            "Bật đèn camera (M918 Light on).",
+            "Đèn camera đã bật (M918 Light on).")]
+public class CameraLightOnAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    private IPlcController? PlcController;
+
+    private const int MAX_TIMEOUT = 4000;
+    private int CountTimeout = 0;
+
+    protected override Task StartAction()
+    {
+        PlcController = ServiceProvider.GetRequiredService<IPlcController>();
+        PlcController.SetLightOn(true); // M918 Light on
+        return base.StartAction();
+    }
+
+    protected override Task ExecuteAction()
+    {
+        if (PlcController is not null && PlcController.SetLightOnValue)
+        {
+            SetStatus(ActionEvent.FINISHED);
+            ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
+        }
+        else if (CountTimeout++ > MAX_TIMEOUT / ActionInterval)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = "Timeout action";
+        }
+
+        return base.ExecuteAction();
+    }
+}
+

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/ControlLight.cs

@@ -0,0 +1,77 @@
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Devices;
+using RobotNet10.RobotApp.Services.Exceptions;
+using RobotNet10.RobotApp.Interfaces;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+/// <summary>
+/// Bật đèn camera (M918 - coil 2966).
+/// action tương tự các action PLC toggle khác: ghi ON, rồi chờ PLC phản hồi đã ON.
+/// </summary>
+[RobotAction(ActionType.CONTROL_LIGHT,
+            [ActionScope.INSTANT, ActionScope.NODE, ActionScope.EDGE],
+            [BlockingType.NONE, BlockingType.SOFT, BlockingType.HARD],
+            "Bật đèn camera (M918 Light on).",
+            "Đèn camera đã bật (M918 Light on).")]
+public class ControlLightAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    private IPlcController? PlcController;
+
+    private const int MAX_TIMEOUT = 4000;
+    private int CountTimeout = 0;
+    private bool IsLightOn = false;
+
+    
+    protected override void Initialize()
+    {
+        base.Initialize();
+
+        var controlTypeParam = Action?.ActionParameters?.FirstOrDefault(p =>
+            string.Equals(p.Key, "CONTROL_TYPE", StringComparison.OrdinalIgnoreCase));
+        if (controlTypeParam is null || string.IsNullOrWhiteSpace(controlTypeParam.Value))
+        {
+            throw new ActionException("ControlLight requires actionParameter CONTROL_TYPE (CONTROL_ON/CONTROL_OFF).");
+        }
+
+        var controlType = controlTypeParam.Value.Trim();
+        if (string.Equals(controlType, "CONTROL_ON", StringComparison.OrdinalIgnoreCase))
+        {
+            IsLightOn = true;
+        }
+        else if (string.Equals(controlType, "CONTROL_OFF", StringComparison.OrdinalIgnoreCase))
+        {
+            IsLightOn = false;
+        }
+        else
+        {
+            throw new ActionException($"CONTROL_TYPE value '{controlType}' is invalid. Expected CONTROL_ON or CONTROL_OFF.");
+        }
+
+        CountTimeout = 0;
+    }
+
+    protected override Task StartAction()
+    {
+        PlcController = ServiceProvider.GetRequiredService<IPlcController>();
+        PlcController.SetLightOn(IsLightOn); // M918 Light on
+        return base.StartAction();
+    }
+
+    protected override Task ExecuteAction()
+    {
+        if (PlcController is not null && PlcController.SetLightOnValue == IsLightOn)
+        {
+            SetStatus(ActionEvent.FINISHED);
+            ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
+        }
+        else if (CountTimeout++ > MAX_TIMEOUT / ActionInterval)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = "Timeout action";
+        }
+
+        return base.ExecuteAction();
+    }
+}
+

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/HomingCameraAction.cs

@@ -0,0 +1,64 @@
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Devices;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+/// <summary>
+/// Action homing camera (lift): gọi trực tiếp xuống động cơ CiA402 (giống device/hub).
+/// blockingType NONE: gửi lệnh homing và kết thúc ngay, không chờ hoàn thành.
+/// </summary>
+[RobotAction(ActionType.HOMING_CAMERA,
+            [ActionScope.INSTANT, ActionScope.NODE, ActionScope.EDGE],
+            [BlockingType.NONE],
+            "Homing camera (lift).",
+            "Homing camera requested.")]
+public class HomingCameraAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    protected override async Task StartAction()
+    {
+        try
+        {
+            var config = ServiceProvider.GetRequiredService<IConfiguration>();
+            var deviceProvider = ServiceProvider.GetRequiredService<IDeviceProvider>();
+
+            var deviceId = config.GetValue<string>("Modules:LiftModule:DeviceId") ?? "lift-motor";
+            var enable = config.GetValue<bool>("Modules:LiftModule:Enable", true);
+            if (!enable)
+            {
+                SetStatus(ActionEvent.FAILED);
+                ResultDescription = "Lift module is disabled in config.";
+                return;
+            }
+
+            var device = deviceProvider.GetDevice(deviceId);
+            if (device is not ICiA402Servo servo)
+            {
+                SetStatus(ActionEvent.FAILED);
+                ResultDescription = $"Device '{deviceId}' not found or is not ICiA402Servo.";
+                return;
+            }
+
+            var homingMethod = config.GetValue<byte>("Modules:LiftModule:HomingMethod", 21);
+            var homingSpeed = config.GetValue<int>("Modules:LiftModule:HomingSpeed", 30000);
+            var homingOffset = config.GetValue<int>("Modules:LiftModule:HomingOffset", 0);
+
+            // Giống device UI: bước 1 SetParam (Apply Params), bước 2 Start Homing
+            Logger?.LogInformation("HomingCamera: SetParam (method={Method}, speed={Speed}, offset={Offset}) then Start Homing.", homingMethod, homingSpeed, homingOffset);
+            await servo.SetHomingMethodAsync(homingMethod, CancellationToken.None);
+            await servo.SetHomingSpeedAsync(homingSpeed, CancellationToken.None);
+            await servo.SetHomingOffsetAsync(homingOffset, CancellationToken.None);
+            await Task.Delay(100, CancellationToken.None); // delay giữa SetParam và Start như khi bấm trên device
+            await servo.StartHomingAsync(homingMethod, homingSpeed, CancellationToken.None);
+
+            SetStatus(ActionEvent.FINISHED);
+            ResultDescription = "Homing camera requested (direct to drive).";
+        }
+        catch (Exception ex)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = $"Homing camera failed: {ex.Message}";
+        }
+
+        await base.StartAction();
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/LiftCameraByHeightAction.cs

@@ -0,0 +1,250 @@
+// using RobotNet.VDA5050.Type;
+// using RobotNet10.RobotApp.Devices;
+// using RobotNet10.RobotApp.Services.Exceptions;
+// using System.Globalization;
+
+// namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+// /// <summary>
+// /// Action đưa camera (lift) tới vị trí theo chiều cao (m).
+// /// Gọi trực tiếp xuống động cơ CiA402 (giống device/hub), không qua LiftModule state machine.
+// /// actionParameters: HEIGHT (unit: m), ví dụ "0.825".
+// /// blockingType NONE: gửi lệnh di chuyển và kết thúc ngay, không chờ hoàn thành.
+// /// </summary>
+// [RobotAction(ActionType.LIFT_CAMERA_BY_HEIGHT,
+//             [ActionScope.INSTANT, ActionScope.NODE, ActionScope.EDGE],
+//             [BlockingType.NONE, BlockingType.HARD, BlockingType.SOFT],
+//             "Lift camera with height (unit: m).",
+//             "Lift camera move requested.")]
+// public class LiftCameraByHeightAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+// {
+//     private double _heightM;
+//     private double _timeoutMs = 20 * 1000;
+
+//     protected override void Initialize()
+//     {
+//         base.Initialize();
+
+//         var heightParam = Action?.ActionParameters?.FirstOrDefault(p =>
+//             string.Equals(p.Key, "HEIGHT", StringComparison.OrdinalIgnoreCase));
+//         if (heightParam is null || string.IsNullOrWhiteSpace(heightParam.Value))
+//         {
+//             throw new ActionException("LiftCameraByHeight requires actionParameter HEIGHT (unit: m).");
+//         }
+
+//         if (!double.TryParse(heightParam.Value, NumberStyles.Any, CultureInfo.InvariantCulture, out _heightM))
+//         {
+//             throw new ActionException($"HEIGHT value '{heightParam.Value}' is not a valid number.");
+//         }
+//     }
+
+//     protected override async Task StartAction()
+//     {
+//         try
+//         {
+//             var config = ServiceProvider.GetRequiredService<IConfiguration>();
+//             var deviceProvider = ServiceProvider.GetRequiredService<IDeviceProvider>();
+
+//             var deviceId = config.GetValue<string>("Modules:LiftModule:DeviceId") ?? "lift-motor";
+//             var enable = config.GetValue<bool>("Modules:LiftModule:Enable", true);
+//             if (!enable)
+//             {
+//                 SetStatus(ActionEvent.FAILED);
+//                 ResultDescription = "Lift module is disabled in config.";
+//                 return;
+//             }
+
+//             var device = deviceProvider.GetDevice(deviceId);
+//             if (device is not ICiA402Servo servo)
+//             {
+//                 SetStatus(ActionEvent.FAILED);
+//                 ResultDescription = $"Device '{deviceId}' not found or is not ICiA402Servo.";
+//                 return;
+//             }
+
+//             // Đọc config để map height (m) -> position (encoder), giống LiftModuleService.GetPositionFromHeightM
+//             var minHeightM = config.GetValue<double>("Modules:LiftModule:MinHeightM", 0);
+//             var maxHeightM = config.GetValue<double>("Modules:LiftModule:MaxHeightM", 1);
+//             var minPosition = config.GetValue<int>("Modules:LiftModule:MinPosition", 0);
+//             var maxPosition = config.GetValue<int>("Modules:LiftModule:MaxPosition", 1000000);
+
+//             int position;
+//             if (maxHeightM <= minHeightM)
+//             {
+//                 position = minPosition;
+//             }
+//             else
+//             {
+//                 var t = Math.Clamp((_heightM - minHeightM) / (maxHeightM - minHeightM), 0, 1);
+//                 position = minPosition + (int)(t * (maxPosition - minPosition));
+//             }
+
+//             var velocity = config.GetValue<uint>("Modules:LiftModule:ProfileVelocity", 100000);
+//             var acceleration = config.GetValue<uint>("Modules:LiftModule:ProfileAcceleration", 50000);
+//             var deceleration = config.GetValue<uint>("Modules:LiftModule:ProfileDeceleration", 50000);
+
+//             Logger?.LogInformation("LiftCameraByHeight: calling servo directly (deviceId={DeviceId}), MoveToPositionAsync(position={Position}, height={HeightM} m).", deviceId, position, _heightM);
+//             await servo.MoveToPositionAsync(position, velocity, acceleration, deceleration, CancellationToken.None);
+
+//             // Timeout = (int)_timeoutMs;
+//             SetStatus(ActionEvent.FINISHED);
+//             ResultDescription = $"Lift camera move to height {_heightM} m requested (direct to drive).";    
+//         }
+//         catch (Exception ex)
+//         {
+//             SetStatus(ActionEvent.FAILED);
+//             ResultDescription = $"Lift camera by height failed: {ex.Message}";
+//         }
+
+//         await base.StartAction();
+//     }
+
+//     // protected override async Task ExecuteAction()
+//     // {
+//     //     // to do: wait for the lift camera to reach the target height
+        
+//     // }
+
+//     // protected override async Task CleanupAction()
+//     // {
+//     //     // to do: cleanup the lift camera
+//     // }
+// }
+
+using RobotNet.VDA5050.Type;
+using RobotNet10.CANOpen.CiA402.Enums;
+using RobotNet10.RobotApp.Devices;
+using RobotNet10.RobotApp.Services.Exceptions;
+using System.Globalization;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+/// <summary>
+/// Action đưa camera (lift) tới vị trí theo chiều cao (m).
+/// Gọi trực tiếp xuống động cơ CiA402 (giống device/hub), không qua LiftModule state machine.
+/// actionParameters: HEIGHT (unit: m), ví dụ "0.825".
+/// blockingType NONE: gửi lệnh di chuyển và kết thúc ngay, không chờ hoàn thành.
+/// </summary>
+[RobotAction(ActionType.LIFT_CAMERA_BY_HEIGHT,
+            [ActionScope.INSTANT, ActionScope.NODE, ActionScope.EDGE],
+            [BlockingType.NONE, BlockingType.HARD],
+            "Lift camera with height (unit: m).",
+            "Lift camera move requested.")]
+public class LiftCameraByHeightAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    private double _heightM;
+
+    protected override void Initialize()
+    {
+        base.Initialize();
+
+        var heightParam = Action?.ActionParameters?.FirstOrDefault(p =>
+            string.Equals(p.Key, "HEIGHT", StringComparison.OrdinalIgnoreCase));
+        if (heightParam is null || string.IsNullOrWhiteSpace(heightParam.Value))
+        {
+            throw new ActionException("LiftCameraByHeight requires actionParameter HEIGHT (unit: m).");
+        }
+
+        if (!double.TryParse(heightParam.Value, NumberStyles.Any, CultureInfo.InvariantCulture, out _heightM))
+        {
+            throw new ActionException($"HEIGHT value '{heightParam.Value}' is not a valid number.");
+        }
+    }
+
+    protected override async Task StartAction()
+    {
+        try
+        {
+            var config = ServiceProvider.GetRequiredService<IConfiguration>();
+            var deviceProvider = ServiceProvider.GetRequiredService<IDeviceProvider>();
+
+            var deviceId = config.GetValue<string>("Modules:LiftModule:DeviceId") ?? "lift-motor";
+            var enable = config.GetValue<bool>("Modules:LiftModule:Enable", true);
+            if (!enable)
+            {
+                SetStatus(ActionEvent.FAILED);
+                ResultDescription = "Lift module is disabled in config.";
+                return;
+            }
+
+            var device = deviceProvider.GetDevice(deviceId);
+            if (device is not ICiA402Servo servo)
+            {
+                SetStatus(ActionEvent.FAILED);
+                ResultDescription = $"Device '{deviceId}' not found or is not ICiA402Servo.";
+                return;
+            }
+
+            // Đọc config để map height (m) -> position (encoder), giống LiftModuleService.GetPositionFromHeightM
+            var minHeightM = config.GetValue<double>("Modules:LiftModule:MinHeightM", 0);
+            var maxHeightM = config.GetValue<double>("Modules:LiftModule:MaxHeightM", 1);
+            var minPosition = config.GetValue<int>("Modules:LiftModule:MinPosition", 0);
+            var maxPosition = config.GetValue<int>("Modules:LiftModule:MaxPosition", 1000000);
+
+            int position;
+            if (maxHeightM <= minHeightM)
+            {
+                position = minPosition;
+            }
+            else
+            {
+                var t = Math.Clamp((_heightM - minHeightM) / (maxHeightM - minHeightM), 0, 1);
+                position = minPosition + (int)(t * (maxPosition - minPosition));
+            }
+
+            var velocity = config.GetValue<uint>("Modules:LiftModule:ProfileVelocity", 100000);
+            var acceleration = config.GetValue<uint>("Modules:LiftModule:ProfileAcceleration", 50000);
+            var deceleration = config.GetValue<uint>("Modules:LiftModule:ProfileDeceleration", 50000);
+            var tolerance = config.GetValue<int>("Modules:LiftModule:ActionTargetTolerance", 900);
+            var checkIntervalMs = config.GetValue<int>("Modules:LiftModule:ActionStatusWordCheckIntervalMs", 100);
+            var timeoutMs = config.GetValue<int>("Modules:LiftModule:ActionMoveTimeoutMs", 300000);
+
+            Logger?.LogInformation("LiftCameraByHeight: calling servo directly (deviceId={DeviceId}), MoveToPositionAsync(position={Position}, height={HeightM} m).", deviceId, position, _heightM);
+            await servo.MoveToPositionAsync(position, velocity, acceleration, deceleration, CancellationToken.None);
+            await WaitForMovementCompletedAsync(servo, position, tolerance, checkIntervalMs, timeoutMs, CancellationToken.None);
+
+            SetStatus(ActionEvent.FINISHED);
+            ResultDescription = $"Lift camera reached height {_heightM} m (direct to drive).";
+        }
+        catch (Exception ex)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = $"Lift camera by height failed: {ex.Message}";
+        }
+
+        await base.StartAction();
+    }
+
+    private static async Task WaitForMovementCompletedAsync(
+        ICiA402Servo servo,
+        int targetPosition,
+        int tolerance,
+        int checkIntervalMs,
+        int timeoutMs,
+        CancellationToken ct)
+    {
+        var startedAt = DateTime.UtcNow;
+        var pollInterval = Math.Max(10, checkIntervalMs);
+        var maxWait = TimeSpan.FromMilliseconds(Math.Max(1000, timeoutMs));
+
+        while (DateTime.UtcNow - startedAt < maxWait)
+        {
+            var statusword = await servo.GetStatuswordAsync(ct);
+            if (statusword.GetState() == DriveState.Fault)
+            {
+                throw new ActionException("Lift movement failed: servo entered fault state.");
+            }
+
+            var currentPosition = await servo.GetActualPositionAsync(ct);
+            if (statusword.TargetReached && Math.Abs(currentPosition - targetPosition) <= Math.Max(50, tolerance))
+            {
+                return;
+            }
+
+            await Task.Delay(pollInterval, ct);
+        }
+
+        throw new TimeoutException($"Lift movement timeout after {maxWait.TotalSeconds:F0}s.");
+    }
+}
+

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotAction.cs

@@ -0,0 +1,360 @@
+using Appccelerate.StateMachine;
+using Appccelerate.StateMachine.Machine;
+using RobotNet.VDA5050;
+using RobotNet.VDA5050.Type;
+using RobotNet10.Common;
+using RobotNet10.NavigationTune.Shared.Models;
+using RobotNet10.RobotApp.Services.Exceptions;
+using System.Diagnostics;
+using System.Reflection;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+public abstract class RobotAction : IAsyncDisposable
+{
+    public ActionType Type { get; }
+    public string Id { get; private set; } = "";
+    public string? Description { get; private set; } 
+    public BlockingType BlockingType { get; private set; }
+    public RobotNet.VDA5050.InstantAction.ActionParameter[] Parameters { get; protected set; } = [];
+    public ActionStatus Status => CurrentStatus;
+    public string ResultDescription { get; set; } = "";
+    public bool IsCompleted => CurrentStatus == ActionStatus.FINISHED || CurrentStatus == ActionStatus.FAILED;
+    public long CompletionTime { get; private set; } = 0;
+    public ActionScope ActionScope { get; set; }
+    public RobotActionAttribute ActionAttribute { get; }
+    public long SequenceNumber { get; internal set; }
+
+    private WatchThreadAsync<RobotAction>? ActionTimer;
+    protected const int ActionInterval = 100;
+
+    protected IServiceProvider ServiceProvider;
+    protected RobotNet.VDA5050.InstantAction.Action? Action;
+    protected ILogger<RobotAction>? Logger;
+    protected bool IsPaused = false;
+    protected ActionStatus HistoryStatus;
+    private bool _justResumed = false;
+
+    private bool _isDisposed = false;
+    private bool IsCancelAction = false;
+    private PassiveStateMachine<ActionStatus, ActionEvent>? _stateMachine;
+    private ActionStatus CurrentStatus;
+    private long StartTime;
+    private int Timeout;
+
+    private readonly Lock _lock = new();
+
+    public RobotAction(IServiceProvider serviceProvider)
+    {
+        var derivedType = GetType();
+
+        ActionAttribute = derivedType.GetCustomAttribute<RobotActionAttribute>()
+            ?? throw new InvalidOperationException(
+                $"Class {derivedType.Name} must have RobotActionAttribute");
+
+        Type = ActionAttribute.ActionType;
+        ServiceProvider = serviceProvider;
+
+        Logger = ServiceProvider.GetRequiredService<ILogger<RobotAction>>();
+        InitializeStatus();
+    }
+
+    public void Initialize(ActionScope actionScope, RobotNet.VDA5050.InstantAction.Action action)
+    {
+        ActionScope = actionScope;
+        Action = action;
+        BlockingType = action.BlockingType;
+        Id = action.ActionId;
+        Description = action.ActionDescription;
+        Initialize();
+    }
+
+    public void Start()
+    {
+        lock (_lock)
+        {
+            if (Status != ActionStatus.WAITING) return;
+            ActionTimer = new(ActionInterval, ActionHandler, Logger);
+            SetStatus(ActionEvent.INITIALIZING);
+            ActionTimer.Start();
+            StartTime = GetCurrentTimeMs();
+        }
+    }
+
+    public void Pause()
+    {
+        if (_isDisposed) return;
+        lock (_lock)
+        {
+            if (IsCompleted) return;
+            HistoryStatus = Status;
+            SetStatus(ActionEvent.PAUSED);
+            IsPaused = true;
+        }
+    }
+
+    public void Resume()
+    {
+        if (_isDisposed) return;
+        lock (_lock)
+        {
+            if (Status == ActionStatus.PAUSED)
+            {
+                IsPaused = false;
+                _justResumed = true;
+                // Restore về trạng thái trước khi pause
+                ActionEvent resumeEvent = HistoryStatus switch
+                {
+                    ActionStatus.RUNNING => ActionEvent.RUNNING,
+                    ActionStatus.INITIALIZING => ActionEvent.INITIALIZING,
+                    _ => ActionEvent.WAITING
+                };
+                SetStatus(resumeEvent);
+            }
+        }
+    }
+
+    public void Cancel()
+    {
+        if (_isDisposed) return;
+        lock (_lock)
+        {
+            if (!IsCompleted) IsCancelAction = true;
+            if (Status == ActionStatus.WAITING) _ = StopAction();
+        }
+    }
+
+    /// <summary>
+    /// VDA5050: Gracefully finish an action (e.g., when robot leaves an edge).
+    /// Sets status to FINISHED instead of FAILED (Cancel).
+    /// </summary>
+    public void Finish()
+    {
+        if (_isDisposed) return;
+        lock (_lock)
+        {
+            if (IsCompleted) return;
+            // State machine allows FINISHED from RUNNING and INITIALIZING
+            if (CurrentStatus == ActionStatus.RUNNING || CurrentStatus == ActionStatus.INITIALIZING)
+            {
+                SetStatus(ActionEvent.FINISHED);
+                if (string.IsNullOrEmpty(ResultDescription))
+                    ResultDescription = "Action completed (edge transition).";
+            }
+            else
+            {
+                // WAITING or PAUSED: cancel as fallback
+                IsCancelAction = true;
+            }
+        }
+    }
+
+    protected virtual Task StartAction()
+    {
+        return Task.CompletedTask;
+    }
+
+    protected virtual Task StopAction()
+    {
+        SetStatus(ActionEvent.FAILED);
+        ResultDescription = "Action bị hủy bỏ.";
+        return Task.CompletedTask;
+    }
+
+    protected virtual Task CompleteAction()
+    {
+        return Task.CompletedTask;
+    }
+
+    /// <summary>
+    /// Giải phóng tài nguyên được sử dụng trong action.
+    /// Luôn được gọi trong DisposeAsync, đảm bảo cleanup cả khi FINISHED lẫn FAILED/Cancel.
+    /// </summary>
+    protected virtual Task CleanupAction()
+    {
+        return Task.CompletedTask;
+    }
+
+    protected virtual Task ExecuteAction()
+    {
+        return Task.CompletedTask;
+    }
+
+    protected virtual Task PauseAction()
+    {
+        return Task.CompletedTask;
+    }
+
+    protected virtual Task ResumeAction()
+    {
+        return Task.CompletedTask;
+    }
+
+    protected virtual void Initialize()
+    {
+        if (Action is null) throw new ActionException("Khởi tạo Action không tồn tại");
+        if (EnumHelper.TryParse(Action.ActionType, out ActionType type))
+        {
+            if (type != Type) throw new ActionException($"ActionType {Action.ActionType} không khớp với action hiện tại {Type}.");
+        }
+        else throw new ActionException($"ActionType {Action.ActionType} không hợp lệ.");
+
+        if (!ActionAttribute.BlockingTypes.Any(bt => bt == BlockingType)) throw new ActionException($"BlockingType {BlockingType} không được hỗ trợ cho action {Type}.");
+        if (!ActionAttribute.ActionScopes.Any(sp => sp == ActionScope)) throw new ActionException($"ActionScope {ActionScope} không được hỗ trợ cho action {Type}.");
+
+        if (Action.ActionParameters != null && Action.ActionParameters.Length > 0)
+        {
+            var para = Action.ActionParameters.FirstOrDefault(p => p.Key == "timeout");
+            if (para is not null && int.TryParse(para.Value, out int miliseconds) && miliseconds > 100) Timeout = miliseconds;
+            else Timeout = -1;
+        }
+    }
+
+    private async Task ActionHandler()
+    {
+        try
+        {
+            if (Timeout > 0)
+            {
+                long now = GetCurrentTimeMs();
+                if (now >= (StartTime + Timeout)) throw new TimeoutException($"Action [{Type} - {Id}] timeout. Timeout: {Timeout} ms ");
+            }
+
+            ActionStatus status;
+            bool isCancel = false;
+            bool justResumed = false;
+            lock (_lock)
+            {
+                status = CurrentStatus;
+                isCancel = IsCancelAction;
+                justResumed = _justResumed;
+                if (_justResumed) _justResumed = false;
+            }
+
+            if (isCancel)
+            {
+                await StopAction();
+            }
+            else
+            {
+                if (status == ActionStatus.INITIALIZING)
+                {
+                    Logger?.LogInformation($"Executing action {Type}");
+                    SetStatus(ActionEvent.RUNNING);
+                    await StartAction();
+                }
+                else if (status == ActionStatus.RUNNING)
+                {
+                    if (justResumed)
+                    {
+                        await ResumeAction();
+                    }
+                    await ExecuteAction();
+                }
+                else if (status == ActionStatus.PAUSED)
+                {
+                    await PauseAction();
+                }
+            }
+
+            if (IsCompleted)
+            {
+                await CompleteAction();
+                await DisposeAsync();
+            }
+        }
+        catch (Exception ex)
+        {
+            Logger?.LogError($"Action [{Type} - {Id}] execution error: {ex.Message}");
+            lock (_lock)
+            {
+                SetStatus(ActionEvent.FAILED);
+            }
+            ResultDescription = $"Thực hiện action [{Type} - {Id}] xảy ra lỗi: {ex.Message}";
+            await DisposeAsync();
+        }
+    }
+
+    private void InitializeStatus()
+    {
+        var builder = new StateMachineDefinitionBuilder<ActionStatus, ActionEvent>();
+
+        builder.In(ActionStatus.WAITING)
+            .ExecuteOnEntry(() => { CurrentStatus = ActionStatus.WAITING; })
+            .On(ActionEvent.INITIALIZING).Goto(ActionStatus.INITIALIZING)
+            .On(ActionEvent.PAUSED).Goto(ActionStatus.PAUSED)
+            .On(ActionEvent.FAILED).Goto(ActionStatus.FAILED);
+
+
+        builder.In(ActionStatus.INITIALIZING)
+            .ExecuteOnEntry(() => { CurrentStatus = ActionStatus.INITIALIZING; })
+            .On(ActionEvent.RUNNING).Goto(ActionStatus.RUNNING)
+            .On(ActionEvent.PAUSED).Goto(ActionStatus.PAUSED)
+            .On(ActionEvent.FINISHED).Goto(ActionStatus.FINISHED)
+            .On(ActionEvent.FAILED).Goto(ActionStatus.FAILED);
+
+        builder.In(ActionStatus.RUNNING)
+            .ExecuteOnEntry(() => { CurrentStatus = ActionStatus.RUNNING; })
+            .On(ActionEvent.PAUSED).Goto(ActionStatus.PAUSED)
+            .On(ActionEvent.FINISHED).Goto(ActionStatus.FINISHED)
+            .On(ActionEvent.FAILED).Goto(ActionStatus.FAILED);
+
+        builder.In(ActionStatus.PAUSED)
+            .ExecuteOnEntry(() => { CurrentStatus = ActionStatus.PAUSED; })
+            .On(ActionEvent.WAITING).Goto(ActionStatus.WAITING)
+            .On(ActionEvent.INITIALIZING).Goto(ActionStatus.INITIALIZING)
+            .On(ActionEvent.RUNNING).Goto(ActionStatus.RUNNING)
+            .On(ActionEvent.FAILED).Goto(ActionStatus.FAILED);
+
+        builder.In(ActionStatus.FINISHED)
+            .ExecuteOnEntry(() =>
+            {
+                CurrentStatus = ActionStatus.FINISHED;
+                CompletionTime = GetCurrentTimeMs();
+            });
+
+        builder.In(ActionStatus.FAILED)
+            .ExecuteOnEntry(() =>
+            {
+                CurrentStatus = ActionStatus.FAILED;
+                CompletionTime = GetCurrentTimeMs();
+            });
+
+        _stateMachine = builder
+            .WithInitialState(ActionStatus.WAITING)
+            .Build()
+            .CreatePassiveStateMachine();
+
+        _stateMachine.Start();
+    }
+
+    private static long GetCurrentTimeMs()
+    {
+        return Stopwatch.GetTimestamp() * 1000 / Stopwatch.Frequency;
+    }
+
+    protected void SetStatus(ActionEvent eventStatus)
+    {
+        lock (_lock)
+        {
+            _stateMachine?.Fire(eventStatus);
+        }
+    }
+
+    public async ValueTask DisposeAsync()
+    {
+        bool shouldStop;
+        lock (_lock)
+        {
+            if (_isDisposed) return;
+            _isDisposed = true;
+            shouldStop = !IsCompleted;
+        }
+
+        if (shouldStop) await StopAction();
+        await CleanupAction();
+        ActionTimer?.Dispose();
+        ActionTimer = null;
+        GC.SuppressFinalize(this);
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotActionAttribute.cs

@@ -0,0 +1,40 @@
+using RobotNet.VDA5050.Factsheet;
+using RobotNet.VDA5050.Type;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+[AttributeUsage(AttributeTargets.Class, AllowMultiple = false, Inherited = false)]
+public class RobotActionAttribute : Attribute
+{
+    public ActionType ActionType { get; }
+    public IReadOnlyList<ActionScope> ActionScopes { get; }
+    public IReadOnlyList<BlockingType> BlockingTypes { get; }
+    public string? ActionDescription { get; }
+    public string? ResultDescription { get; }
+    public RobotActionAttribute(
+        ActionType actionType,
+        ActionScope[] scopes,
+        BlockingType[] blockingTypes,
+        string? description = null,
+        string? resultDescription = null)
+    {
+        ArgumentNullException.ThrowIfNull(scopes);
+        ArgumentNullException.ThrowIfNull(blockingTypes);
+
+        if (!Enum.IsDefined(actionType))
+        {
+            throw new ArgumentException("Invalid action type.", nameof(actionType));
+        }
+
+        if (scopes.Length == 0)
+            throw new ArgumentException("Scopes cannot be empty.", nameof(scopes));
+        if (blockingTypes.Length == 0)
+            throw new ArgumentException("Blocking types cannot be empty.", nameof(blockingTypes));
+
+        ActionType = actionType;
+        ActionScopes = Array.AsReadOnly(scopes);
+        BlockingTypes = Array.AsReadOnly(blockingTypes);
+        ActionDescription = description;
+        ResultDescription = resultDescription;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotActionExample.cs

@@ -0,0 +1,71 @@
+using RobotNet.VDA5050.Factsheet;
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Services.Exceptions;
+using System.Collections.ObjectModel;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+[RobotAction(ActionType.EXAMPLE, 
+            [ActionScope.INSTANT, ActionScope.NODE, ActionScope.EDGE], 
+            [BlockingType.NONE, BlockingType.SOFT, BlockingType.HARD],
+            "This is an example robot action.",
+            "Example robot action completed.")]
+public class RobotActionExample(IServiceProvider serviceProvider) : RobotAction(serviceProvider)
+{
+    private static readonly ReadOnlyCollection<ActionParameter> ActionParameters = new List<ActionParameter>
+    {
+        new() {
+            Key = "exampleParam",
+            ValueDataType = ValueDataType.STRING,
+            Description = "This is an example string parameter.",
+            IsOptional = true
+        },
+    }.AsReadOnly();
+
+    string paramExample = "";
+
+    protected override void Initialize()
+    {
+        base.Initialize();
+
+        if (ActionParameters.Count > 0)
+        {
+            foreach (var parameterStore in ActionParameters)
+            {
+                if (!parameterStore.IsOptional)
+                {
+                    if (Action is null || Action.ActionParameters is null || !Action.ActionParameters.Any(a => a.Key == parameterStore.Key)) throw new ActionException($"Thiếu tham số bắt buộc '{parameterStore.Key}' cho action {Type}.");
+                }
+            }
+            Parameters = Action is null || Action.ActionParameters is null ? [] : Action.ActionParameters;
+        }
+
+        var para = Parameters.FirstOrDefault(p => p.Key == "exampleParam") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'exampleParam'");
+        paramExample = para.Value?.ToString() ?? "";
+    }
+
+    protected override Task StartAction()
+    {
+        return base.StartAction();
+    }
+
+    protected override Task ExecuteAction()
+    {
+        return base.ExecuteAction();
+    }
+
+    protected override Task StopAction()
+    {
+        return base.StopAction();
+    }
+
+    protected override Task PauseAction()
+    {
+        return base.PauseAction();
+    }
+
+    protected override Task ResumeAction()
+    {
+        return base.ResumeAction();
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotActionProvider.cs

@@ -0,0 +1,105 @@
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Interfaces;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+
+public class RobotActionProvider(Logger<RobotActionProvider> Logger, IServiceProvider ServiceProvider) : BackgroundService, IRobotActionProvider
+{
+    public bool IsInitialized { get; private set; }
+    private Dictionary<ActionType, Type> Actions = [];
+
+    public RobotAction GetRobotAction(ActionType type)
+    {
+        if (!Actions.TryGetValue(type, out var actionType))
+        {
+            Logger.Error($"RobotAction not found for ActionType: {type}");
+            throw new InvalidOperationException($"RobotAction not found for ActionType: {type}");
+        }
+
+        try
+        {
+            var instance = ActivatorUtilities.CreateInstance(ServiceProvider, actionType);
+
+            if (instance is not RobotAction robotAction)
+            {
+                Logger.Error($"Type {actionType.Name} is not a RobotAction");
+                throw new InvalidOperationException($"Type {actionType.Name} is not a RobotAction");
+            }
+
+            return robotAction;
+        }
+        catch (Exception ex)
+        {
+            Logger.Error($"Error creating instance of {actionType.Name}: {ex.Message}");
+            throw;
+        }
+    }
+
+    private static Dictionary<ActionType, Type> DiscoverActions()
+    {
+        var actionTypes = new Dictionary<ActionType, Type>();
+        var assemblies = AppDomain.CurrentDomain.GetAssemblies().Where(a => a.FullName?.StartsWith("RobotNet10.RobotApp") == true);
+        foreach (var assembly in assemblies)
+        {
+            var types = assembly.GetTypes().Where(t => t.IsClass && !t.IsAbstract && t.IsSubclassOf(typeof(RobotAction)));
+            foreach (var type in types)
+            {
+                var attributes = type.GetCustomAttributes(typeof(RobotActionAttribute), false);
+                if (attributes.Length > 0)
+                {
+                    foreach(var attribute in attributes)
+                    {
+                        if(attribute is RobotActionAttribute robotAttribute)
+                        {
+                            actionTypes[robotAttribute.ActionType] = type;
+                            break;
+                        }
+                    }
+                }
+            }
+        }
+        return actionTypes;
+    }
+
+    protected override async Task ExecuteAsync(CancellationToken stoppingToken)
+    {
+        await Task.Yield();
+
+        try
+        {
+            Logger.Info("Initializing RobotActionProvider...");
+            Actions = DiscoverActions();
+            IsInitialized = true;
+            Logger.Info($"Discovered {Actions.Count} robot actions.");
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"Failed to discover robot actions. {ex}");
+            throw;
+        }
+    }
+
+    public RobotAction[] GetRobotActions()
+    {
+        try
+        {
+            List<RobotAction> robotActions = [];
+            foreach (var actionType in Actions.Values)
+            {
+                var instance = ActivatorUtilities.CreateInstance(ServiceProvider, actionType);
+                if (instance is not RobotAction robotAction)
+                {
+                    Logger.Error($"Type {actionType.Name} is not a RobotAction");
+                    throw new InvalidOperationException($"Type {actionType.Name} is not a RobotAction");
+                }
+                robotActions.Add(robotAction);
+            }
+            return [.. robotActions];
+        }
+        catch (Exception ex)
+        {
+            throw new Exception ($"Error get RobotActions: {ex.Message}");
+        }
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotCancelOrderAction.cs

@@ -0,0 +1,56 @@
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Interfaces;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+
+[RobotAction(ActionType.CANCEL_ORDER,
+            [ActionScope.INSTANT],
+            [BlockingType.NONE, BlockingType.SOFT, BlockingType.HARD],
+            "Hủy bỏ Order hiện tại của robot.",
+            "Robot đã hủy bỏ Order hiện tại.")]
+public class RobotCancelOrderAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    private const int MAX_TIMEOUT = 4000;
+    private int CountTimeout = 0;
+
+    private IOrder? RobotOrder;
+    private IAction? RobotAction;
+    protected override Task StartAction()
+    {
+        RobotOrder = ServiceProvider.GetRequiredService<IOrder>();
+        RobotAction = ServiceProvider.GetRequiredService<IAction>();
+        RobotOrder.StopOrder();
+        RobotAction.StopOrderAction();
+        CountTimeout = 0;
+        return base.StartAction();
+    }
+
+    protected override Task StopAction()
+    {
+        return Task.CompletedTask;  
+    }
+
+    protected override Task ExecuteAction()
+    {
+        if (RobotOrder is null || RobotAction is null)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = $"Không thể tìm thấy module quản lý {(RobotOrder is null ? "Order" : RobotAction is null ? "Action" : "")}";
+        }
+        else
+        {
+            if (RobotOrder.NodeStates.Length == 0 && RobotOrder.EdgeStates.Length == 0 && !RobotAction.HasActionRunning)
+            {
+                SetStatus(ActionEvent.FINISHED);
+                ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
+            }
+            else if(CountTimeout++ > MAX_TIMEOUT / ActionInterval)
+            {
+                SetStatus(ActionEvent.FAILED);
+                ResultDescription = "Timeout action";
+            }
+        }
+        return base.ExecuteAction();
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotDockToAction.cs

@@ -0,0 +1,194 @@
+using Microsoft.EntityFrameworkCore;
+using RobotNet.VDA5050.Factsheet;
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Data;
+using RobotNet10.RobotApp.Detection;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Services.Exceptions;
+using RobotNet10.RobotApp.Shared.Enums;
+using RobotNet10.Shared.Detection;
+using RobotNet10.Shared.Enum;
+using RobotNet10.Shared.Numbers;
+using System.Collections.ObjectModel;
+using System.Text.Json;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+[RobotAction(ActionType.DOCK_TO,
+            [ActionScope.INSTANT, ActionScope.NODE],
+            [BlockingType.HARD],
+            "Robot di chuyển vào vị trí đặc biệt",
+            "Robot đã dock đến vị trí sạc hoặc bến đỗ.")]
+public class RobotDockToAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    private static readonly ReadOnlyCollection<ActionParameter> ActionParameters = new List<ActionParameter>
+    {
+        new()
+        {
+            Key = "stationId",
+            Description = "ID của vị trí dock.",
+            ValueDataType = ValueDataType.STRING,
+            IsOptional = false,
+        },
+        new()
+        {
+            Key = "direction",
+            Description = "Hướng di chuyển: FORWARD, BACKWARD",
+            ValueDataType = ValueDataType.STRING,
+            IsOptional = true,
+        },
+    }.AsReadOnly();
+
+    private IMarkerDetector? MarkerDetector;
+    private INavigation? Navigation;
+    private IDetectSession? DetectSession;
+    private string? StationId;
+    private RobotDirection? Direction = null;
+
+    private const int MAX_TIMEOUT = 60000 * 5;
+    private int CountTimeout = 0;
+    private bool IsFindedGoal = false;
+    private bool IsStartDockTo = false;
+    private bool IsHasLoad = false;
+
+    protected override async Task StartAction()
+    {
+        MarkerDetector = ServiceProvider.GetRequiredService<IMarkerDetector>();
+        Navigation = ServiceProvider.GetRequiredService<INavigation>();
+
+        if (!string.IsNullOrEmpty(StationId))
+        {
+            using var scope = ServiceProvider.CreateScope();
+            var dbContext = scope.ServiceProvider.GetRequiredService<ApplicationDbContext>();
+            var config = await dbContext.DockStationConfigs
+                .Include(d => d.MarkerEntries)
+                .FirstOrDefaultAsync(d => d.StationId == StationId && d.IsActive);
+
+            if (config is not null)
+            {
+                var MarkersSearchRequest = new MarkersSearchRequest
+                {
+                    X = config.X,
+                    Y = config.Y,
+                    Yaw = config.Yaw,
+                    Width = config.Width,
+                    Length = config.Length,
+                    MarkerSearchRequests = [.. config.MarkerEntries
+                        .OrderBy(m => m.Priority)
+                        .Select(m => new MarkerEntry
+                        {
+                            MarkerId = m.MarkerId ?? string.Empty,
+                            Type = (MarkerType)m.Type,
+                            Priority = m.Priority,
+                            DeviceId = m.DeviceId ?? string.Empty,
+                            Code = m.Code ?? string.Empty,
+                            ReferencePoints = DeserializeReferencePoints(m.ReferencePointsJson)
+                        })]
+                };
+                DetectSession = await MarkerDetector.CreateSessionAsync(MarkersSearchRequest);
+                var plcController = scope.ServiceProvider.GetRequiredService<IPlcController>();
+                IsHasLoad = plcController.SetHasLoadValue;
+                //var StateMachine = scope.ServiceProvider.GetRequiredService<RobotStateMachine>();
+                //StateMachine.Fire(RobotEventType.StartDocking);
+                await base.StartAction();
+            }
+        }
+        else
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = "Cannot get Marker Detection";
+        }
+    }
+
+    private static Vector2[] DeserializeReferencePoints(string? json)
+    {
+        if (string.IsNullOrWhiteSpace(json)) return [];
+        try { return JsonSerializer.Deserialize<Vector2[]>(json) ?? []; }
+        catch { return []; }
+    }
+
+    protected override Task StopAction()
+    {
+        Navigation?.CancelMovement();
+        return base.StopAction();
+    }
+
+    protected override Task CleanupAction()
+    {
+        DetectSession?.Dispose();
+        DetectSession = null;
+        MarkerDetector?.Dispose();
+        MarkerDetector = null;
+        //using var scope = ServiceProvider.CreateAsyncScope();
+        //var StateMachine = scope.ServiceProvider.GetRequiredService<RobotStateMachine>();
+        //StateMachine.Fire(RobotEventType.CompleteDocking);
+        return base.CleanupAction();
+    }
+
+    protected override Task ExecuteAction()
+    {
+        if (DetectSession is null)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = "Module Detect Marker is not existed";
+        }
+        else if (Navigation is null)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = "Module Navigation is not existed";
+        }
+        else
+        {
+            if (!IsFindedGoal) IsFindedGoal = DetectSession.Goal is not null;
+
+            if (CountTimeout++ > MAX_TIMEOUT / ActionInterval)
+            {
+                Navigation?.CancelMovement();
+                SetStatus(ActionEvent.FAILED);
+                ResultDescription = "Timeout action";
+            }
+            else if (!IsFindedGoal) base.ExecuteAction();
+            else
+            {
+                if (!IsStartDockTo)
+                {
+                    Navigation.DockTo(DetectSession, IsHasLoad, Direction);
+                    IsStartDockTo = true;
+                }
+                if (Navigation.State == NavigationState.Completed)
+                {
+                    SetStatus(ActionEvent.FINISHED);
+                    ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
+                }
+                else if (Navigation.State == NavigationState.Error)
+                {
+                    SetStatus(ActionEvent.FAILED);
+                    ResultDescription = $"Action Handle [{Type} - {Id}] is failed: Navigation Failed";
+                }
+            }
+        }
+        return base.ExecuteAction();
+    }
+
+    protected override void Initialize()
+    {
+        base.Initialize();
+
+        if (ActionParameters.Count > 0)
+        {
+            foreach (var parameterStore in ActionParameters)
+            {
+                if (!parameterStore.IsOptional)
+                {
+                    if (Action is null || Action.ActionParameters is null || !Action.ActionParameters.Any(a => a.Key == parameterStore.Key)) throw new ActionException($"Thiếu tham số bắt buộc '{parameterStore.Key}' cho action {Type}.");
+                }
+            }
+            Parameters = Action is null || Action.ActionParameters is null ? [] : Action.ActionParameters;
+        }
+
+        var stationPara = Parameters.FirstOrDefault(p => p.Key == "stationId") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'stationId'");
+        StationId = stationPara.Value;
+        var directionPara = Parameters.FirstOrDefault(p => p.Key == "direction");
+        Direction = directionPara is not null && Enum.TryParse<RobotDirection>(directionPara.Value, out var direction) ? direction : null;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotDropAction.cs

@@ -0,0 +1,90 @@
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Client.Pages;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Modules;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+[RobotAction(ActionType.DROP,
+            [ActionScope.INSTANT, ActionScope.NODE, ActionScope.EDGE],
+            [BlockingType.HARD],
+            "Hạ thấp pallet.",
+            "Robot đã hạ thấp pallet.")]
+public class RobotDropAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    private const int MAX_TIMEOUT = 60000;
+    private int CountTimeout = 0;
+
+    private ILoad? LoadManager;
+    private ILiftModule? LiftModule;
+    private IPlcController? PlcController;
+    private CancellationTokenSource CancellationToken = new();
+
+    protected override async Task StartAction()
+    {
+        try
+        {
+            LoadManager = ServiceProvider.GetRequiredService<ILoad>();
+            LiftModule = ServiceProvider.GetRequiredService<ILiftModule>();
+            PlcController = ServiceProvider.GetRequiredService<IPlcController>();
+
+            if (!LiftModule.IsReady)
+            {
+                SetStatus(ActionEvent.FAILED);
+                ResultDescription = "Lift module not ready";
+                return;
+            }
+
+            CancellationToken = new CancellationTokenSource();
+            PlcController.SetOperationState(OperationState.Lifting);
+            await LiftModule.LiftDownAsync(CancellationToken.Token);
+            CountTimeout = 0;
+            await base.StartAction();
+        }
+        catch (Exception ex)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = $"Drop Failed: {ex.Message}";
+        }
+    }
+
+    protected override Task StopAction()
+    {
+        CancellationToken.Cancel();
+        return base.StopAction();
+    }
+
+    protected override Task CleanupAction()
+    {
+        CancellationToken.Dispose();
+        PlcController?.SetOperationState(OperationState.None);
+        return base.CleanupAction();
+    }
+
+    protected override async Task ExecuteAction()
+    {
+        if (LiftModule is null)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = "Lift module not found";
+        }
+        else if (LiftModule.State == LiftModuleState.Error)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = "Lift module error";
+        }
+        else if (LiftModule.Position == LiftPosition.Bottom)
+        {
+            LoadManager?.ClearLoad();
+            SetStatus(ActionEvent.FINISHED);
+            ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? "Robot has dropped the load." : ActionAttribute.ResultDescription;
+        }
+        else if (CountTimeout++ > MAX_TIMEOUT / ActionInterval)
+        {
+            CancellationToken.Cancel();
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = "Timeout action";
+        }
+        await base.ExecuteAction();
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotFactsheetRequestAction.cs

@@ -0,0 +1,25 @@
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Interfaces;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+[RobotAction(ActionType.FACTSHEET_REQUEST,
+            [ActionScope.INSTANT, ActionScope.NODE, ActionScope.EDGE],
+            [BlockingType.NONE, BlockingType.SOFT, BlockingType.HARD],
+            "Yêu cầu gửi Factsheet robot ngay lập tức.",
+            "Robot đã gửi Factsheet ngay lập tức.")]
+public class RobotFactsheetRequestAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    protected override async Task StartAction()
+    {
+        var RobotFactsheet = ServiceProvider.GetRequiredService<IFactsheet>();
+        await RobotFactsheet.PubFactsheet();
+        SetStatus(ActionEvent.FINISHED);
+        ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
+    }
+
+    protected override Task ExecuteAction()
+    {
+        return base.ExecuteAction();
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotInitPositionAction.cs

@@ -0,0 +1,94 @@
+using RobotNet.VDA5050.Factsheet;
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Services.Exceptions;
+using System.Collections.ObjectModel;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+[RobotAction(ActionType.INIT_POSITION,
+            [ActionScope.INSTANT, ActionScope.NODE],
+            [BlockingType.HARD],
+            "Khởi tạo lại vị trí robot.",
+            "Robot đã khởi tạo lại vị trí.")]
+public class RobotInitPositionAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    private static readonly ReadOnlyCollection<ActionParameter> ActionParameters = new List<ActionParameter>
+    {
+        new()
+        {
+            Key = "x",
+            Description = "Tọa độ X của vị trí khởi tạo.",
+            ValueDataType = ValueDataType.FLOAT,
+            IsOptional = false,
+        },
+        new()
+        {
+            Key = "y",
+            Description = "Tọa độ Y của vị trí khởi tạo.",
+            ValueDataType = ValueDataType.FLOAT,
+            IsOptional = false,
+        },
+        new()
+        {
+            Key = "theta",
+            Description = "Góc quay (theta) của vị trí khởi tạo. (rad)",
+            ValueDataType = ValueDataType.FLOAT,
+            IsOptional = false,
+        }
+    }.AsReadOnly();
+
+    double X = 0;
+    double Y = 0;
+    double Theta = 0;
+    ILocalization? Localization;
+
+    protected override Task StartAction()
+    {
+        Localization = ServiceProvider.GetRequiredService<ILocalization>();
+        var initPose = Localization.SetInitializePosition(X, Y, Theta);
+        if (!initPose.IsSuccess)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = initPose.Message;
+        }
+        else
+        {
+            SetStatus(ActionEvent.FINISHED);
+            ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
+        }
+        return base.StartAction();
+    }
+
+    protected override void Initialize()
+    {
+        base.Initialize();
+
+        if (ActionParameters.Count > 0)
+        {
+            foreach (var parameterStore in ActionParameters)
+            {
+                if (!parameterStore.IsOptional)
+                {
+                    if (Action is null || Action.ActionParameters is null || !Action.ActionParameters.Any(a => a.Key == parameterStore.Key)) throw new ActionException($"Thiếu tham số bắt buộc '{parameterStore.Key}' cho action {Type}.");
+                }
+            }
+            Parameters = Action is null || Action.ActionParameters is null ? [] : Action.ActionParameters;
+        }
+
+        var xPara = Parameters.FirstOrDefault(p => p.Key == "x") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'x'");
+        var yPara = Parameters.FirstOrDefault(p => p.Key == "y") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'y'");
+        var thetaPara = Parameters.FirstOrDefault(p => p.Key == "theta") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'theta'");
+
+        var xParse = double.TryParse(xPara.Value, out double xData);
+        var yParse = double.TryParse(yPara.Value, out double yData);
+        var thetaParse = double.TryParse(thetaPara.Value, out double thetaData);
+
+        if (!xParse) throw new ActionException($"Action {Type} có parameter 'x' không đúng kiểu dữ liệu");
+        if (!yParse) throw new ActionException($"Action {Type} có parameter 'y' không đúng kiểu dữ liệu");
+        if (!thetaParse) throw new ActionException($"Action {Type} có parameter 'theta' không đúng kiểu dữ liệu");
+        X = xData;
+        Y = yData;
+        Theta = thetaData;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotLiftRotateAction.cs

@@ -0,0 +1,95 @@
+using RobotNet.VDA5050.Factsheet;
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Modules;
+using RobotNet10.RobotApp.Services.Exceptions;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+[RobotAction(ActionType.LIFT_ROTATE,
+            [ActionScope.INSTANT, ActionScope.NODE, ActionScope.EDGE],
+            [BlockingType.HARD],
+            "Xoay bàn nâng của robot.",
+            "Robot đã xoay bàn nâng.")]
+public class RobotLiftRotateAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    private static readonly IReadOnlyList<ActionParameter> ActionParameters = new List<ActionParameter>
+    {
+        new()
+            {
+                Key = "angle",
+                Description = "Góc xoay của bàn nâng. (rad)",
+                ValueDataType = ValueDataType.FLOAT,
+                IsOptional = false,
+            }
+    }.AsReadOnly();
+    private double Angle = 0; // Degree
+    private IRotationModule? RotationModule;
+    private IPlcController? PlcController;
+
+    private const int MAX_TIMEOUT = 60000 * 2;
+    private int CountTimeout = 0;
+
+    protected override Task StartAction()
+    {
+        RotationModule = ServiceProvider.GetRequiredService<IRotationModule>();
+        PlcController = ServiceProvider.GetRequiredService<IPlcController>();
+        PlcController.SetOperationState(OperationState.LiftRotating);
+        RotationModule.RotateToAngleAsync(Angle);
+        return base.StartAction();
+    }
+
+    protected override Task CleanupAction()
+    {
+        PlcController?.SetOperationState(OperationState.None);
+        return base.CleanupAction();
+    }
+
+    protected override async Task ExecuteAction()
+    {
+        if (RotationModule is null)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = "Không tìm thấy mô-đun xoay.";
+        }
+        else if (RotationModule.State == RotationModuleState.Error)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = "Mô-đun xoay gặp lỗi.";
+
+        }
+        else if (RotationModule.State == RotationModuleState.Ready && Math.Abs(await RotationModule.GetCurrentAngleAsync() - Angle) < 1)
+        {
+            SetStatus(ActionEvent.FINISHED);
+            ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
+        }
+        else if (CountTimeout++ >= MAX_TIMEOUT / ActionInterval)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = "Timeout action";
+        }
+        await base.ExecuteAction();
+    }
+
+    protected override void Initialize()
+    {
+        base.Initialize();
+
+        if (ActionParameters.Count > 0)
+        {
+            foreach (var parameterStore in ActionParameters)
+            {
+                if (!parameterStore.IsOptional)
+                {
+                    if (Action is null || Action.ActionParameters is null || !Action.ActionParameters.Any(a => a.Key == parameterStore.Key)) throw new ActionException($"Thiếu tham số bắt buộc '{parameterStore.Key}' cho action {Type}.");
+                }
+            }
+            Parameters = Action is null || Action.ActionParameters is null ? [] : Action.ActionParameters;
+        }
+
+        var anglePara = Parameters.FirstOrDefault(p => p.Key == "angle") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'angle'");
+        var angleParse = double.TryParse(anglePara.Value, out double angleData);
+        if (!angleParse) throw new ActionException($"Action {Type} có parameter 'angle' không đúng kiểu dữ liệu");
+        Angle = angleData * 180 / Math.PI;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotMoveStraightToCoorAction.cs

@@ -0,0 +1,128 @@
+using RobotNet.VDA5050.Factsheet;
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Services.Exceptions;
+using RobotNet10.RobotApp.Shared.Enums;
+using System.Collections.ObjectModel;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+[RobotAction(ActionType.MOVE_STRAIGHT_TO_COOR,
+            [ActionScope.INSTANT],
+            [BlockingType.HARD],
+            "Di chuyển thẳng đến tọa độ xác định.",
+            "Robot đã di chuyển thẳng đến tọa độ xác định.")]
+public class RobotMoveStraightToCoorAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    private static readonly ReadOnlyCollection<ActionParameter> ActionParameters = new List<ActionParameter>
+    {
+        new()
+        {
+            Key = "x",
+            Description = "Tọa độ X đích đến.",
+            ValueDataType = ValueDataType.FLOAT,
+            IsOptional = false,
+        },
+        new()
+        {
+            Key = "y",
+            Description = "Tọa độ Y đích đến.",
+            ValueDataType = ValueDataType.FLOAT,
+            IsOptional = false,
+        },
+        new()
+        {
+            Key = "direction",
+            Description = "Hướng di chuyển: FORWARD, BACKWARD",
+            ValueDataType = ValueDataType.STRING,
+            IsOptional = true,
+        },
+    }.AsReadOnly();
+
+    private INavigation? Navigation;
+    private double TargetX;
+    private double TargetY;
+    private RobotDirection? Direction = null;
+
+    private const int MAX_TIMEOUT = 60000 * 5;
+    private int CountTimeout = 0;
+    private bool IsStartMoveStraight = false;
+    private bool IsHasLoad = false;
+
+    protected override Task StartAction()
+    {
+        Navigation = ServiceProvider.GetRequiredService<INavigation>();
+        using var scope = ServiceProvider.CreateScope();
+        var plcController = scope.ServiceProvider.GetRequiredService<IPlcController>();
+        IsHasLoad = plcController.SetHasLoadValue;
+        return base.StartAction();
+    }
+
+    protected override Task StopAction()
+    {
+        Navigation?.CancelMovement();
+        return base.StopAction();
+    }
+
+    protected override Task ExecuteAction()
+    {
+        if (Navigation is null)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = "Module Navigation is not existed";
+        }
+        else
+        {
+            if (CountTimeout++ > MAX_TIMEOUT / ActionInterval)
+            {
+                Navigation?.CancelMovement();
+                SetStatus(ActionEvent.FAILED);
+                ResultDescription = "Timeout action";
+            }
+            else
+            {
+                if (!IsStartMoveStraight)
+                {
+                    Navigation.MoveStraight(TargetX, TargetY, IsHasLoad, Direction);
+                    IsStartMoveStraight = true;
+                }
+                if (Navigation.State == NavigationState.Completed)
+                {
+                    SetStatus(ActionEvent.FINISHED);
+                    ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
+                }
+                else if (Navigation.State == NavigationState.Error)
+                {
+                    SetStatus(ActionEvent.FAILED);
+                    ResultDescription = $"Action Handle [{Type} - {Id}] is failed: Navigation Failed";
+                }
+            }
+        }
+        return base.ExecuteAction();
+    }
+
+    protected override void Initialize()
+    {
+        base.Initialize();
+
+        if (ActionParameters.Count > 0)
+        {
+            foreach (var parameterStore in ActionParameters)
+            {
+                if (!parameterStore.IsOptional)
+                {
+                    if (Action is null || Action.ActionParameters is null || !Action.ActionParameters.Any(a => a.Key == parameterStore.Key))
+                        throw new ActionException($"Thiếu tham số bắt buộc '{parameterStore.Key}' cho action {Type}.");
+                }
+            }
+            Parameters = Action is null || Action.ActionParameters is null ? [] : Action.ActionParameters;
+        }
+
+        var xPara = Parameters.FirstOrDefault(p => p.Key == "x") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'x'");
+        var yPara = Parameters.FirstOrDefault(p => p.Key == "y") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'y'");
+        TargetX = double.Parse(xPara.Value);
+        TargetY = double.Parse(yPara.Value);
+        var directionPara = Parameters.FirstOrDefault(p => p.Key == "direction");
+        Direction = directionPara is not null && Enum.TryParse<RobotDirection>(directionPara.Value, out var direction) ? direction : null;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotMoveStraightWithDistanceAction.cs

@@ -0,0 +1,132 @@
+using RobotNet.VDA5050.Factsheet;
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Services.Exceptions;
+using RobotNet10.RobotApp.Shared.Enums;
+using System.Collections.ObjectModel;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+[RobotAction(ActionType.MOVE_STRAIGHT_WITH_DISTANCE,
+            [ActionScope.INSTANT],
+            [BlockingType.HARD],
+            "Di chuyển thẳng với khoảng cách xác định.",
+            "Robot đã di chuyển thẳng với khoảng cách xác định.")]
+public class RobotMoveStraightWithDistanceAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    private static readonly ReadOnlyCollection<ActionParameter> ActionParameters = new List<ActionParameter>
+    {
+        new()
+        {
+            Key = "distance",
+            Description = "Khoảng cách di chuyển. (m)",
+            ValueDataType = ValueDataType.FLOAT,
+            IsOptional = false,
+        },
+        new()
+        {
+            Key = "direction",
+            Description = "Hướng di chuyển: FORWARD, BACKWARD",
+            ValueDataType = ValueDataType.STRING,
+            IsOptional = true,
+        },
+        new()
+        {
+            Key = "angle",
+            Description = "Góc di chuyển so với hướng hiện tại của robot. (rad)",
+            ValueDataType = ValueDataType.FLOAT,
+            IsOptional = false,
+        }
+    }.AsReadOnly();
+
+    private INavigation? Navigation;
+    private ILocalization? Localization;
+    private double Distance;
+    private double Angle;
+    private RobotDirection? Direction = null;
+
+    private const int MAX_TIMEOUT = 60000 * 5;
+    private int CountTimeout = 0;
+    private bool IsStartMoveStraight = false;
+    private bool IsHasLoad = false;
+
+    protected override Task StartAction()
+    {
+        Navigation = ServiceProvider.GetRequiredService<INavigation>();
+        Localization = ServiceProvider.GetRequiredService<ILocalization>();
+        using var scope = ServiceProvider.CreateScope();
+        var plcController = scope.ServiceProvider.GetRequiredService<IPlcController>();
+        IsHasLoad = plcController.SetHasLoadValue;
+        return base.StartAction();
+    }
+
+    protected override Task StopAction()
+    {
+        Navigation?.CancelMovement();
+        return base.StopAction();
+    }
+
+    protected override Task ExecuteAction()
+    {
+        if (Navigation is null || Localization is null)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = "Module Navigation or Localization is not existed";
+        }
+        else
+        {
+            if (CountTimeout++ > MAX_TIMEOUT / ActionInterval)
+            {
+                Navigation?.CancelMovement();
+                SetStatus(ActionEvent.FAILED);
+                ResultDescription = "Timeout action";
+            }
+            else
+            {
+                if (!IsStartMoveStraight)
+                {
+                    double targetX = Localization.X + Distance * Math.Cos(Angle);
+                    double targetY = Localization.Y + Distance * Math.Sin(Angle);
+                    Navigation.MoveStraight(targetX, targetY, IsHasLoad, Direction);
+                    IsStartMoveStraight = true;
+                }
+                if (Navigation.State == NavigationState.Completed)
+                {
+                    SetStatus(ActionEvent.FINISHED);
+                    ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
+                }
+                else if (Navigation.State == NavigationState.Error)
+                {
+                    SetStatus(ActionEvent.FAILED);
+                    ResultDescription = $"Action Handle [{Type} - {Id}] is failed: Navigation Failed";
+                }
+            }
+        }
+        return base.ExecuteAction();
+    }
+
+    protected override void Initialize()
+    {
+        base.Initialize();
+
+        if (ActionParameters.Count > 0)
+        {
+            foreach (var parameterStore in ActionParameters)
+            {
+                if (!parameterStore.IsOptional)
+                {
+                    if (Action is null || Action.ActionParameters is null || !Action.ActionParameters.Any(a => a.Key == parameterStore.Key))
+                        throw new ActionException($"Thiếu tham số bắt buộc '{parameterStore.Key}' cho action {Type}.");
+                }
+            }
+            Parameters = Action is null || Action.ActionParameters is null ? [] : Action.ActionParameters;
+        }
+
+        var distancePara = Parameters.FirstOrDefault(p => p.Key == "distance") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'distance'");
+        Distance = double.Parse(distancePara.Value);
+        var anglePara = Parameters.FirstOrDefault(p => p.Key == "angle") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'angle'");
+        Angle = double.Parse(anglePara.Value);
+        var directionPara = Parameters.FirstOrDefault(p => p.Key == "direction");
+        Direction = directionPara is not null && Enum.TryParse<RobotDirection>(directionPara.Value, out var direction) ? direction : null;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotMutedBaseOffAction.cs

@@ -0,0 +1,39 @@
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Interfaces;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+
+[RobotAction(ActionType.MUTED_BASE_OFF,
+            [ActionScope.INSTANT, ActionScope.NODE, ActionScope.EDGE],
+            [BlockingType.NONE, BlockingType.SOFT, BlockingType.HARD],
+            "Tắt chế độ muted base robot.",
+            "Robot đã tắt chế độ muted base.")]
+public class RobotMutedBaseOffAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    private IPlcController? PlcController;
+
+    private const int MAX_TIMEOUT = 4000;
+    private int CountTimeout = 0;
+    protected override Task StartAction()
+    {
+        PlcController = ServiceProvider.GetRequiredService<IPlcController>();
+        PlcController.SetMutedBase(false);
+        return base.StartAction();
+    }
+
+    protected override Task ExecuteAction()
+    {
+        if (PlcController is not null && !PlcController.MutedBase)
+        {
+            SetStatus(ActionEvent.FINISHED);
+            ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
+        }
+        else if (CountTimeout++ > MAX_TIMEOUT / ActionInterval)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = "Timeout action";
+        }
+        return base.ExecuteAction();
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotMutedBaseOnAction.cs

@@ -0,0 +1,38 @@
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Interfaces;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+[RobotAction(ActionType.MUTED_BASE_ON,
+            [ActionScope.INSTANT, ActionScope.NODE, ActionScope.EDGE],
+            [BlockingType.NONE, BlockingType.SOFT, BlockingType.HARD],
+            "Bật chế độ muted base robot.",
+            "Robot đã bật chế độ muted base.")]
+public class RobotMutedBaseOnAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    private IPlcController? PlcController;
+
+    private const int MAX_TIMEOUT = 4000;
+    private int CountTimeout = 0;
+    protected override Task StartAction()
+    {
+        PlcController = ServiceProvider.GetRequiredService<IPlcController>();
+        PlcController.SetMutedBase(true);
+        return base.StartAction();
+    }
+
+    protected override Task ExecuteAction()
+    {
+        if(PlcController is not null && PlcController.MutedBase)
+        {
+            SetStatus(ActionEvent.FINISHED);
+            ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
+        }
+        else if (CountTimeout++ > MAX_TIMEOUT / ActionInterval)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = "Timeout action";
+        }
+        return base.ExecuteAction();
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotMutedLoadOffAction.cs

@@ -0,0 +1,37 @@
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Interfaces;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+[RobotAction(ActionType.MUTED_LOAD_OFF,
+            [ActionScope.INSTANT, ActionScope.NODE, ActionScope.EDGE],
+            [BlockingType.NONE, BlockingType.SOFT, BlockingType.HARD],
+            "Tắt chế độ muted load robot.",
+            "Robot đã tắt chế độ muted load.")]
+public class RobotMutedLoadOffAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    private IPlcController? PlcController;
+    private const int MAX_TIMEOUT = 4000;
+    private int CountTimeout = 0;
+    protected override Task StartAction()
+    {
+        PlcController = ServiceProvider.GetRequiredService<IPlcController>();
+        PlcController.SetMutedLoad(false);
+        return base.StartAction();
+    }
+
+    protected override Task ExecuteAction()
+    {
+        if (PlcController is not null && !PlcController.MutedLoad)
+        {
+            SetStatus(ActionEvent.FINISHED);
+            ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
+        }
+        else if (CountTimeout++ > MAX_TIMEOUT / ActionInterval)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = "Timeout action";
+        }
+        return base.ExecuteAction();
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotMutedLoadOnAction.cs

@@ -0,0 +1,38 @@
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Interfaces;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+[RobotAction(ActionType.MUTED_LOAD_ON,
+            [ActionScope.INSTANT, ActionScope.NODE, ActionScope.EDGE],
+            [BlockingType.NONE, BlockingType.SOFT, BlockingType.HARD],
+            "Bật chế độ muted load robot.",
+            "Robot đã bật chế độ muted load.")]
+public class RobotMutedLoadOnAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    private IPlcController? PlcController;
+
+    private const int MAX_TIMEOUT = 4000;
+    private int CountTimeout = 0;
+    protected override Task StartAction()
+    {
+        PlcController = ServiceProvider.GetRequiredService<IPlcController>();
+        PlcController.SetMutedLoad(true);
+        return base.StartAction();
+    }
+
+    protected override Task ExecuteAction()
+    {
+        if (PlcController is not null && PlcController.MutedLoad)
+        {
+            SetStatus(ActionEvent.FINISHED);
+            ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
+        }
+        else if (CountTimeout++ > MAX_TIMEOUT / ActionInterval)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = "Timeout action";
+        }
+        return base.ExecuteAction();
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotPickAction.cs

@@ -0,0 +1,89 @@
+using NLog;
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Client.Pages;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Modules;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+[RobotAction(ActionType.PICK,
+            [ActionScope.INSTANT, ActionScope.NODE, ActionScope.EDGE],
+            [BlockingType.HARD],
+            "Nâng cao pallet.",
+            "Robot đã nâng cao pallet.")]
+public class RobotPickAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    private const int MAX_TIMEOUT = 60000;
+    private int CountTimeout = 0;
+    private ILoad? LoadManager;
+    private ILiftModule? LiftModule;
+    private IPlcController? PlcController;
+    private CancellationTokenSource CancellationToken = new();
+    protected override async Task StartAction()
+    {
+        try
+        {
+            LoadManager = ServiceProvider.GetRequiredService<ILoad>();
+            LiftModule = ServiceProvider.GetRequiredService<ILiftModule>();
+            PlcController = ServiceProvider.GetRequiredService<IPlcController>();
+
+            if (!LiftModule.IsReady)
+            {
+                SetStatus(ActionEvent.FAILED);
+                ResultDescription = "Lift module not ready";
+                return;
+            }
+
+            CancellationToken = new CancellationTokenSource();
+            PlcController.SetOperationState(OperationState.Lifting);
+            _ = LiftModule.LiftUpAsync(CancellationToken.Token);
+            CountTimeout = 0;
+            await base.StartAction();
+        }
+        catch (Exception ex)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = "Pick failed: " + ex.Message;
+        }
+    }
+
+    protected override Task StopAction()
+    {
+        CancellationToken.Cancel();
+        return base.StopAction();
+    }
+
+    protected override Task CleanupAction()
+    {
+        CancellationToken.Dispose();
+        PlcController?.SetOperationState(OperationState.None);
+        return base.CleanupAction();
+    }
+
+    protected override async Task ExecuteAction()
+    {
+        if (LiftModule is null)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = "Lift module not found";
+        }
+        else if (LiftModule.State == LiftModuleState.Error)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = "Lift module error";
+        }
+        else if (LiftModule.Position == LiftPosition.Top)
+        {
+            LoadManager?.AddLoad(new());
+            SetStatus(ActionEvent.FINISHED);
+            ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? "Robot has picked up the load." : ActionAttribute.ResultDescription;
+        }
+        else if (CountTimeout++ > MAX_TIMEOUT / ActionInterval)
+        {
+            CancellationToken.Cancel();
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = "Timeout action";
+        }
+        await base.ExecuteAction();
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotRotateAction.cs

@@ -0,0 +1,96 @@
+using MudBlazor.Extensions;
+using RobotNet.VDA5050.Factsheet;
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Services.Exceptions;
+using System.Collections.ObjectModel;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+[RobotAction(ActionType.ROTATE,
+            [ActionScope.INSTANT, ActionScope.NODE, ActionScope.EDGE],
+            [BlockingType.HARD],
+            "Xoay robot tại chỗ.",
+            "Robot đã xoay tại chỗ.")]
+public class RobotRotateAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    private static readonly ReadOnlyCollection<ActionParameter> ActionParameters = new List<ActionParameter>
+    {
+        new()
+            {
+                Key = "angle",
+                Description = "Góc xoay của robot. (rad)",
+                ValueDataType = ValueDataType.FLOAT,
+                IsOptional = false,
+            }
+    }.AsReadOnly();
+
+    private double Angle = 0;
+    private INavigation? RobotNavigation;
+    private ILocalization? Localization;
+
+    private const int MAX_TIMEOUT = 60000 * 2;
+    private int CountTimeout = 0;
+    protected override Task StartAction()
+    {
+        RobotNavigation = ServiceProvider.GetRequiredService<INavigation>();
+        Localization = ServiceProvider.GetRequiredService<ILocalization>();
+        RobotNavigation.Rotate(Angle);
+        CountTimeout = 0;
+        return base.StartAction();
+    }
+
+    protected override Task StopAction()
+    {
+        RobotNavigation?.CancelMovement();
+        return base.StopAction();
+    }
+
+    protected override Task ExecuteAction()
+    {
+        if (RobotNavigation is null)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = "Module Navigation is not existed";
+        }
+        else if (RobotNavigation.State == NavigationState.Completed && Localization is not null && Math.Abs(Localization.Theta - Angle) * 180 / Math.PI < 5)
+        {
+            SetStatus(ActionEvent.FINISHED);
+            ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
+        }
+        else if (RobotNavigation.State == NavigationState.Error)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = $"Action Handle [{Type} - {Id}] - angle: {Angle} is failed";
+        }
+        else if (CountTimeout++ > MAX_TIMEOUT / ActionInterval)
+        {
+            RobotNavigation?.CancelMovement();
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = "Timeout action";
+        }
+        return base.ExecuteAction();
+    }
+
+    protected override void Initialize()
+    {
+        base.Initialize();
+
+        if (ActionParameters.Count > 0)
+        {
+            foreach (var parameterStore in ActionParameters)
+            {
+                if (!parameterStore.IsOptional)
+                {
+                    if (Action is null || Action.ActionParameters is null || !Action.ActionParameters.Any(a => a.Key == parameterStore.Key)) throw new ActionException($"Thiếu tham số bắt buộc '{parameterStore.Key}' cho action {Type}.");
+                }
+            }
+            Parameters = Action is null || Action.ActionParameters is null ? [] : Action.ActionParameters;
+        }
+
+        var anglePara = Parameters.FirstOrDefault(p => p.Key == "angle") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'angle'");
+        var angleParse = double.TryParse(anglePara.Value, out double angleData);
+        if (!angleParse) throw new ActionException($"Action {Type} có parameter 'angle' không đúng kiểu dữ liệu");
+        Angle = angleData;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotRotateKeepLift.cs

@@ -0,0 +1,59 @@
+using RobotNet.VDA5050.Factsheet;
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Services.Exceptions;
+using System.Collections.ObjectModel;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+[RobotAction(ActionType.ROTATE_KEEP_LIFT,
+            [ActionScope.INSTANT, ActionScope.NODE, ActionScope.EDGE],
+            [BlockingType.HARD],
+            "Xoay robot tại chỗ giữ nguyên trạng thái bàn nâng.",
+            "Robot đã xoay tại chỗ giữ nguyên trạng thái bàn nâng.")]
+public class RobotRotateKeepLift(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    private static readonly ReadOnlyCollection<ActionParameter> ActionParameters = new List<ActionParameter>
+    {
+        new()
+            {
+                Key = "angle",
+                Description = "Góc xoay của robot. (rad)",
+                ValueDataType = ValueDataType.FLOAT,
+                IsOptional = false,
+            }
+    }.AsReadOnly();
+    double Angle = 0;
+    protected override Task StartAction()
+    {
+        SetStatus(ActionEvent.FINISHED);
+        ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
+        return base.StartAction();
+    }
+
+    protected override Task ExecuteAction()
+    {
+        return base.ExecuteAction();
+    }
+
+    protected override void Initialize()
+    {
+        base.Initialize();
+
+        if (ActionParameters.Count > 0)
+        {
+            foreach (var parameterStore in ActionParameters)
+            {
+                if (!parameterStore.IsOptional)
+                {
+                    if (Action is null || Action.ActionParameters is null || !Action.ActionParameters.Any(a => a.Key == parameterStore.Key)) throw new ActionException($"Thiếu tham số bắt buộc '{parameterStore.Key}' cho action {Type}.");
+                }
+            }
+            Parameters = Action is null || Action.ActionParameters is null ? [] : Action.ActionParameters;
+        }
+
+        var anglePara = Parameters.FirstOrDefault(p => p.Key == "angle") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'angle'");
+        var angleParse = double.TryParse(anglePara.Value, out double angleData);
+        if (!angleParse) throw new ActionException($"Action {Type} có parameter 'angle' không đúng kiểu dữ liệu");
+        Angle = angleData;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotScriptAction.cs

@@ -0,0 +1,57 @@
+using RobotNet.VDA5050.Factsheet;
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Services.Exceptions;
+using System.Collections.ObjectModel;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+[RobotAction(ActionType.SCRIPT,
+            [ActionScope.INSTANT, ActionScope.NODE, ActionScope.EDGE],
+            [BlockingType.NONE, BlockingType.SOFT, BlockingType.HARD],
+            "This is an script robot action.",
+            "Script robot action completed.")]
+public class RobotScriptAction(IServiceProvider serviceProvider) : RobotAction(serviceProvider)
+{
+    private static readonly ReadOnlyCollection<ActionParameter> ActionParameters = new List<ActionParameter>
+    {
+        new() {
+            Key = "missionName",
+            ValueDataType = ValueDataType.STRING,
+            Description = "This is an mission name of script mission.",
+            IsOptional = true
+        },
+    }.AsReadOnly();
+
+    private string Name = "";
+    protected override void Initialize()
+    {
+        base.Initialize();
+
+        if (ActionParameters.Count > 0)
+        {
+            foreach (var parameterStore in ActionParameters)
+            {
+                if (!parameterStore.IsOptional)
+                {
+                    if (Action is null || Action.ActionParameters is null || !Action.ActionParameters.Any(a => a.Key == parameterStore.Key)) throw new ActionException($"Thiếu tham số bắt buộc '{parameterStore.Key}' cho action {Type}.");
+                }
+            }
+            Parameters = Action is null || Action.ActionParameters is null ? [] : Action.ActionParameters;
+        }
+
+        var para = Parameters.FirstOrDefault(p => p.Key == "missionName") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'missionName'");
+        if(string.IsNullOrEmpty(para.Value)) throw new ActionException($"Action {Type}, parameter 'missionName' có value rỗng");
+        Name = para.Value;
+    }
+
+    protected override Task StartAction()
+    {
+        return base.StartAction();
+    }
+
+    protected override Task ExecuteAction()
+    {
+        return base.ExecuteAction();
+    }
+
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotStartChargingAction.cs

@@ -0,0 +1,23 @@
+using RobotNet.VDA5050.Type;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+[RobotAction(ActionType.START_CHARGING,
+            [ActionScope.INSTANT, ActionScope.NODE],
+            [BlockingType.HARD],
+            "Bắt đầu quá trình sạc pin.",
+            "Robot đã bắt đầu sạc pin.")]
+public class RobotStartChargingAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    protected override Task StartAction()
+    {
+        SetStatus(ActionEvent.FINISHED);
+        ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
+        return base.StartAction();
+    }
+
+    protected override Task ExecuteAction()
+    {
+        return base.ExecuteAction();
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotStartPauseAction.cs

@@ -0,0 +1,26 @@
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Interfaces;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+[RobotAction(ActionType.START_PAUSE,
+            [ActionScope.INSTANT],
+            [BlockingType.NONE, BlockingType.SOFT, BlockingType.HARD],
+            "Tam dừng robot.",
+            "Robot đã tạm dừng.")]
+public class RobotStartPauseAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    protected override Task StartAction()
+    {
+        var RobotController = ServiceProvider.GetRequiredService<IRobotController>();
+        RobotController.Pause();
+        SetStatus(ActionEvent.FINISHED);
+        ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
+        return base.StartAction();
+    }
+
+    protected override Task ExecuteAction()
+    {
+        return base.ExecuteAction();
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotStateRequestAction.cs

@@ -0,0 +1,25 @@
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Interfaces;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+[RobotAction(ActionType.STATE_REQUEST,
+            [ActionScope.INSTANT, ActionScope.NODE, ActionScope.EDGE],
+            [BlockingType.NONE, BlockingType.SOFT, BlockingType.HARD],
+            "Yêu cầu gửi trạng thái robot ngay lập tức.",
+            "Robot đã gửi trạng thái ngay lập tức.")]
+public class RobotStateRequestAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    protected override async Task StartAction()
+    {
+        var RobotStates = ServiceProvider.GetRequiredService<IState>();
+        await RobotStates.PubState();
+        SetStatus(ActionEvent.FINISHED);
+        ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
+    }
+
+    protected override Task ExecuteAction()
+    {
+        return base.ExecuteAction();
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotStopChargingAction.cs

@@ -0,0 +1,23 @@
+using RobotNet.VDA5050.Type;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+[RobotAction(ActionType.STOP_CHARGING,
+            [ActionScope.INSTANT, ActionScope.NODE],
+            [BlockingType.HARD],
+            "Kết thúc quá trình sạc pin.",
+            "Robot đã kết thúc sạc pin.")]
+public class RobotStopChargingAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    protected override Task StartAction()
+    {
+        SetStatus(ActionEvent.FINISHED);
+        ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
+        return base.StartAction();
+    }
+
+    protected override Task ExecuteAction()
+    {
+        return base.ExecuteAction();
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotStopPauseAction.cs

@@ -0,0 +1,26 @@
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Interfaces;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+[RobotAction(ActionType.STOP_PAUSE,
+            [ActionScope.INSTANT],
+            [BlockingType.NONE, BlockingType.SOFT, BlockingType.HARD],
+            "Tiếp tục hoạt động robot sau khi tạm dừng.",
+            "Robot đã tiếp tục hoạt động.")]
+public class RobotStopPauseAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    protected override Task StartAction()
+    {
+        var RobotController = ServiceProvider.GetRequiredService<IRobotController>();
+        RobotController.Resume();
+        SetStatus(ActionEvent.FINISHED);
+        ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
+        return base.StartAction();
+    }
+
+    protected override Task ExecuteAction()
+    {
+        return base.ExecuteAction();
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Connection/IRobotConnectionsService.cs

@@ -0,0 +1,62 @@
+using RobotNet.VDA5050.Connection;
+using RobotNet.VDA5050.Factsheet;
+using RobotNet.VDA5050.InstantAction;
+using RobotNet.VDA5050.State;
+using RobotNet.VDA5050.Type;
+using RobotNet.VDA5050.Visualization;
+
+namespace RobotNet10.RobotApp.Services.Robot.Connection;
+
+/// <summary>
+/// Service interface for managing MQTT connections to robots via VDA5050 protocol
+/// </summary>
+public interface IRobotConnectionsService
+{
+    /// <summary>
+    /// Start MQTT connection and subscribe to topics
+    /// </summary>
+    Task StartAsync(CancellationToken? cancellationToken);
+
+    /// <summary>
+    /// Stop MQTT connection
+    /// </summary>
+    Task StopAsync();
+
+    /// <summary>
+    /// Check if MQTT client is connected
+    /// </summary>
+    bool IsConnected { get; }
+
+    /// <summary>
+    /// Publish order message from robot
+    /// </summary>
+    Task<bool> PublishStateAsync(StateMsg state, CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Publish visualization message from robot
+    /// </summary>
+    Task<bool> PublishVisualizationAsync(VisualizationMsg visualization, CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Publish factsheet message from robot
+    /// </summary>
+    /// <param name="factsheet"></param>
+    /// <param name="cancellationToken"></param>
+    /// <returns></returns>
+    Task<bool> PublishFactsheetAsync(FactSheetMsg factsheet, CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Publish connection message from robot
+    /// </summary>
+    /// <param name="connection"></param>
+    /// <param name="cancellationToken"></param>
+    /// <returns></returns>
+    Task<bool> PublishConnectionAsync(ConnectionMsg connection, CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Publish Connection state from robot 
+    /// </summary>
+    /// <param name="state"></param>
+    /// <returns></returns>
+    Task PublishConnectionStateAsync(ConnectionState state);
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Connection/Models/VDA5050ProtocolConfig.cs

@@ -0,0 +1,12 @@
+namespace RobotNet10.RobotApp.Services.Robot.Connection.Models;
+
+/// <summary>
+/// VDA5050 Protocol configuration
+/// </summary>
+public class VDA5050ProtocolConfig
+{
+    public string Manufacturer { get; set; } = string.Empty;
+    public string Version { get; set; } = string.Empty;
+    public string TopicPrefix { get; set; } = string.Empty;
+    public string SerialNumber { get; set; } = string.Empty;
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Connection/RobotConnectionsService.cs

@@ -0,0 +1,424 @@
+using MQTTnet;
+using MQTTnet.Packets;
+using RobotNet.VDA5050;
+using RobotNet.VDA5050.Connection;
+using RobotNet.VDA5050.Factsheet;
+using RobotNet.VDA5050.InstantAction;
+using RobotNet.VDA5050.Order;
+using RobotNet.VDA5050.State;
+using RobotNet.VDA5050.Type;
+using RobotNet.VDA5050.Visualization;
+using RobotNet10.MqttConnection;
+using RobotNet10.RobotApp.Events;
+using RobotNet10.RobotApp.Services.ConfigManager;
+using System.Text;
+using System.Text.Json;
+
+namespace RobotNet10.RobotApp.Services.Robot.Connection;
+
+/// <summary>
+/// Service implementation for managing MQTT connections to robots via VDA5050 protocol
+/// </summary>
+public class RobotConnectionsService(
+    IConnectionConfig configManager,
+    IRobotEventBus eventBus,
+    IServiceProvider serviceProvider,
+    Logger<RobotConnectionsService> logger,
+    ILogger<MQTTClient> mqttLogger) : IRobotConnectionsService
+{
+    private readonly IConnectionConfig _configManager = configManager;
+    private readonly IRobotEventBus _eventBus = eventBus;
+    private readonly IServiceProvider _serviceProvider = serviceProvider;
+    private readonly Logger<RobotConnectionsService> _logger = logger;
+    private readonly ILogger<MQTTClient> _mqttLogger = mqttLogger;
+
+    private MQTTClient? _mqttClient;
+    private readonly SemaphoreSlim _connectionSemaphore = new(1, 1);
+
+    public bool IsConnected => _mqttClient is not null && _mqttClient.IsConnected;
+
+    public async Task StartAsync(CancellationToken? cancellationToken)
+    {
+        if (!_connectionSemaphore.Wait(1000)) return;
+        try
+        {
+            if (IsConnected) return;
+
+            await StopAsync();
+
+            var mqttConfig = _configManager.GetMqttConfig();
+            var vdaConfig = _configManager.GetVDA5050Config();
+
+            MqttTopicFilter[] topics = [
+                new MqttTopicFilterBuilder().WithTopic($"{vdaConfig.TopicPrefix}/{vdaConfig.Manufacturer}/+/{VDA5050Topic.ORDER.ToJsonString()}")
+                .WithAtMostOnceQoS()
+                .Build(),
+                new MqttTopicFilterBuilder().WithTopic($"{vdaConfig.TopicPrefix}/{vdaConfig.Manufacturer}/+/{VDA5050Topic.INSTANTACTIONS.ToJsonString()}")
+                .WithAtMostOnceQoS()
+                .Build()
+            ];
+
+            _mqttClient = new MQTTClient(mqttConfig, topics, _mqttLogger);
+            _mqttClient.MessageUpdated += MessageUpdated;
+            if (_mqttClient is not null) await _mqttClient.ConnectAsync(cancellationToken);
+            if (_mqttClient is not null) await _mqttClient.SubscribeAsync(cancellationToken);
+
+            // Publish ONLINE once broker connection and subscriptions are ready.
+            await PublishConnectionStateAsync(ConnectionState.ONLINE);
+
+            _logger.Info("RobotConnectionsService started successfully");
+        }
+        catch (Exception ex)
+        {
+            _logger.Warning($"Connection broker is failed: {ex.Message}");
+        }
+        finally
+        {
+            _connectionSemaphore.Release();
+        }
+    }
+
+    public async Task StopAsync()
+    {
+        if (_mqttClient is not null)
+        {
+            await _mqttClient.DisposeAsync();
+            _mqttClient = null;
+            _logger.Info("RobotConnectionsService stopped");
+        }
+    }
+
+    private async Task MessageUpdated(MqttApplicationMessageReceivedEventArgs e)
+    {
+        try
+        {
+            var topic = e.ApplicationMessage.Topic;
+            var payload = Encoding.UTF8.GetString(e.ApplicationMessage.Payload);
+            var (robotId, messageType) = ParseVDA5050Topic(topic);
+            if (!string.IsNullOrEmpty(robotId) && !string.IsNullOrEmpty(messageType))
+            {
+                var vdaConfig = _configManager.GetVDA5050Config();
+                if (robotId == vdaConfig.SerialNumber)
+                {
+                    if (messageType == VDA5050Topic.ORDER.ToJsonString())
+                    {
+                        HandleOrderMessageAsync(payload);
+                    }
+                    else if (messageType == VDA5050Topic.INSTANTACTIONS.ToJsonString())
+                    {
+                        HandleInstantActionMessageAsync(payload);
+                    }
+                }
+            }
+            else
+            {
+                _logger.Warning("Failed to parse topic");
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger.Warning($"Error processing message: {ex.Message}");
+        }
+    }
+
+    private (string? robotId, string? messageType) ParseVDA5050Topic(string topic)
+    {
+        try
+        {
+            if (string.IsNullOrEmpty(topic)) return (null, null);
+
+            var vdaConfig = _configManager.GetVDA5050Config();
+            ReadOnlySpan<char> topicSpan = topic.AsSpan();
+            var manufacturerSpan = $"/{vdaConfig.Manufacturer}/".AsSpan();
+            int manufacturerIndex = topicSpan.IndexOf(manufacturerSpan);
+
+            if (manufacturerIndex == -1) return (null, null);
+
+            var remaining = topicSpan[(manufacturerIndex + manufacturerSpan.Length)..];
+            int firstSlash = remaining.IndexOf('/');
+            if (firstSlash == -1) return (null, null);
+
+            var robotId = remaining[..firstSlash].ToString();
+            var messageType = remaining[(firstSlash + 1)..].ToString();
+
+            return (robotId, messageType);
+        }
+        catch (Exception ex)
+        {
+            _logger.Warning($"Parse VDA5050 Topic failed: {ex}");
+            return (null, null);
+        }
+    }
+
+    private void HandleOrderMessageAsync(string payload)
+    {
+        try
+        {
+            var orderMsg = JsonSerializer.Deserialize<OrderMsg>(payload, JsonOptionExtends.Read);
+            if (orderMsg is null) return;
+            _eventBus.PublishOrderMessageReceived(orderMsg);
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error handling order message: {ex.Message}");
+        }
+    }
+
+    private void HandleInstantActionMessageAsync(string payload)
+    {
+        try
+        {
+            var instantActionMsg = JsonSerializer.Deserialize<InstantActionsMsg>(payload, JsonOptionExtends.Read);
+            if (instantActionMsg is null) return;
+
+            _eventBus.PublishInstantActionMessageReceived(instantActionMsg);
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error handling instant action message: {ex.Message}");
+        }
+    }
+
+    private string BuildPublishTopic(string robotId, VDA5050Topic topic)
+    {
+        var vdaConfig = _configManager.GetVDA5050Config();
+        return $"{vdaConfig.TopicPrefix}/{vdaConfig.Manufacturer}/{robotId}/{topic.ToJsonString()}";
+    }
+
+    private async Task<bool> EnsureMqttClientReadyAsync(CancellationToken cancellationToken = default)
+    {
+        if (_mqttClient is not null && IsConnected)
+        {
+            return true;
+        }
+
+        // Startup can publish before the async connection task finishes.
+        _logger.Info("Mqtt Client not initialized yet, attempting to connect...");
+        await StartAsync(cancellationToken);
+
+        return _mqttClient is not null && IsConnected;
+    }
+
+    public async Task<bool> PublishStateAsync(StateMsg state, CancellationToken cancellationToken = default)
+    {
+        if (!await EnsureMqttClientReadyAsync(cancellationToken))
+        {
+            _logger.Warning("Cannot publish state: MQTT client is not initialized");
+            return false;
+        }
+
+        if (state == null)
+        {
+            _logger.Warning("Cannot publish state: state message is null");
+            return false;
+        }
+
+        if (string.IsNullOrEmpty(state.SerialNumber))
+        {
+            _logger.Warning("Cannot publish state: SerialNumber is null or empty");
+            return false;
+        }
+
+        var vdaConfig = _configManager.GetVDA5050Config();
+        if (state.SerialNumber != vdaConfig.SerialNumber)
+        {
+            _logger.Warning("Cannot publish state: state.SerialNumber is diffirent SerialNumber setting");
+            return false;
+        }
+
+        if (!IsConnected)
+        {
+            _logger.Warning($"Cannot publish state: MQTT client is not connected");
+            return false;
+        }
+
+        try
+        {
+            var topic = BuildPublishTopic(state.SerialNumber, VDA5050Topic.STATE);
+            var data = JsonSerializer.Serialize(state, JsonOptionExtends.Write);
+            await _mqttClient.PublishAsync(topic, data);
+            return true;
+        }
+        catch (OperationCanceledException)
+        {
+            _logger.Warning($"Publish state  was cancelled");
+            return false;
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error publishing state: {ex.Message}");
+            return false;
+        }
+    }
+
+    public async Task<bool> PublishVisualizationAsync(VisualizationMsg visualization, CancellationToken cancellationToken = default)
+    {
+        if (!await EnsureMqttClientReadyAsync(cancellationToken))
+        {
+            _logger.Warning("Cannot publish visualization: MQTT client is not initialized");
+            return false;
+        }
+
+        if (visualization == null)
+        {
+            _logger.Warning("Cannot publish visualization: visualization message is null");
+            return false;
+        }
+
+        if (string.IsNullOrEmpty(visualization.SerialNumber))
+        {
+            _logger.Warning("Cannot publish visualization: SerialNumber is null or empty");
+            return false;
+        }
+
+        var vdaConfig = _configManager.GetVDA5050Config();
+        if (visualization.SerialNumber != vdaConfig.SerialNumber)
+        {
+            _logger.Warning("Cannot publish visualization: visualization.SerialNumber is diffirent SerialNumber setting");
+            return false;
+        }
+
+        if (!IsConnected)
+        {
+            _logger.Warning($"Cannot publish visualization: MQTT client is not connected");
+            return false;
+        }
+
+        try
+        {
+            var topic = BuildPublishTopic(visualization.SerialNumber, VDA5050Topic.VISUALIZATION);
+            var data = JsonSerializer.Serialize(visualization, JsonOptionExtends.Write);
+            await _mqttClient.PublishAsync(topic, data);
+            return true;
+        }
+        catch (OperationCanceledException)
+        {
+            _logger.Warning($"Publish visualization  was cancelled");
+            return false;
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error publishing visualization: {ex.Message}");
+            return false;
+        }
+    }
+
+    public async Task<bool> PublishFactsheetAsync(FactSheetMsg factsheet, CancellationToken cancellationToken = default)
+    {
+        if (!await EnsureMqttClientReadyAsync(cancellationToken))
+        {
+            _logger.Warning("Cannot publish factsheet: MQTT client is not initialized");
+            return false;
+        }
+
+        if (factsheet == null)
+        {
+            _logger.Warning("Cannot publish factsheet: factsheet message is null");
+            return false;
+        }
+
+        if (string.IsNullOrEmpty(factsheet.SerialNumber))
+        {
+            _logger.Warning("Cannot publish factsheet: SerialNumber is null or empty");
+            return false;
+        }
+
+        var vdaConfig = _configManager.GetVDA5050Config();
+        if (factsheet.SerialNumber != vdaConfig.SerialNumber)
+        {
+            _logger.Warning("Cannot publish factsheet: factsheet.SerialNumber is diffirent SerialNumber setting");
+            return false;
+        }
+
+        if (!IsConnected)
+        {
+            _logger.Warning($"Cannot publish factsheet: MQTT client is not connected");
+            return false;
+        }
+
+        try
+        {
+            var topic = BuildPublishTopic(factsheet.SerialNumber, VDA5050Topic.FACTSHEET);
+            var data = JsonSerializer.Serialize(factsheet, JsonOptionExtends.Write);
+            await _mqttClient.PublishAsync(topic, data);
+            return true;
+        }
+        catch (OperationCanceledException)
+        {
+            _logger.Warning($"Publish factsheet  was cancelled");
+            return false;
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error publishing factsheet: {ex.Message}");
+            return false;
+        }
+    }
+
+    public async Task<bool> PublishConnectionAsync(ConnectionMsg connection, CancellationToken cancellationToken = default)
+    {
+        if (!await EnsureMqttClientReadyAsync(cancellationToken))
+        {
+            _logger.Warning("Cannot publish connection: MQTT client is not initialized");
+            return false;
+        }
+
+        if (connection == null)
+        {
+            _logger.Warning("Cannot publish connection: connection message is null");
+            return false;
+        }
+
+        if (string.IsNullOrEmpty(connection.SerialNumber))
+        {
+            _logger.Warning("Cannot publish connection: SerialNumber is null or empty");
+            return false;
+        }
+
+        var vdaConfig = _configManager.GetVDA5050Config();
+        if (connection.SerialNumber != vdaConfig.SerialNumber)
+        {
+            _logger.Warning("Cannot publish connection: connection.SerialNumber is diffirent SerialNumber setting");
+            return false;
+        }
+
+        if (!IsConnected)
+        {
+            _logger.Warning($"Cannot publish connection: MQTT client is not connected");
+            return false;
+        }
+
+        try
+        {
+            var topic = BuildPublishTopic(connection.SerialNumber, VDA5050Topic.CONNECTION);
+            var data = JsonSerializer.Serialize(connection, JsonOptionExtends.Write);
+            await _mqttClient.PublishAsync(topic, data, retain: true);
+            return true;
+        }
+        catch (OperationCanceledException)
+        {
+            _logger.Warning($"Publish connection  was cancelled");
+            return false;
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error publishing connection: {ex.Message}");
+            return false;
+        }
+    }
+
+    public async Task PublishConnectionStateAsync(ConnectionState state)
+    {
+        var vdaConfig = _configManager.GetVDA5050Config();
+        var connectionMsg = new ConnectionMsg
+        {
+            HeaderId = 1,
+            SerialNumber = vdaConfig.SerialNumber,
+            Timestamp = DateTime.Now,
+            Manufacturer = vdaConfig.Manufacturer,
+            ConnectionState = state,
+            Version = vdaConfig.Version
+        };
+        await PublishConnectionAsync(connectionMsg);
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Helper/ActionConflictDetector.cs

@@ -0,0 +1,280 @@
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Services.Robot.Actions;
+
+namespace RobotNet10.RobotApp.Services.Robot.Helper;
+
+/// <summary>
+/// Detects conflicts between actions according to VDA5050
+/// </summary>
+public class ActionConflictDetector
+{
+    // VDA5050: Counter-action pairs that conflict
+    private static readonly Dictionary<ActionType, ActionType> CounterActions = new()
+    {
+        { ActionType.START_CHARGING, ActionType.STOP_CHARGING },
+        { ActionType.STOP_CHARGING, ActionType.START_CHARGING },
+        { ActionType.START_PAUSE, ActionType.STOP_PAUSE },
+        { ActionType.STOP_PAUSE, ActionType.START_PAUSE },
+    };
+
+    // Actions that target the same resource and cannot run simultaneously
+    private static readonly HashSet<ActionType> LoadHandlingActions =
+    [
+        ActionType.PICK,
+        ActionType.DROP,
+        ActionType.LIFT_ROTATE,
+        ActionType.ROTATE,
+        ActionType.ROTATE_KEEP_LIFT
+    ];
+
+    private static readonly HashSet<ActionType> ChargingActions =
+    [
+        ActionType.START_CHARGING,
+        ActionType.STOP_CHARGING
+    ];
+
+    // Actions that use the Navigation module - cannot run simultaneously
+    private static readonly HashSet<ActionType> NavigationActions =
+    [
+        ActionType.DOCK_TO,
+        ActionType.MOVE_STRAIGHT_TO_COOR,
+        ActionType.MOVE_STRAIGHT_WITH_DISTANCE,
+        ActionType.FINE_POSITIONING,
+        ActionType.INIT_POSITION,
+        ActionType.START_CHARGING,
+        ActionType.STOP_CHARGING
+    ];
+
+    // Functional module actions - cannot run while robot is moving (navigation active)
+    private static readonly HashSet<ActionType> FunctionalModuleActions =
+    [
+        ActionType.PICK,
+        ActionType.DROP,
+        ActionType.LIFT_ROTATE,
+        ActionType.ROTATE,
+        ActionType.ROTATE_KEEP_LIFT,
+        ActionType.DOCK_TO,
+        ActionType.DETECT_OBJECT,
+        ActionType.START_CHARGING,
+        ActionType.STOP_CHARGING,
+        ActionType.FINE_POSITIONING
+    ];
+
+    /// <summary>
+    /// Check if instant action conflicts with any running actions (ORDER or INSTANT)
+    /// </summary>
+    public ConflictResult CheckConflict(
+        RobotNet.VDA5050.InstantAction.Action instantAction,
+        IEnumerable<RobotAction> runningActions,
+        bool isOrderActive = false,
+        bool isDriving = false)
+    {
+        if (!RobotNet.VDA5050.EnumHelper.TryParse(instantAction.ActionType, out ActionType instantType))
+        {
+            return ConflictResult.Invalid("Invalid action type");
+        }
+
+        // VDA5050: cancelOrder and read-only actions must NEVER be blocked
+        if (instantType == ActionType.CANCEL_ORDER ||
+            instantType == ActionType.STATE_REQUEST ||
+            instantType == ActionType.FACTSHEET_REQUEST)
+        {
+            return ConflictResult.NoConflict();
+        }
+
+        // Check: Navigation instant action while Order is active
+        if (isOrderActive && NavigationActions.Contains(instantType))
+        {
+            return ConflictResult.Conflict(
+                ConflictType.NavigationOrderConflict,
+                $"Navigation action {instantType} rejected - Order is active, cannot execute navigation instant actions",
+                null
+            );
+        }
+
+        // Check: Functional module or navigation action while robot is driving
+        if (isDriving && (FunctionalModuleActions.Contains(instantType) || NavigationActions.Contains(instantType)))
+        {
+            return ConflictResult.Conflict(
+                ConflictType.DrivingConflict,
+                $"Action {instantType} rejected - robot is currently moving",
+                null
+            );
+        }
+
+        foreach (var runningAction in runningActions)
+        {
+            // 1. Check counter-action conflict
+            var counterConflict = CheckCounterActionConflict(instantType, runningAction);
+            if (counterConflict.HasConflict)
+            {
+                return counterConflict;
+            }
+
+            // 2. Check resource conflict
+            var resourceConflict = CheckResourceConflict(instantAction, instantType, runningAction);
+            if (resourceConflict.HasConflict)
+            {
+                return resourceConflict;
+            }
+
+            // 3. Check navigation conflict (two navigation actions cannot run simultaneously)
+            var navConflict = CheckNavigationConflict(instantType, runningAction);
+            if (navConflict.HasConflict)
+            {
+                return navConflict;
+            }
+
+            // 4. Check BlockingType conflict
+            var blockingConflict = CheckBlockingTypeConflict(instantAction, runningAction);
+            if (blockingConflict.HasConflict)
+            {
+                return blockingConflict;
+            }
+        }
+
+        return ConflictResult.NoConflict();
+    }
+
+    private static ConflictResult CheckCounterActionConflict(ActionType instantType, RobotAction runningAction)
+    {
+        if (CounterActions.TryGetValue(instantType, out var counterType) &&
+            counterType == runningAction.Type)
+        {
+            return ConflictResult.Conflict(
+                ConflictType.CounterAction,
+                $"InstantAction {instantType} conflicts with running action {runningAction.Type}",
+                runningAction.Id
+            );
+        }
+
+        return ConflictResult.NoConflict();
+    }
+
+    private static ConflictResult CheckResourceConflict(
+        RobotNet.VDA5050.InstantAction.Action instantAction,
+        ActionType instantType,
+        RobotAction runningAction)
+    {
+        // Check if both actions target load handling
+        if (LoadHandlingActions.Contains(instantType) &&
+            LoadHandlingActions.Contains(runningAction.Type))
+        {
+            // Check if same LHD (Load Handling Device)
+            var instantLhd = GetParameterValue(instantAction.ActionParameters, "lhd");
+            var orderLhd = GetParameterValue(runningAction.Parameters, "lhd");
+
+            // If both specify LHD and they're the same, or if neither specifies (default LHD)
+            if (string.IsNullOrEmpty(instantLhd) || string.IsNullOrEmpty(orderLhd) ||
+                instantLhd == orderLhd)
+            {
+                return ConflictResult.Conflict(
+                    ConflictType.ResourceConflict,
+                    $"InstantAction {instantType} conflicts with {runningAction.Type} - same Load Handling Device",
+                    runningAction.Id
+                );
+            }
+        }
+
+        // Check if both actions target charging
+        if (ChargingActions.Contains(instantType) &&
+            ChargingActions.Contains(runningAction.Type))
+        {
+            return ConflictResult.Conflict(
+                ConflictType.ResourceConflict,
+                $"InstantAction {instantType} conflicts with {runningAction.Type} - same charging system",
+                runningAction.Id
+            );
+        }
+
+        return ConflictResult.NoConflict();
+    }
+
+    private static ConflictResult CheckNavigationConflict(ActionType instantType, RobotAction runningAction)
+    {
+        // Two navigation actions cannot run simultaneously
+        if (NavigationActions.Contains(instantType) &&
+            NavigationActions.Contains(runningAction.Type) &&
+            !runningAction.IsCompleted)
+        {
+            return ConflictResult.Conflict(
+                ConflictType.NavigationConflict,
+                $"Navigation action {instantType} conflicts with running navigation action {runningAction.Type}",
+                runningAction.Id
+            );
+        }
+
+        return ConflictResult.NoConflict();
+    }
+
+    private static ConflictResult CheckBlockingTypeConflict(
+        RobotNet.VDA5050.InstantAction.Action instantAction,
+        RobotAction runningAction)
+    {
+        // HARD instant action cannot run when HARD action is running
+        if (instantAction.BlockingType == BlockingType.HARD &&
+            runningAction.BlockingType == BlockingType.HARD &&
+            !runningAction.IsCompleted)
+        {
+            return ConflictResult.Conflict(
+                ConflictType.BlockingTypeConflict,
+                $"InstantAction (HARD) cannot run while action {runningAction.Type} (HARD) is running",
+                runningAction.Id
+            );
+        }
+
+        return ConflictResult.NoConflict();
+    }
+
+    private static string? GetParameterValue(
+        RobotNet.VDA5050.InstantAction.ActionParameter[]? parameters,
+        string key)
+    {
+        return parameters?.FirstOrDefault(p => p.Key == key)?.Value;
+    }
+}
+
+/// <summary>
+/// Result of conflict detection
+/// </summary>
+public class ConflictResult
+{
+    public bool HasConflict { get; init; }
+    public ConflictType Type { get; init; }
+    public string Description { get; init; } = "";
+    public string? ConflictingActionId { get; init; }
+
+    public static ConflictResult NoConflict() => new() { HasConflict = false };
+
+    public static ConflictResult Conflict(ConflictType type, string description, string? conflictingActionId = null)
+        => new()
+        {
+            HasConflict = true,
+            Type = type,
+            Description = description,
+            ConflictingActionId = conflictingActionId
+        };
+
+    public static ConflictResult Invalid(string description)
+        => new()
+        {
+            HasConflict = true,
+            Type = ConflictType.Invalid,
+            Description = description
+        };
+}
+
+/// <summary>
+/// Types of conflicts
+/// </summary>
+public enum ConflictType
+{
+    None,
+    CounterAction,          // e.g., startCharging vs stopCharging
+    ResourceConflict,       // e.g., two pick actions on same LHD
+    NavigationConflict,     // e.g., two navigation actions (dockTo vs moveStraight)
+    NavigationOrderConflict,// Navigation instant action while Order is active
+    DrivingConflict,        // Functional module action while robot is moving
+    BlockingTypeConflict,   // e.g., HARD vs HARD
+    Invalid                 // Invalid action type or parameters
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Helper/OrderConverter.cs

@@ -0,0 +1,215 @@
+using RobotNet.VDA5050.Order;
+using RobotNet.VDA5050.Type;
+using RobotNet10.Common;
+using RobotNet10.RobotApp.Services.Exceptions;
+using RobotNet10.RobotApp.Services.Robot.Models;
+using RobotNet10.RobotApp.Shared.Enums;
+
+namespace RobotNet10.RobotApp.Services.Robot.Helper;
+
+public class OrderConverter
+{
+    public static (OrderNode[] Nodes, OrderEdge[] Edges) Validate(Node[] nodes, Edge[] edges, double currentTheta)
+    {
+        if (nodes.Length < 2) throw new PathPlannerException(RobotErrors.Error1002(nodes.Length));
+        if (edges.Length != nodes.Length - 1) throw new PathPlannerException(RobotErrors.Error1004(nodes.Length, edges.Length));
+
+        OrderNode[] orderNodes = [..nodes.Select(n => new OrderNode
+        {
+            NodeId = n.NodeId,
+            SequenceId = n.SequenceId,
+            X = n.NodePosition?.X ?? 0,
+            Y = n.NodePosition?.Y ?? 0,
+            Theta = n.NodePosition?.Theta,
+            AllowedDeviationXY = n.NodePosition?.AllowedDeviationXY,
+            AllowedDeviationTheta = n.NodePosition?.AllowedDeviationTheta,
+        })];
+
+        List<OrderEdge> orderEdges = [];
+        foreach (var edge in edges)
+        {
+            var trajectory = edge.Trajectory;
+            var controlPoints = trajectory?.ControlPoints;
+            orderEdges.Add(new()
+            {
+                EdgeId = edge.EdgeId,
+                SequenceId = edge.SequenceId,
+                StartNodeId = edge.StartNodeId,
+                EndNodeId = edge.EndNodeId,
+                Orientation = edge.Orientation,
+                OrientationType = edge.OrientationType,
+                RotationAllowed = edge.RotationAllowed,
+                Speed = edge.MaxSpeed,
+                Degree = edge.Trajectory?.Degree ?? 1,
+                ControlPoint1X = controlPoints is { Length: > 2 } ? controlPoints[1].X : 0,
+                ControlPoint1Y = controlPoints is { Length: > 2 } ? controlPoints[1].Y : 0,
+                ControlPoint2X = controlPoints is { Length: > 3 } ? controlPoints[2].X : 0,
+                ControlPoint2Y = controlPoints is { Length: > 3 } ? controlPoints[2].Y : 0,
+            });
+        }
+        // cần xử lí để lấy direction
+        var currentDirection = GetDirectionInNode(nodes[0].NodePosition?.Theta ?? currentTheta, orderNodes[0], orderNodes[1], orderEdges[0]);
+        for(int i = 0; i < orderEdges.Count; i++)
+        {
+            currentDirection = OrientationToDirection(currentDirection, orderNodes[i], orderNodes[i + 1], orderEdges[i]);
+            orderEdges[i].Direction = currentDirection;
+            orderNodes[i].ContinueTheta = GetAngleInNodeStart(orderNodes[i], orderNodes[i + 1], orderEdges[i]);
+            if (i > 0)
+            {
+                var inNodeAngle = GetAngleInNodeEnd(orderNodes[i], orderNodes[i - 1], orderEdges[i - 1]);
+                if (orderNodes[i].Theta is { } theta && Math.Abs(SpaceCompute.NormalizeRadianAngle(inNodeAngle) - SpaceCompute.NormalizeRadianAngle(theta)) > 0.04)
+                {
+                    orderNodes[i].IsWaitRotating = true;
+                }
+                if (!orderNodes[i].IsWaitRotating && orderNodes[i].ContinueTheta is { } continueTheta)
+                {
+                    if (Math.Abs(SpaceCompute.NormalizeRadianAngle(inNodeAngle) - SpaceCompute.NormalizeRadianAngle(continueTheta)) > 0.785)
+                    {
+                        orderNodes[i].IsWaitRotating = true;
+                    }
+                }
+            }
+        }
+        return (orderNodes , [..orderEdges]);
+    }
+
+    private static RobotDirection ConvertTangentialOrientation(double orientation)
+    {
+        // Normalize về [0, 2*PI] để dễ xử lý
+        double normalizedAngle = SpaceCompute.NormalizeRadianAngle(orientation);
+        if (normalizedAngle < 0) normalizedAngle += 2 * Math.PI;
+
+        // Forward: orientation gần 0 (hoặc 2*PI)
+        // Backward: orientation gần PI
+
+        // Kiểm tra gần 0 hoặc 2*PI (Forward)
+        if (normalizedAngle <= Math.PI / 2 || normalizedAngle >= 3 * Math.PI / 2)
+        {
+            return RobotDirection.FORWARD;
+        }
+        // Kiểm tra gần PI (Backward)
+        else
+        {
+            return RobotDirection.BACKWARD;
+        }
+    }
+
+    private static RobotDirection ConvertGlobalOrientation(double orientation, OrderNode inNode, OrderNode futureNode, OrderEdge edge)
+    {
+        (double futurex, double futurey) = SpaceCompute.BezierPoint(0.1, new()
+        {
+            StartX = inNode.X,
+            StartY = inNode.Y,
+            EndX = futureNode.X,
+            EndY = futureNode.Y,
+            ControlPoint1X = edge.ControlPoint1X ?? 0,
+            ControlPoint1Y = edge.ControlPoint1Y ?? 0,
+            ControlPoint2X = edge.ControlPoint2X ?? 0,
+            ControlPoint2Y = edge.ControlPoint2Y ?? 0,
+            Degree = edge.Degree,
+        });
+
+        var edgeAngle = Math.Atan2(futurey - inNode.Y, futurex - inNode.X);
+
+        // Tính góc chênh lệch giữa orientation và edge angle
+        double angleDiff = SpaceCompute.NormalizeRadianAngle(orientation - edgeAngle);
+
+        // Nếu góc chênh lệch gần 0 -> Forward
+        // Nếu góc chênh lệch gần PI -> Backward
+        double absAngleDiff = Math.Abs(angleDiff);
+
+        if (absAngleDiff <= Math.PI / 2)
+        {
+            return RobotDirection.FORWARD;
+        }
+        else
+        {
+            return RobotDirection.BACKWARD;
+        }
+    }
+
+    private static RobotDirection GetDirectionInNode(double currentTheta, OrderNode inNode, OrderNode futureNode, OrderEdge edge)
+    {
+        (double futurex, double futurey) = SpaceCompute.BezierPoint(0.1, new()
+        {
+            StartX = inNode.X,
+            StartY = inNode.Y,
+            EndX = futureNode.X,
+            EndY = futureNode.Y,
+            ControlPoint1X = edge.ControlPoint1X ?? 0,
+            ControlPoint1Y = edge.ControlPoint1Y ?? 0,
+            ControlPoint2X = edge.ControlPoint2X ?? 0,
+            ControlPoint2Y = edge.ControlPoint2Y ?? 0,
+            Degree = edge.Degree,
+        });
+        (double robotx, double roboty) =
+        (
+            inNode.X + Math.Cos(currentTheta),
+            inNode.Y + Math.Sin(currentTheta)
+        );
+
+        var angle = SpaceCompute.GetVectorAngle(
+                inNode.X,
+                inNode.Y,
+                robotx,
+                roboty,
+                futurex,
+                futurey);
+        return angle > 90 ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
+    }
+
+    private static double GetAngleInNodeEnd(OrderNode inNode, OrderNode oldNode, OrderEdge edge)
+    {
+        (double oldX, double oldY) = SpaceCompute.BezierPoint(0.9, new()
+        {
+            StartX = oldNode.X,
+            StartY = oldNode.Y,
+            EndX = inNode.X,
+            EndY = inNode.Y,
+            ControlPoint1X = edge.ControlPoint1X ?? 0,
+            ControlPoint1Y = edge.ControlPoint1Y ?? 0,
+            ControlPoint2X = edge.ControlPoint2X ?? 0,
+            ControlPoint2Y = edge.ControlPoint2Y ?? 0,
+            Degree = edge.Degree,
+        });
+        var dy = inNode.Y - oldY;
+        var dx = inNode.X - oldX;
+        return edge.Direction == RobotDirection.FORWARD ? Math.Atan2(dy, dx) : Math.Atan2(-dy, -dx);
+    }
+
+    private static double GetAngleInNodeStart(OrderNode inNode, OrderNode futureNode, OrderEdge edge)
+    {
+        (double futureX, double futureY) = SpaceCompute.BezierPoint(0.1, new()
+        {
+            StartX = inNode.X,
+            StartY = inNode.Y,
+            EndX = futureNode.X,
+            EndY = futureNode.Y,
+            ControlPoint1X = edge.ControlPoint1X ?? 0,
+            ControlPoint1Y = edge.ControlPoint1Y ?? 0,
+            ControlPoint2X = edge.ControlPoint2X ?? 0,
+            ControlPoint2Y = edge.ControlPoint2Y ?? 0,
+            Degree = edge.Degree,
+        });
+        var dy = futureY - inNode.Y;
+        var dx = futureX - inNode.X;
+        return edge.Direction == RobotDirection.FORWARD ? Math.Atan2(dy, dx) : Math.Atan2(-dy, -dx);
+    }
+
+    public static RobotDirection OrientationToDirection(RobotDirection currentDirection, OrderNode inNode, OrderNode futureNode, OrderEdge edge)
+    {
+        if(edge.Orientation.HasValue && edge.OrientationType is not null)
+        {
+            switch (edge.OrientationType)
+            {
+                case OrientationType.TANGENTIAL:
+                    return ConvertTangentialOrientation(edge.Orientation.Value);
+
+                case OrientationType.GLOBAL:
+                    return ConvertGlobalOrientation(edge.Orientation.Value, inNode, futureNode, edge);
+            }
+        }
+        if (inNode.Theta.HasValue) return GetDirectionInNode(inNode.Theta.Value, inNode, futureNode, edge);
+        return currentDirection;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/HighPrecisionTimer.cs

@@ -0,0 +1,157 @@
+using System.Diagnostics;
+using System.Runtime.InteropServices;
+
+namespace RobotNet10.RobotApp.Services.Robot;
+
+internal static partial class Windows
+{
+    [LibraryImport("winmm.dll")]
+    internal static partial uint timeBeginPeriod(uint uPeriod);
+
+    [LibraryImport("winmm.dll")]
+    internal static partial uint timeEndPeriod(uint uPeriod);
+}
+
+public static class HighPrecisionTimerHelper
+{
+    public static void EnableHighPrecision()
+    {
+        if (RuntimeInformation.IsOSPlatform(OSPlatform.Windows))
+        {
+            _ = Windows.timeBeginPeriod(2);
+        }
+    }
+
+    public static void DisableHighPrecision()
+    {
+        if (RuntimeInformation.IsOSPlatform(OSPlatform.Windows))
+        {
+            _ = Windows.timeEndPeriod(2);
+        }
+    }
+}
+
+public class HighPrecisionTimer<T>(int Interval, Action Callback, Logger<T>? Logger) : IDisposable where T : class
+{
+    public bool Disposed;
+    private Thread? Thread;
+    private long IntervalTicks;
+    private long NextDueTime;
+    private readonly Lock Lock = new();
+
+    private void Handler()
+    {
+        while (!Disposed)
+        {
+            long now = Stopwatch.GetTimestamp();
+
+            bool shouldRun = false;
+
+            lock (Lock)
+            {
+                if (Disposed) break;
+                if (now >= NextDueTime)
+                {
+                    shouldRun = true;
+                    long scheduledTime = NextDueTime;
+                    NextDueTime += IntervalTicks;
+
+                    // Tự đồng bộ nếu lệch quá
+                    long driftTicks = now - scheduledTime;
+                    if (driftTicks > IntervalTicks / 2)
+                    {
+                        Logger?.Warning($"High-res timer drift: {driftTicks * 1000.0 / Stopwatch.Frequency:F3}ms. Resync.");
+                        NextDueTime = now + IntervalTicks;
+                    }
+                }
+            }
+
+            // === BƯỚC 2: Chạy callback ===
+            if (shouldRun)
+            {
+                try
+                {
+                    Callback.Invoke();
+                }
+                catch (Exception ex)
+                {
+                    Logger?.Error($"Callback error in high-precision timer: {ex}");
+                }
+            }
+
+            // === BƯỚC 3: Chờ chính xác đến lần sau ===
+            long sleepUntil = NextDueTime;
+            while (!Disposed)
+            {
+                now = Stopwatch.GetTimestamp();
+                long remaining = sleepUntil - now;
+
+                if (remaining <= 0)
+                    break;
+
+                // > 1ms → Sleep
+                if (remaining > Stopwatch.Frequency / 1000)
+                {
+                    Thread.Sleep(1);
+                }
+                // < 1ms → SpinWait
+                else
+                {
+                    Thread.SpinWait((int)(remaining / 10));
+                }
+            }
+        }
+    }
+
+    public void Start()
+    {
+        if (!Disposed)
+        {
+            lock (Lock)
+            {
+                if (Interval < 30) HighPrecisionTimerHelper.EnableHighPrecision();
+                IntervalTicks = (long)(Interval * (Stopwatch.Frequency / 1000.0));
+                Thread = new Thread(Handler) { IsBackground = true, Priority = ThreadPriority.Highest };
+                NextDueTime = Stopwatch.GetTimestamp() + IntervalTicks;
+                Thread.Start();
+            }
+        }
+        else throw new ObjectDisposedException(nameof(HighPrecisionTimer<T>));
+    }
+
+    public void Stop()
+    {
+        if (Disposed) return;
+
+        if (Thread != null)
+        {
+            Disposed = true;
+            lock (Lock)
+            {
+                Thread.Join(100);
+                Thread = null;
+                HighPrecisionTimerHelper.DisableHighPrecision();
+            }
+        }
+    }
+
+    public void Dispose()
+    {
+        Dispose(true);
+        GC.SuppressFinalize(this);
+    }
+
+    protected virtual void Dispose(bool disposing)
+    {
+        if (Disposed) return;
+
+        if (disposing) Stop();
+
+        Disposed = true;
+    }
+
+    ~HighPrecisionTimer()
+    {
+        Dispose(false);
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Models/OrderEdge.cs

@@ -0,0 +1,25 @@
+using RobotNet.VDA5050.Order;
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Shared.Enums;
+
+namespace RobotNet10.RobotApp.Services.Robot.Models;
+
+public class OrderEdge
+{
+    public string EdgeId { get; set; } = string.Empty;
+    public int SequenceId { get; set; }
+    public string StartNodeId { get; set; } = string.Empty;
+    public string EndNodeId { get; set; } = string.Empty;
+
+    public double? Orientation { get; set; }
+    public double? Speed { get; set; }
+    public OrientationType? OrientationType { get; set; }
+    public RobotDirection Direction { get; set; }
+    public bool? RotationAllowed { get; set; }
+
+    public int Degree { get; set; }
+    public double? ControlPoint1X { get; set; }
+    public double? ControlPoint1Y { get; set; }
+    public double? ControlPoint2X { get; set; }
+    public double? ControlPoint2Y { get; set; }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Models/OrderNode.cs

@@ -0,0 +1,14 @@
+namespace RobotNet10.RobotApp.Services.Robot.Models;
+
+public class OrderNode
+{
+    public string NodeId { get; set; } = string.Empty;
+    public int SequenceId { get; set; }
+    public double X { get; set; }
+    public double Y { get; set; }
+    public double? Theta { get; set; }
+    public double? AllowedDeviationXY { get; set; }
+    public double? AllowedDeviationTheta { get; set; }
+    public bool IsWaitRotating { get; set; }
+    public double? ContinueTheta { get; set; }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Modules/RobotLocalization.cs

@@ -0,0 +1,116 @@
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Xloc;
+using RobotNet10.RobotApp.Services.ConfigManager;
+using RobotNet10.RobotApp.Services.Simulation;
+using RobotNet10.Shared;
+using RobotNet10.Shared.Geometry;
+using RobotNet10.RobotApp.Shared.Enums;
+
+namespace RobotNet10.RobotApp.Services.Robot.Modules;
+
+
+public class RobotLocalization(IRobotConfiguration RobotConfiguration,
+                               XlocIntegrationService xlocService,
+                               SimulationVisualization SimVisualization,
+                               Logger<RobotLocalization> Logger) 
+                               : ILocalization
+{
+    public double X => IsSimulation ? SimVisualization.X : GetXlocX();
+    public double Y => IsSimulation ? SimVisualization.Y : GetXlocY();
+    public double Theta => IsSimulation ? SimVisualization.Theta * Math.PI / 180 : GetXlocTheta();
+    public bool IsReady => IsSimulation ? true : IsXlocReady();
+    public string CurrentActiveMap => IsSimulation ? "" : GetXlocCurrentActiveMap();
+    public double DeviationRange { get; private set; }
+    public double LocalizationScore => IsSimulation ? 1.0 : GetXlocLocalizationScore();
+    public bool PositionInitialized => IsSimulation ? true : GetXlocPositionInitialized();
+    private bool IsSimulation => RobotConfiguration.GetSimulationConfig().IsEnable;
+
+    private double GetXlocX()
+    {
+        var pose = xlocService.GetCurrentPose2D();
+        return pose?.x ?? 0.0;
+    }
+
+    private double GetXlocY()
+    {
+        var pose = xlocService.GetCurrentPose2D();
+        return pose?.y ?? 0.0;
+    }
+
+    private double GetXlocTheta()
+    {
+        var pose = xlocService.GetCurrentPose2D();
+        return pose?.yaw ?? 0.0;
+    }
+
+    private bool IsXlocReady()
+    {
+        var diagnostics = xlocService.GetDiagnostics();
+        // 0=MAPPING, 1=LOCALIZATION, 2=PROCESSING, 3=READY, 4=ERROR. Accept 1,2,3 so orders are allowed once localizing.
+        if (diagnostics == null) return false;
+        return diagnostics.XlocState is 1 or 2 or 3;
+    }
+
+    private string GetXlocCurrentActiveMap()
+    {
+        var diagnostics = xlocService.GetDiagnostics();
+        return diagnostics?.CurrentActiveMap ?? "";
+    }
+
+    private double GetXlocLocalizationScore()
+    {
+        var diagnostics = xlocService.GetDiagnostics();
+        return diagnostics?.Reliability ?? 0.0; // Use Reliability (0.0 to 1.0) as LocalizationScore
+    }
+
+    private bool GetXlocPositionInitialized()
+    {
+        // Position is initialized if we have a valid pose from XLOC and it's not in ERROR state
+        var pose = xlocService.GetCurrentPose2D();
+        var diagnostics = xlocService.GetDiagnostics();
+        
+        return pose.HasValue && diagnostics?.XlocState != 4; // 4 = ERROR
+    }
+
+    public double DistanceTo(double x, double y)
+    {
+        return Math.Sqrt(Math.Pow(x - X, 2) + Math.Pow(y - Y, 2));
+    }
+
+    public MessageResult SetInitializePosition(double x, double y, double theta)
+    {
+        try
+        {
+            if (IsSimulation)
+            {
+                SimVisualization.LocalizationInitialize(x, y, theta * 180 / Math.PI);
+                return new(true);
+            }
+            else
+            {
+                // Use XlocIntegrationService to set initial pose
+                // theta is in radians, convert to radians for xloc (it expects radians)
+                bool result = xlocService.SetInitialPose(x, y, 0.0, 0.0, 0.0, theta);
+                if (result)
+                {
+                    return new(true, "Initial position set successfully");
+                }
+                else
+                {
+                    return new(false, "Failed to set initial position");
+                }
+            }
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"Initialize robot position failed: {ex.Message}");
+            return new(false, $"Initialize robot position failed: {ex.Message}");
+        }
+    }
+
+    // private bool GetIsReady()
+    // {
+    //     if (IsSimulation) return true;
+    //     return xlocService.IsReady;
+    // }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Modules/RobotNavigation.cs

@@ -0,0 +1,339 @@
+using RobotNet.VDA5050.Order;
+using RobotNet10.RobotApp.Detection;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Services.ConfigManager;
+using RobotNet10.RobotApp.Services.Exceptions;
+using RobotNet10.RobotApp.Services.Simulation;
+using RobotNet10.RobotApp.Shared.Enums;
+
+namespace RobotNet10.RobotApp.Services.Robot.Modules;
+
+public class RobotNavigation(
+    IRobotConfiguration robotConfiguration,
+    IServiceProvider serviceProvider,
+    RobotNet10.RobotApp.Navigation.NavigationIntegrationService navigationIntegrationService,
+    ILogger<RobotNavigation> logger) : INavigation
+{
+    public bool IsReady { get; private set; }
+    private bool _navResultSubscribed;
+
+    public bool Driving
+    {
+        get
+        {
+            if (IsSimulation)
+                return SimNavigation?.Driving ?? false;
+
+            var feedback = navigationIntegrationService.GetFeedback();
+            if (feedback == null) return false;
+            return feedback.NavigationState is RobotNet10.RobotApp.Navigation.NavigationState.Controlling;
+            // return feedback.NavigationState is RobotNet10.RobotApp.Navigation.NavigationState.Active
+            //     or RobotNet10.RobotApp.Navigation.NavigationState.Planning
+            //     or RobotNet10.RobotApp.Navigation.NavigationState.Controlling;
+        }
+    }
+
+    public double VelocityX => IsSimulation ? (SimNavigation?.VelocityX ?? 0) : (navigationIntegrationService.GetTwist()?.x ?? 0);
+    public double VelocityY => IsSimulation ? (SimNavigation?.VelocityY ?? 0) : (navigationIntegrationService.GetTwist()?.y ?? 0);
+    public double Omega => IsSimulation ? (SimNavigation?.Omega ?? 0) : (navigationIntegrationService.GetTwist()?.theta ?? 0);
+    public RobotNet10.RobotApp.Interfaces.NavigationState State => _lastFinishedState ?? (IsSimulation ? (SimNavigation?.State ?? RobotNet10.RobotApp.Interfaces.NavigationState.Idle) : MapNavigationState(navigationIntegrationService.GetFeedback()?.NavigationState));
+
+    public IReadOnlyList<NavigationNode>? CurrentPath
+    {
+        get
+        {
+            if (IsSimulation)
+                return null;
+
+            var globalPath = navigationIntegrationService.GetGlobalPathData();
+            if (globalPath == null || globalPath.Points.Count == 0)
+                return null;
+
+            return globalPath.Points.Select(p => new NavigationNode
+            {
+                Id = Guid.NewGuid(),
+                X = p.X,
+                Y = p.Y,
+                Theta = p.Theta
+            }).ToList();
+        }
+    }
+
+    // C API navigation currently does not expose these dock monitoring values.
+    public bool IsDockingActive => false;
+    public NavigationNode? DockGoal => null;
+    public string DockPhase => string.Empty;
+    public string DockDirection => string.Empty;
+    public int DockRetryCount => 0;
+    public int DockMaxRetries => 0;
+    public int DockWaypointCount => 0;
+    public NavigationNode? DockStartNode => null;
+    public IReadOnlyList<NavigationNode>? DockWaypoints => null;
+
+    private volatile SimulationNavigation? SimNavigation;
+    private RobotNet10.RobotApp.Interfaces.NavigationState? _lastFinishedState;
+    private bool IsSimulation => robotConfiguration.GetSimulationConfig().IsEnable;
+
+    public event Action<RobotNet10.RobotApp.Interfaces.NavigationState>? OnNavigationFinished;
+
+    public void CancelMovement()
+    {
+        if (IsSimulation)
+        {
+            SimNavigation?.CancelMovement();
+            return;
+        }
+
+        navigationIntegrationService.Cancel();
+    }
+
+    public void Move(OrderMsg order, bool hasLoad = false)
+    {
+        _lastFinishedState = null;
+        var nodes = order.Nodes;
+        var edges = order.Edges;
+
+        if (IsSimulation)
+        {
+            if (SimNavigation is not null) throw new NavigationException("The Sim Navigation module is called during operation.");
+            SimNavigation = SimulationNavigationManager.GetNavigation(robotConfiguration.GetRobotPhysicalConfig().NavigationType, serviceProvider);
+            SimNavigation.OnNavigationFinished += NavigationFinished;
+            SimNavigation.Move(order, hasLoad);
+            return;
+        }
+
+        if (nodes.Length == 0)
+            throw new NavigationException("Move failed: nodes list is empty.");
+
+        var target = nodes[^1];
+        var (targetX, targetY, theta) = GetNodePose(target, nodes);
+        var (qz, qw) = ToYawQuaternion(theta);
+
+        // Convert VDA5050 order (from MQTT server) to OrderData and run full graph navigation
+        var orderData = RobotNet10.RobotApp.Navigation.VDA5050ToOrderDataConverter.ToOrderData(nodes, edges, orderMsg: order);
+        if (!navigationIntegrationService.MoveToOrder(orderData, targetX, targetY, 0.0, 0.0, 0.0, qz, qw))
+            throw new NavigationException("Move failed: Navigation C API service is not ready or rejected goal.");
+    }
+
+    public void MoveStraight(double x, double y, bool hasLoad = false, RobotDirection? direction = null)
+    {
+        if (IsSimulation)
+        {
+            if (SimNavigation is not null) throw new NavigationException("The Sim Navigation module is called during operation.");
+            SimNavigation = SimulationNavigationManager.GetNavigation(robotConfiguration.GetRobotPhysicalConfig().NavigationType, serviceProvider);
+            SimNavigation.OnNavigationFinished += NavigationFinished;
+            SimNavigation.MoveStraight(x, y, hasLoad, direction);
+            return;
+        }
+
+        var current = navigationIntegrationService.GetRobotPose2D();
+        var currentX = current?.x ?? 0.0;
+        var currentY = current?.y ?? 0.0;
+        var heading = Math.Atan2(y - currentY, x - currentX);
+        var (qz, qw) = ToYawQuaternion(heading);
+
+        if (!navigationIntegrationService.MoveTo(x, y, 0.0, 0.0, 0.0, qz, qw))
+            throw new NavigationException("MoveStraight failed: Navigation C API service is not ready or rejected goal.");
+    }
+
+    public void Pause()
+    {
+        if (IsSimulation) SimNavigation?.Pause();
+        else navigationIntegrationService.Pause();
+    }
+
+    public void Resume()
+    {
+        if (IsSimulation) SimNavigation?.Resume();
+        else navigationIntegrationService.Resume();
+    }
+
+    public void Rotate(double angle)
+    {
+        _lastFinishedState = null;
+
+        if (IsSimulation)
+        {
+            if (SimNavigation is not null) throw new NavigationException("The Sim Navigation module is called during operation.");
+            SimNavigation = SimulationNavigationManager.GetNavigation(robotConfiguration.GetRobotPhysicalConfig().NavigationType, serviceProvider);
+            SimNavigation.OnNavigationFinished += NavigationFinished;
+            SimNavigation.Rotate(angle * 180 / Math.PI);
+            return;
+        }
+
+        var pose = navigationIntegrationService.GetRobotPose2D();
+        var x = pose?.x ?? 0.0;
+        var y = pose?.y ?? 0.0;
+        var (qz, qw) = ToYawQuaternion(angle);
+
+        if (!navigationIntegrationService.RotateTo(x, y, 0.0, 0.0, 0.0, qz, qw))
+            throw new NavigationException("Rotate failed: Navigation C API service is not ready or rejected goal.");
+    }
+
+    public void DockTo(IDetectSession session, bool hasLoad = false, RobotDirection? direction = null)
+    {
+        _lastFinishedState = null;
+
+        if (IsSimulation)
+        {
+            if (SimNavigation is not null) throw new NavigationException("The Sim Navigation module is called during operation.");
+            SimNavigation = SimulationNavigationManager.GetNavigation(robotConfiguration.GetRobotPhysicalConfig().NavigationType, serviceProvider);
+            SimNavigation.OnNavigationFinished += NavigationFinished;
+            SimNavigation.DockTo(session, hasLoad, direction);
+            return;
+        }
+
+        var goal = session.Goal ?? throw new NavigationException("DockTo failed: session goal is missing.");
+        var markerName = "dock-marker";
+        var p = goal.Pose.Position;
+        var o = goal.Pose.Orientation;
+
+        if (!navigationIntegrationService.DockTo(markerName, p.X, p.Y, p.Z, o.X, o.Y, o.Z, o.W))
+            throw new NavigationException("DockTo failed: Navigation C API service is not ready or rejected goal.");
+    }
+
+    public void RefreshOrder(Node[] nodes, Edge[] edges)
+    {
+        logger.LogWarning("RefreshOrder is not yet implemented for C API navigation path.");
+    }
+
+    public void UpdateOrder(string lastBaseNodeId)
+    {
+        if (IsSimulation)
+        {
+            SimNavigation?.UpdateOrder(lastBaseNodeId);
+            return;
+        }
+
+        logger.LogDebug("UpdateOrder called in C API mode with lastBaseNodeId={LastBaseNodeId}.", lastBaseNodeId);
+    }
+
+    public void SafetyStop()
+    {
+        if (IsSimulation) SimNavigation?.SafetyStop();
+        else navigationIntegrationService.Cancel();
+    }
+
+    public void Refresh()
+    {
+        if (IsSimulation) SimNavigation?.Refresh();
+    }
+
+    private void NavigationFinished(RobotNet10.RobotApp.Interfaces.NavigationState state)
+    {
+        _lastFinishedState = state;
+        OnNavigationFinished?.Invoke(state);
+
+        if (IsSimulation) SimNavigation?.OnNavigationFinished -= NavigationFinished;
+        SimNavigation = null;
+    }
+
+    public void SetSpeed(double speed)
+    {
+        if (IsSimulation) SimNavigation?.SetSpeed(speed);
+        else 
+        {
+            logger.LogInformation("SetSpeed called with speed={Speed}", speed);
+            if (!navigationIntegrationService.SetTwistLinear(speed, 0.0, 0.0))
+                throw new NavigationException($"SetSpeed failed: unable to set linear velocity to {speed} via Navigation C API.");
+        }
+    }
+
+    public void Start()
+    {
+        IsReady = IsSimulation || navigationIntegrationService.IsInitialized;
+        if (!IsSimulation && !_navResultSubscribed)
+        {
+            navigationIntegrationService.OnNavigationResult += OnNavigationResultReceived;
+            _navResultSubscribed = true;
+        }
+    }
+
+    private void OnNavigationResultReceived(RobotNet10.RobotApp.Navigation.NavigationState state)
+    {
+        var mapped = MapNavigationState(state);
+        NavigationFinished(mapped);
+    }
+
+    public void Stop()
+    {
+        if (SimNavigation is not null)
+        {
+            SimNavigation.CancelMovement();
+        }
+        else
+        {
+            navigationIntegrationService.Cancel();
+        }
+    }
+
+    private static (double qz, double qw) ToYawQuaternion(double yaw)
+    {
+        var half = yaw / 2.0;
+        return (Math.Sin(half), Math.Cos(half));
+    }
+
+    private static (double x, double y, double theta) GetNodePose(Node target, Node[] allNodes)
+    {
+        var (x, y, thetaOpt) = ExtractNodePosition(target);
+        if (thetaOpt.HasValue)
+            return (x, y, thetaOpt.Value);
+
+        if (allNodes.Length >= 2)
+        {
+            var (prevX, prevY, _) = ExtractNodePosition(allNodes[^2]);
+            return (x, y, Math.Atan2(y - prevY, x - prevX));
+        }
+
+        return (x, y, 0.0);
+    }
+
+    private static (double x, double y, double? theta) ExtractNodePosition(Node node)
+    {
+        // VDA5050 Node may store coordinates in NodePosition, while legacy models may use X/Y/Theta directly.
+        var nodeType = node.GetType();
+
+        var nodePosProp = nodeType.GetProperty("NodePosition");
+        if (nodePosProp?.GetValue(node) is object nodePos)
+        {
+            var posType = nodePos.GetType();
+            var xObj = posType.GetProperty("X")?.GetValue(nodePos);
+            var yObj = posType.GetProperty("Y")?.GetValue(nodePos);
+            var thetaObj = posType.GetProperty("Theta")?.GetValue(nodePos);
+
+            return (
+                xObj is null ? 0.0 : Convert.ToDouble(xObj),
+                yObj is null ? 0.0 : Convert.ToDouble(yObj),
+                thetaObj is null ? null : Convert.ToDouble(thetaObj));
+        }
+
+        var xLegacy = nodeType.GetProperty("X")?.GetValue(node);
+        var yLegacy = nodeType.GetProperty("Y")?.GetValue(node);
+        var thetaLegacy = nodeType.GetProperty("Theta")?.GetValue(node);
+
+        return (
+            xLegacy is null ? 0.0 : Convert.ToDouble(xLegacy),
+            yLegacy is null ? 0.0 : Convert.ToDouble(yLegacy),
+            thetaLegacy is null ? null : Convert.ToDouble(thetaLegacy));
+    }
+
+    private static RobotNet10.RobotApp.Interfaces.NavigationState MapNavigationState(RobotNet10.RobotApp.Navigation.NavigationState? state)
+    {
+        return state switch
+        {
+            RobotNet10.RobotApp.Navigation.NavigationState.Pending => RobotNet10.RobotApp.Interfaces.NavigationState.Waiting,
+            RobotNet10.RobotApp.Navigation.NavigationState.Planning => RobotNet10.RobotApp.Interfaces.NavigationState.Initializing,
+            RobotNet10.RobotApp.Navigation.NavigationState.Active => RobotNet10.RobotApp.Interfaces.NavigationState.Moving,
+            RobotNet10.RobotApp.Navigation.NavigationState.Controlling => RobotNet10.RobotApp.Interfaces.NavigationState.Moving,
+            RobotNet10.RobotApp.Navigation.NavigationState.Clearing => RobotNet10.RobotApp.Interfaces.NavigationState.Moving,
+            RobotNet10.RobotApp.Navigation.NavigationState.Succeeded => RobotNet10.RobotApp.Interfaces.NavigationState.Completed,
+            RobotNet10.RobotApp.Navigation.NavigationState.Paused => RobotNet10.RobotApp.Interfaces.NavigationState.Paused,
+            RobotNet10.RobotApp.Navigation.NavigationState.Preempted => RobotNet10.RobotApp.Interfaces.NavigationState.Canceled,
+            RobotNet10.RobotApp.Navigation.NavigationState.Recalled => RobotNet10.RobotApp.Interfaces.NavigationState.Canceled,
+            RobotNet10.RobotApp.Navigation.NavigationState.Rejected => RobotNet10.RobotApp.Interfaces.NavigationState.Error,
+            RobotNet10.RobotApp.Navigation.NavigationState.Aborted => RobotNet10.RobotApp.Interfaces.NavigationState.Error,
+            RobotNet10.RobotApp.Navigation.NavigationState.Lost => RobotNet10.RobotApp.Interfaces.NavigationState.Error,
+            _ => RobotNet10.RobotApp.Interfaces.NavigationState.Idle
+        };
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Modules/RobotPlcAddress.cs

@@ -0,0 +1,105 @@
+namespace RobotNet10.RobotApp.Services.Robot;
+
+/// <summary>
+/// Modbus coil addresses aligned with PLC mapping document.
+/// Input (read): 2848-2879 (M800-M821 sensors, M825-M831 speed SLS).
+/// Output (write): 2948-2977 (M900-M909 state, M915-M917 operation, M920-M929 actions).
+/// </summary>
+public partial class RobotPlcController
+{
+    // === Input coils: ReadOnlyStartAddress 2848 (0x0b20), offsets vs 2848 ===
+    public static readonly ushort ReadOnlyStartAddress = 0x0b20; // 2848, M800
+    public static readonly ushort EmergencyOffsetAddress = 0;    // M800 EMC
+    public static readonly ushort BumperOffsetAddress = 1;        // M801 Bumper
+
+    public static readonly ushort LidarFrontProtectFieldOffsetAddress = 2;   // M802 Lidar NS3-FR
+    public static readonly ushort LidarBackProtectFieldOffsetAddress = 3;   // M803 Lidar NS3-RR
+    public static readonly ushort LidarFrontTimProtectFieldOffsetAddress = 4; // M804 Lidar TIM718S-FR
+
+    public static readonly ushort LiftedUpOffsetAddress = 5;   // M805 Lift up limit
+    public static readonly ushort LiftedDownOffsetAddress = 6;  // M806 Lift down limit
+    public static readonly ushort LiftHomeOffsetAddress = 7;    // M807 Rotate homing
+
+    public static readonly ushort LeftMotorReadyOffsetAddress = 8;   // M808
+    public static readonly ushort RightMotorReadyOffsetAddress = 9;  // M809
+    public static readonly ushort LiftMotorReadyOffsetAddress = 10; // M810
+
+    public static readonly ushort SwitchLockOffsetAddress = 11;   // M811 Lock
+    public static readonly ushort SwitchAutoOffsetAddress = 12;   // M812 Auto
+    public static readonly ushort SwitchManualOffsetAddress = 13; // M813 Manual
+
+    public static readonly ushort StartButtonOffsetAddress = 14;  // M814 Start
+    public static readonly ushort ResetButtonOffsetAddress = 15;  // M815 Reset
+    public static readonly ushort StopButtonOffsetAddress = 16;   // M816 Stop
+
+    public static readonly ushort HasLoadOffsetAddress = 17;           // M817 Báo có tải
+    public static readonly ushort EnabledChargerOffsetAddress = 18;   // M818 PLC charging contact
+    public static readonly ushort ResponseChargingOffsetAddress = 19; // M819 Response Charging
+    public static readonly ushort MutedBaseOffsetAddress = 20;       // M820 Response Muted Base
+    public static readonly ushort MutedLoadOffsetAddress = 21;        // M821 Response Muted Load
+
+    // Speed SLS: 2873-2879 (M825-M831)
+    public static readonly ushort SpeedLimitReadAddress = 0x0b39; // 2873, M825
+    public static readonly ushort SpeedRange = 7;
+    public static readonly ushort SpeedVerySlowOffetAddress = 0;   // M825 0.15
+    public static readonly ushort SpeedSlowOffetAddress = 1;       // M826 0.25
+    public static readonly ushort SpeedNormalOffetAddress = 2;      // M827 0.55
+    public static readonly ushort SpeedMediumOffetAddress = 3;      // M828 0.9
+    public static readonly ushort SpeedOptimalOffetAddress = 4;     // M829 1.28
+    public static readonly ushort SpeedFastOffetAddress = 5;       // M830 1.6
+    public static readonly ushort SpeedVeryFastOffetAddress = 6;     // M831 1.9 Overspeed
+
+    // Robot state: 2948-2957 (M900-M909 INIT, PAUSE, IDLE, PROCESSING, DOCKING, MAINTENANCE, MANUAL, OVERRIDE, CHARGING, Error)
+    public static readonly ushort RobotStateWriteAddress = 0x0b84; // 2948, M900
+    public static readonly bool[] RobotInitState = [true, false, false, false, false, false, false, false, false, false];
+    public static readonly bool[] RobotPauseState = [false, true, false, false, false, false, false, false, false, false];
+    public static readonly bool[] RobotIdleState = [false, false, true, false, false, false, false, false, false, false];
+    public static readonly bool[] RobotProccessingState = [false, false, false, true, false, false, false, false, false, false];
+    public static readonly bool[] RobotDockingState = [false, false, false, false, true, false, false, false, false, false];
+    public static readonly bool[] RobotMaintenanceState = [false, false, false, false, false, true, false, false, false, false];
+    public static readonly bool[] RobotManualState = [false, false, false, false, false, false, true, false, false, false];
+    public static readonly bool[] RobotOverrideState = [false, false, false, false, false, false, false, true, false, false];
+    public static readonly bool[] RobotCharingState = [false, false, false, false, false, false, false, false, true, false];
+    public static readonly bool[] RobotErrorState = [false, false, false, false, false, false, false, false, false, true];
+
+    // Movement/lift: 2963-2965 (M915 Moving, M916 Lifting, M917 Rotating)
+    public static readonly ushort RobotOperationWriteAddress = 0x0b93; // 2963, M915
+    public static readonly bool[] RobotExecuteClearState = [false, false, false];
+    public static readonly bool[] RobotExecuteMoveState = [true, false, false];
+    public static readonly bool[] RobotExecuteLiftingState = [false, true, false];
+    public static readonly bool[] RobotExecuteLiftRotatingState = [false, false, true];
+
+    // M918 Bật đèn — coil 2966 (TCP address)
+    public static readonly ushort SetLightOnAddress = 0x0b96; // 2966, M918 Bật đèn
+
+    // Actions: 2968-2977 (M920-M929)
+    public static readonly ushort EnableChargerAddress = 0x0b98;     // 2968 M920 Bắt tiếp điểm
+    public static readonly ushort SetHorizontalLoadAddress = 0x0b99;  // 2969 M921 Báo tải nằm ngang
+    public static readonly ushort SetMutedBaseAddress = 0x0b9a;     // 2970 M922 Set Muted Base
+    public static readonly ushort SetMutedLoadAddress = 0x0b9b;      // 2971 M923 Muted Load
+
+    public static readonly ushort SetRFModeAddress = 0x0b9c;         // 2972 M924-M926 RF Default/Maintenance/Override
+    public static readonly bool[] RFModeNone = [false, false, false];
+    public static readonly bool[] RFModeDefault = [true, false, false];
+    public static readonly bool[] RFModeMaintenance = [false, true, false];
+    public static readonly bool[] RFModeOverride = [false, false, true];
+
+
+    public static readonly ushort SetHasLoadAddress = 0x0b9f;   // 2975 M927 Báo có tải
+    public static readonly ushort SetRFEStopAddress = 0x0ba0;    // 2976 M928 EMC RF Remote
+    public static readonly ushort SetBatteryLowAddress = 0x0ba1; // 2977 M929 Pin yếu
+
+    /// <summary>Ghi M815 Alarm Reset xuống PLC — cùng coil với nút M815 (2848+15=2863), pulse ON rồi OFF để PLC alarm reset.</summary>
+    public static readonly ushort AlarmResetM815WriteAddress = (ushort)(ReadOnlyStartAddress + ResetButtonOffsetAddress); // 2863 M815
+
+    // Hướng di chuyển: truyền xuống PLC — tiến M931, lùi M932, không đi thì cả 2 off
+    public static readonly ushort DirectionForwardAddress = 0x0ba3;  // 2979 M931 Tiến
+    public static readonly ushort DirectionBackwardAddress = 0x0ba4; // 2980 M932 Lùi
+
+    // Lift module: Homing (pulse), Velocity (up/down coils), Position (holding register 32-bit)
+    public static readonly ushort LiftHomingAddress = 0x0ba5;           // M933 Lift homing (pulse)
+    public static readonly ushort LiftVelocityUpAddress = 0x0ba6;       // M934 Lift lên (velocity)
+    public static readonly ushort LiftVelocityDownAddress = 0x0ba7;    // M935 Lift xuống (velocity)
+    public static readonly ushort LiftTargetPositionRegister = 0x0bc0;  // D register: target position (32-bit = 2 registers). 10000 = 0.01m
+    public static readonly ushort LiftGoToPositionAddress = 0x0ba8;    // M936 Trigger di chuyển đến vị trí (pulse)
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Modules/RobotPlcController.cs

@@ -0,0 +1,466 @@
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Devices;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Services.ConfigManager;
+
+namespace RobotNet10.RobotApp.Services.Robot;
+
+public partial class RobotPlcController(IRobotConfiguration RobotConfiguration, IDeviceProvider DeviceProvider, Logger<RobotPlcController> Logger) : IPlcController
+{
+    public bool IsReady { get; private set; } = false;
+    public bool IsDisconected => !IsSimulation && (ModbusTcpDevice is null || !ModbusTcpDevice.IsConnected);
+    public event Action<SafetySpeed>? OnSafetySpeedChanged;
+    public event Action<OperatingMode>? OnPeripheralModeChanged;
+    public event Action<PeripheralButton>? OnButtonPressed;
+    public event Action<StopStateType>? OnStop;
+
+    private bool IsSimulation => RobotConfiguration.GetSimulationConfig().IsEnable;
+
+    private IModbusTcpDevice? ModbusTcpDevice;
+
+    // Edge detection tracking fields
+    private StopStateType _lastStopState = StopStateType.None;
+    private bool _lastButtonStart, _lastButtonReset, _lastButtonStop;
+
+    public async Task Start(CancellationToken cancellationToken)
+    {
+        LidarBackProtectField = true;
+        LidarFrontProtectField = true;
+        if (IsSimulation)
+        {
+            PeripheralMode = OperatingMode.AUTOMATIC;
+        }
+        else if (ModbusTcpDevice is null)
+        {
+            while (!cancellationToken.IsCancellationRequested)
+            {
+                if (DeviceProvider.AreDevicesLoaded) break;
+                await Task.Delay(500);
+            }
+
+            var device = DeviceProvider.GetDevice("plc-001");
+            if (device is IModbusTcpDevice modbusDevice)
+            {
+                ModbusTcpDevice = modbusDevice;
+                ModbusTcpDevice.DataRegisterChanged += ModbusDataChanged;
+                while (!cancellationToken.IsCancellationRequested)
+                {
+                    if (modbusDevice.IsConnected) break;
+                    await Task.Delay(500);
+                }
+            }
+            else return;
+        }
+        IsReady = true;
+    }
+
+    public void Stop()
+    {
+
+        ModbusTcpDevice?.DataRegisterChanged -= ModbusDataChanged;
+        ModbusTcpDevice = null;
+
+        // Reset edge detection state
+        _lastStopState = StopStateType.None;
+        _lastButtonStart = false;
+        _lastButtonReset = false;
+        _lastButtonStop = false;
+
+        IsReady = false;
+    }
+
+    private void ModbusDataChanged(ModbusRegisterType type)
+    {
+        if (type == ModbusRegisterType.Coil)
+        {
+            // Read-only data from PLC
+            ReadSafetyProtect();
+            ReadSafetySpeed();
+            ReadButton();
+            ReadSwitch();
+            ReadLiftState();
+            ReadMotorState();
+            ReadOtherState();
+        }
+    }
+
+    public void SetHorizontalLoad(bool value)
+    {
+        if (IsSimulation) return;
+        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
+        var write = ModbusTcpDevice.WriteCoilAsync(SetHorizontalLoadAddress, value, CancellationToken.None);
+        write.Wait();
+    }
+
+    public void SetMutedBase(bool muted)
+    {
+        if (IsSimulation) return;
+        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
+        var write = ModbusTcpDevice.WriteCoilAsync(SetMutedBaseAddress, muted, CancellationToken.None);
+        write.Wait();
+    }
+
+    public void SetMutedLoad(bool muted)
+    {
+        if (IsSimulation) return;
+        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
+        var write = ModbusTcpDevice.WriteCoilAsync(SetMutedLoadAddress, muted, CancellationToken.None);
+        write.Wait();
+    }
+
+    /// <summary>Bật/tắt đèn — ghi coil M918 (TCP address 2966).</summary>
+    public void SetLightOn(bool value)
+    {
+        if (IsSimulation) return;
+        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
+        var write = ModbusTcpDevice.WriteCoilAsync(SetLightOnAddress, value, CancellationToken.None);
+        write.Wait();
+    }
+
+    public void SetOperationState(OperationState state)
+    {
+        if (IsSimulation) return;
+        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
+        var write = Task.Run(async () =>
+        {
+            switch (state)
+            {
+                case OperationState.Move:
+                    await ModbusTcpDevice.WriteCoilsAsync(RobotOperationWriteAddress, RobotExecuteMoveState);
+                    break;
+                case OperationState.Lifting:
+                    await ModbusTcpDevice.WriteCoilsAsync(RobotOperationWriteAddress, RobotExecuteLiftingState);
+                    break;
+                case OperationState.LiftRotating:
+                    await ModbusTcpDevice.WriteCoilsAsync(RobotOperationWriteAddress, RobotExecuteLiftRotatingState);
+                    break;
+                case OperationState.None:
+                default:
+                    await ModbusTcpDevice.WriteCoilsAsync(RobotOperationWriteAddress, RobotExecuteClearState);
+                    break;
+            }
+        });
+        write.Wait();
+    }
+
+    public void SetSystemState(SystemState state)
+    {
+        if (IsSimulation) return;
+        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
+        var write = Task.Run(async () =>
+        {
+            switch (state)
+            {
+                case SystemState.INIT:
+                    await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotInitState);
+                    break;
+                case SystemState.PAUSED:
+                    await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotPauseState);
+                    break;
+                case SystemState.IDLE:
+                    await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotIdleState);
+                    break;
+                case SystemState.PROCCESSING:
+                    await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotProccessingState);
+                    break;
+                case SystemState.DOCKING:
+                    await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotDockingState);
+                    break;
+                case SystemState.MAINTENANCE:
+                    await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotMaintenanceState);
+                    break;
+                case SystemState.MANUAL:
+                    await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotManualState);
+                    break;
+                case SystemState.OVERRIDE:
+                    await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotOverrideState);
+                    break;
+                case SystemState.CHARGING:
+                    await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotCharingState);
+                    break;
+                case SystemState.ERROR:
+                    await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotErrorState);
+                    break;
+                default:
+                    break;
+            }
+        });
+        write.Wait();
+    }
+
+    public void SetEnableCharger(bool value)
+    {
+        if (IsSimulation) return;
+        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
+        var write = ModbusTcpDevice.WriteCoilAsync(EnableChargerAddress, value, CancellationToken.None);
+        write.Wait();
+    }
+
+    public void SetRFMode(RFMode mode)
+    {
+        if (IsSimulation) return;
+        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
+        var write = Task.Run(async () =>
+        {
+            switch (mode)
+            {
+                case RFMode.Default:
+                    await ModbusTcpDevice.WriteCoilsAsync(SetRFModeAddress, RFModeDefault);
+                    break;
+                case RFMode.Maintenance:
+                    await ModbusTcpDevice.WriteCoilsAsync(SetRFModeAddress, RFModeMaintenance);
+                    break;
+                case RFMode.Override:
+                    await ModbusTcpDevice.WriteCoilsAsync(SetRFModeAddress, RFModeOverride);
+                    break;
+                case RFMode.None:
+                default:
+                    await ModbusTcpDevice.WriteCoilsAsync(SetRFModeAddress, RFModeNone);
+                    break;
+            }
+        });
+        write.Wait();
+    }
+
+    public void SetHasLoad(bool hasLoad)
+    {
+        if (IsSimulation) return;
+        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
+        var write = ModbusTcpDevice.WriteCoilAsync(SetHasLoadAddress, hasLoad, CancellationToken.None);
+        write.Wait();
+    }
+
+    public void SetRFEStop(bool stop)
+    {
+        if (IsSimulation) return;
+        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
+        var write = ModbusTcpDevice.WriteCoilAsync(SetRFEStopAddress, stop, CancellationToken.None);
+        write.Wait();
+    }
+
+    public void SetBatteryLow(bool value)
+    {
+        if (IsSimulation) return;
+        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
+        var write = ModbusTcpDevice.WriteCoilAsync(SetBatteryLowAddress, value, CancellationToken.None);
+        write.Wait();
+    }
+
+    /// <summary>Ghi hướng di chuyển xuống PLC: tiến M931, lùi M932; không đi thì cả hai off.</summary>
+    public void SetDirectionForwardBackward(bool forward, bool backward)
+    {
+        if (IsSimulation) return;
+        if (ModbusTcpDevice is null) return;
+        // Tiến: M931 on, M932 off. Lùi: M931 off, M932 on. Không đi: cả hai off (không bao giờ cả hai on)
+        var w1 = ModbusTcpDevice.WriteCoilAsync(DirectionForwardAddress, forward, CancellationToken.None);
+        var w2 = ModbusTcpDevice.WriteCoilAsync(DirectionBackwardAddress, backward, CancellationToken.None);
+        Task.WaitAll(w1, w2);
+    }
+
+    /// <summary>Ghi M815 Alarm Reset xuống PLC (pulse) — gửi ngay ON rồi OFF, cùng coil như khi bấm M815 trên device.</summary>
+    public void WriteAlarmResetM815()
+    {
+        if (IsSimulation) return;
+        if (ModbusTcpDevice is null) return;
+        ushort addr = AlarmResetM815WriteAddress;
+        ModbusTcpDevice.WriteCoilImmediateAsync(addr, true, CancellationToken.None).GetAwaiter().GetResult();
+        Thread.Sleep(150);
+        ModbusTcpDevice.WriteCoilImmediateAsync(addr, false, CancellationToken.None).GetAwaiter().GetResult();
+        Logger.Info($"WriteAlarmResetM815: pulsed coil {addr} (M815) ON -> OFF");
+    }
+
+    /// <summary>Lift: Homing — pulse coil M933.</summary>
+    public void LiftHoming()
+    {
+        if (IsSimulation) return;
+        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected.");
+        ushort addr = LiftHomingAddress;
+        ModbusTcpDevice.WriteCoilImmediateAsync(addr, true, CancellationToken.None).GetAwaiter().GetResult();
+        Thread.Sleep(150);
+        ModbusTcpDevice.WriteCoilImmediateAsync(addr, false, CancellationToken.None).GetAwaiter().GetResult();
+        Logger.Info($"LiftHoming: pulsed coil {addr} (M933) ON -> OFF");
+    }
+
+    /// <summary>Lift: Điều khiển velocity — lên (M934), xuống (M935); cả hai off = dừng.</summary>
+    public void SetLiftVelocity(bool up, bool down)
+    {
+        if (IsSimulation) return;
+        if (ModbusTcpDevice is null) return;
+        var w1 = ModbusTcpDevice.WriteCoilAsync(LiftVelocityUpAddress, up, CancellationToken.None);
+        var w2 = ModbusTcpDevice.WriteCoilAsync(LiftVelocityDownAddress, down, CancellationToken.None);
+        Task.WaitAll(w1, w2);
+    }
+
+    /// <summary>Lift: Ghi vị trí đích (10000 = 0.01m) vào 2 holding registers rồi pulse M936.</summary>
+    public void SetLiftPositionAndGo(int position)
+    {
+        if (IsSimulation) return;
+        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected.");
+        var high = (ushort)((position >> 16) & 0xFFFF);
+        var low = (ushort)(position & 0xFFFF);
+        ModbusTcpDevice.WriteHoldingRegistersAsync(LiftTargetPositionRegister, [high, low], CancellationToken.None).GetAwaiter().GetResult();
+        ModbusTcpDevice.WriteCoilImmediateAsync(LiftGoToPositionAddress, true, CancellationToken.None).GetAwaiter().GetResult();
+        Thread.Sleep(150);
+        ModbusTcpDevice.WriteCoilImmediateAsync(LiftGoToPositionAddress, false, CancellationToken.None).GetAwaiter().GetResult();
+        Logger.Info($"SetLiftPositionAndGo: position={position} (10000=0.01m), pulsed M936");
+    }
+
+    private static SafetySpeed ReadSpeed(bool[] flag)
+    {
+        if (flag.Length < 7) return SafetySpeed.Very_Slow;
+        if (flag[0]) return SafetySpeed.Very_Slow;   // giới hạn chặt nhất
+        if (flag[1]) return SafetySpeed.Slow;
+        if (flag[2]) return SafetySpeed.Normal;
+        if (flag[3]) return SafetySpeed.Medium;
+        if (flag[4]) return SafetySpeed.Optimal;
+        if (flag[5]) return SafetySpeed.Fast;
+        if (flag[6]) return SafetySpeed.Very_Fast;
+
+        return SafetySpeed.Very_Fast; // không có giới hạn nào
+    }
+
+    private void ReadSafetySpeed()
+    {
+        var device = ModbusTcpDevice;
+        if (device is null) return;
+
+        bool[] speed = device.ReadCoils(SpeedLimitReadAddress, SpeedRange);
+        if (speed.Length == SpeedRange)
+        {
+            var activeSpeed = ReadSpeed(speed);
+            if (activeSpeed != SafetySpeed)
+            {
+                SafetySpeed = activeSpeed;
+                OnSafetySpeedChanged?.Invoke(activeSpeed);
+            } 
+        }
+        else Logger.Warning($"Read Safety Speed is failed: data length {speed.Length} is wrong.");
+    }
+
+    private void ReadButton()
+    {
+        var device = ModbusTcpDevice;
+        if (device is null) return;
+
+        bool[] buttons = device.ReadCoils((ushort)(ReadOnlyStartAddress + StartButtonOffsetAddress), 3);
+        if (buttons.Length == 3)
+        {
+            var newStart = buttons[0];
+            var newReset = buttons[1];
+            var newStop = buttons[2];
+
+            // Rising edge detection - only fire when button state changes from false to true
+            if (newStart && !_lastButtonStart) OnButtonPressed?.Invoke(PeripheralButton.Start);
+            if (newReset && !_lastButtonReset) OnButtonPressed?.Invoke(PeripheralButton.Reset);
+            if (newStop && !_lastButtonStop) OnButtonPressed?.Invoke(PeripheralButton.Stop);
+
+            // Update tracking state
+            _lastButtonStart = newStart;
+            _lastButtonReset = newReset;
+            _lastButtonStop = newStop;
+
+            // Update public properties
+            ButtonStart = newStart;
+            ButtonReset = newReset;
+            ButtonStop = newStop;
+        }
+        else Logger.Warning($"Read button is failed: data length {buttons.Length} is wrong.");
+    }
+
+    private void ReadSwitch()
+    {
+        var device = ModbusTcpDevice;
+        if (device is null) return;
+
+        bool[] switchs = device.ReadCoils((ushort)(ReadOnlyStartAddress + SwitchLockOffsetAddress), 3);
+        if (switchs.Length == 3)
+        {
+            var oldMode = PeripheralMode;
+            if (switchs[0])
+            {
+                PeripheralMode = OperatingMode.SERVICE;
+            }
+            else if (switchs[1])
+            {
+                PeripheralMode = OperatingMode.AUTOMATIC;
+            }
+            else if (switchs[2])
+            {
+                PeripheralMode = OperatingMode.MANUAL;
+            }
+            if (oldMode != PeripheralMode) OnPeripheralModeChanged?.Invoke(PeripheralMode);
+        }
+        else Logger.Warning($"Read switch mode is failed: data length {switchs.Length} is wrong.");
+    }
+
+    private void ReadSafetyProtect()
+    {
+        var device = ModbusTcpDevice;
+        if (device is null) return;
+
+        bool[] sensors = device.ReadCoils((ushort)(ReadOnlyStartAddress + EmergencyOffsetAddress), 5);
+        if (sensors.Length == 5)
+        {
+            Emergency = sensors[0];
+            Bumper = sensors[1];
+            LidarFrontProtectField = sensors[2];
+            LidarBackProtectField = sensors[3];
+            LidarFrontTimProtectField = sensors[4];
+
+            // Determine current stop state
+            StopStateType currentState;
+            if (Emergency) currentState = StopStateType.EMC;
+            else if (Bumper) currentState = StopStateType.Bumper;
+            else currentState = StopStateType.None;
+
+            // Only fire event when state actually changes
+            if (currentState != _lastStopState)
+            {
+                _lastStopState = currentState;
+                OnStop?.Invoke(currentState);
+            }
+        }
+        else Logger.Warning($"Read safety protect is failed: data length {sensors.Length} is wrong.");
+    }
+
+    /// <summary>
+    /// Update lift state from Modbus cache
+    /// </summary>
+    private void ReadLiftState()
+    {
+        var device = ModbusTcpDevice;
+        if (device is null) return;
+
+        LiftedUp = device.ReadCoil((ushort)(ReadOnlyStartAddress + LiftedUpOffsetAddress));
+        LiftedDown = device.ReadCoil((ushort)(ReadOnlyStartAddress + LiftedDownOffsetAddress));
+        LiftHome = device.ReadCoil((ushort)(ReadOnlyStartAddress + LiftHomeOffsetAddress));
+    }
+
+    /// <summary>
+    /// Update motor ready state from Modbus cache
+    /// </summary>
+    private void ReadMotorState()
+    {
+        var device = ModbusTcpDevice;
+        if (device is null) return;
+
+        LeftMotorReady = device.ReadCoil((ushort)(ReadOnlyStartAddress + LeftMotorReadyOffsetAddress));
+        RightMotorReady = device.ReadCoil((ushort)(ReadOnlyStartAddress + RightMotorReadyOffsetAddress));
+        LiftMotorReady = device.ReadCoil((ushort)(ReadOnlyStartAddress + LiftMotorReadyOffsetAddress));
+    }
+
+    /// <summary>
+    /// Update other state from Modbus cache
+    /// </summary>
+    private void ReadOtherState()
+    {
+        var device = ModbusTcpDevice;
+        if (device is null) return;
+
+        HasLoad = device.ReadCoil((ushort)(ReadOnlyStartAddress + HasLoadOffsetAddress));
+        EnabledCharger = device.ReadCoil((ushort)(ReadOnlyStartAddress + EnabledChargerOffsetAddress));
+        Charging = device.ReadCoil((ushort)(ReadOnlyStartAddress + ResponseChargingOffsetAddress));
+        MutedBase = device.ReadCoil((ushort)(ReadOnlyStartAddress + MutedBaseOffsetAddress));
+        MutedLoad = device.ReadCoil((ushort)(ReadOnlyStartAddress + MutedLoadOffsetAddress));
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Modules/RobotPlcData.cs

@@ -0,0 +1,111 @@
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Interfaces;
+
+namespace RobotNet10.RobotApp.Services.Robot;
+
+public partial class RobotPlcController
+{
+    public OperatingMode PeripheralMode { get; private set; }
+    public SafetySpeed SafetySpeed { get; private set; }
+
+    public bool Emergency { get; private set; }
+    public bool Bumper { get; private set; }
+
+    public bool LidarFrontProtectField { get; private set; }
+    public bool LidarBackProtectField { get; private set; }
+    public bool LidarFrontTimProtectField { get; private set; }
+
+    // Lift state - now cached instead of direct read
+    public bool LiftedUp { get; private set; }
+    public bool LiftedDown { get; private set; }
+    public bool LiftHome { get; private set; }
+
+    // Motor state - now cached instead of direct read
+    public bool LeftMotorReady { get; private set; }
+    public bool RightMotorReady { get; private set; }
+    public bool LiftMotorReady { get; private set; }
+
+    public bool ButtonStart { get; private set; }
+    public bool ButtonStop { get; private set; }
+    public bool ButtonReset { get; private set; }
+
+    // Other state - now cached instead of direct read
+    public bool HasLoad { get; private set; }
+    public bool EnabledCharger { get; private set; }
+    public bool Charging { get; private set; }
+    public bool MutedBase { get; private set; }
+    public bool MutedLoad { get; private set; }
+
+    // Write state tracking - đọc từ write addresses của PLC
+    public SystemState CurrentSystemState => ReadSystemState();
+    public OperationState CurrentOperationState => ReadOperationState();
+    public RFMode CurrentRFMode => ReadRFMode();
+    public bool SetHorizontalLoadValue => ModbusTcpDevice?.ReadCoil(SetHorizontalLoadAddress) ?? false;
+    public bool SetMutedBaseValue => ModbusTcpDevice?.ReadCoil(SetMutedBaseAddress) ?? false;
+    public bool SetMutedLoadValue => ModbusTcpDevice?.ReadCoil(SetMutedLoadAddress) ?? false;
+    public bool SetEnableChargerValue => ModbusTcpDevice?.ReadCoil(EnableChargerAddress) ?? false;
+    public bool SetHasLoadValue => ModbusTcpDevice?.ReadCoil(SetHasLoadAddress) ?? false;
+    public bool SetRFEStopValue => ModbusTcpDevice?.ReadCoil(SetRFEStopAddress) ?? false;
+    public bool SetBatteryLowValue => ModbusTcpDevice?.ReadCoil(SetBatteryLowAddress) ?? false;
+    public bool SetLightOnValue => ModbusTcpDevice?.ReadCoil(SetLightOnAddress) ?? false;
+
+    private SystemState ReadSystemState()
+    {
+        var device = ModbusTcpDevice;
+        if (device is null) return SystemState.INIT;
+
+        bool[] states = device.ReadCoils(RobotStateWriteAddress, 10);
+        if (states.Length == 10)
+        {
+            // Decode state from one-hot encoded coils
+            if (states[0]) return SystemState.INIT;
+            else if (states[1]) return SystemState.PAUSED;
+            else if (states[2]) return SystemState.IDLE;
+            else if (states[3]) return SystemState.PROCCESSING;
+            else if (states[4]) return SystemState.DOCKING;
+            else if (states[5]) return SystemState.MAINTENANCE;
+            else if (states[6]) return SystemState.MANUAL;
+            else if (states[7]) return SystemState.OVERRIDE;
+            else if (states[8]) return SystemState.CHARGING;
+            else if (states[9]) return SystemState.ERROR;
+        }
+        else Logger.Warning($"Read system state is failed: data length {states.Length} is wrong.");
+        return SystemState.INIT;
+    }
+
+    private OperationState ReadOperationState()
+    {
+        var device = ModbusTcpDevice;
+        if (device is null) return OperationState.None;
+
+        bool[] states = device.ReadCoils(RobotOperationWriteAddress, 3);
+        if (states.Length == 3)
+        {
+            // Decode operation state from one-hot encoded coils
+            if (states[0]) return OperationState.Move;
+            else if (states[1]) return OperationState.Lifting;
+            else if (states[2]) return OperationState.LiftRotating;
+            else return OperationState.None;
+        }
+        else Logger.Warning($"Read operation state is failed: data length {states.Length} is wrong.");
+        return OperationState.None;
+    }
+
+    private RFMode ReadRFMode()
+    {
+        var device = ModbusTcpDevice;
+        if (device is null) return RFMode.None;
+
+        bool[] modes = device.ReadCoils(SetRFModeAddress, 3);
+        if (modes.Length == 3)
+        {
+            // Decode RF mode from one-hot encoded coils
+            if (modes[0]) return RFMode.Default;
+            else if (modes[1]) return RFMode.Maintenance;
+            else if (modes[2]) return RFMode.Override;
+            else return RFMode.None;
+        }
+        else Logger.Warning($"Read RF mode is failed: data length {modes.Length} is wrong.");
+        return RFMode.None;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/RobotActionController.cs

@@ -0,0 +1,277 @@
+using RobotNet.VDA5050;
+using RobotNet.VDA5050.State;
+using RobotNet.VDA5050.Type;
+using RobotNet10.Common;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Services.Exceptions;
+using RobotNet10.RobotApp.Services.Robot.Actions;
+using RobotNet10.RobotApp.Services.Robot.Helper;
+using System.Collections.Concurrent;
+using System.Diagnostics;
+
+namespace RobotNet10.RobotApp.Services.Robot;
+
+public class RobotActionController(ILogger<RobotActionController> Logger,
+                                   IRobotActionProvider RobotActionProvider,
+                                   IError ErrorManager,
+                                   INavigation NavigationManager,
+                                   IServiceScopeFactory ServiceScopeFactory) : BackgroundService, IAction
+{
+    public ActionState[] ActionStates => [.. Actions.Values.OrderBy(a => a.SequenceNumber).Select(a => new ActionState
+        {
+            ActionId = a.Id,
+            ActionType = a.Type.ToJsonString(),
+            ActionDescription = a.Description,
+            ActionStatus = a.Status,
+            ResultDescription = a.ResultDescription,
+        })];
+    public bool HasActionRunning => !ActionQueue.IsEmpty || Actions.Values.Any(a => a.Type != ActionType.CANCEL_ORDER && !a.IsCompleted);
+    public bool HasActionWaitting => !ActionQueue.IsEmpty;
+
+    private readonly ConcurrentDictionary<string, RobotAction> Actions = [];
+    private readonly ConcurrentQueue<(ActionScope scope, RobotNet.VDA5050.InstantAction.Action action)> ActionQueue = [];
+    private readonly ActionConflictDetector _conflictDetector = new();
+
+    private WatchThread<RobotActionController>? HandlerTimer;
+    private const int HandlerInterval = 200;
+    private const int CompletedActionRetentionMs = 300000; // 5 minutes
+    private int _cleanupCounter = 0;
+    private const int CleanupIntervalIterations = 50; // Cleanup every 50 iterations (10 seconds)
+    private volatile bool _isClearing = false;
+    private long _sequenceCounter = 0;
+
+    public RobotAction? this[string actionId] => Actions.TryGetValue(actionId, out RobotAction? action) && action is not null ? action : null;
+
+    public void AddInstantAction(RobotNet.VDA5050.InstantAction.Action[] actions)
+    {
+        using var scope = ServiceScopeFactory.CreateAsyncScope();
+        var OrderManager = scope.ServiceProvider.GetRequiredService<IOrder>();
+        foreach (var action in actions)
+        {
+            if (Actions.TryGetValue(action.ActionId, out _)) continue;
+
+            // VDA5050: Check for conflicts with ALL running actions (ORDER + INSTANT)
+            var runningActions = Actions.Values.Where(a => !a.IsCompleted).ToList();
+            bool isOrderActive = OrderManager.NodeStates.Length > 0 || OrderManager.EdgeStates.Length > 0;
+            bool isDriving = NavigationManager.Driving;
+
+            var conflictResult = _conflictDetector.CheckConflict(action, runningActions, isOrderActive, isDriving);
+
+            if (conflictResult.HasConflict)
+            {
+                // Reject instant action and report error
+                var error = new RobotError
+                {
+                    ErrorType = "instantActionConflict",
+                    ErrorLevel = ErrorLevel.WARNING,
+                    ErrorDescription = $"INSTANT action {action.ActionType} bị từ chối: {conflictResult.Description}",
+                    ErrorReferences = [
+                        new() { ReferenceKey = "actionId", ReferenceValue = action.ActionId },
+                        new() { ReferenceKey = "conflictType", ReferenceValue = conflictResult.Type.ToString() }
+                    ]
+                };
+
+                if (!string.IsNullOrEmpty(conflictResult.ConflictingActionId))
+                {
+                    error.ErrorReferences = [
+                        .. error.ErrorReferences,
+                        new() { ReferenceKey = "conflictingActionId", ReferenceValue = conflictResult.ConflictingActionId }
+                    ];
+                }
+
+                ErrorManager.AddError(error, TimeSpan.FromSeconds(10));
+                Logger.LogWarning($"INSTANT action {action.ActionId} (type: {action.ActionType}) rejected due to conflict: {conflictResult.Description}");
+                continue; // Skip this action
+            }
+
+            // No conflict - add to queue
+            ActionQueue.Enqueue((ActionScope.INSTANT, action));
+        }
+    }
+
+    public void AddOrderActions(RobotNet.VDA5050.InstantAction.Action[] actions, ActionScope scope = ActionScope.NODE)
+    {
+        foreach (var action in actions)
+        {
+            if (Actions.TryGetValue(action.ActionId, out _)) continue;
+            ActionQueue.Enqueue((scope, action));
+        }
+    }
+
+    public IEnumerable<RobotAction> GetRunningActions()
+    {
+        return Actions.Values.Where(a => !a.IsCompleted);
+    }
+
+    public void StartOrderAction(string actionId)
+    {
+        if (Actions.TryGetValue(actionId, out RobotAction? robotAction) && robotAction is not null)
+        {
+            robotAction.Start();
+        }
+    }
+
+    public void StopOrderAction(string actionId = "")
+    {
+        if (string.IsNullOrEmpty(actionId))
+        {
+            foreach (var action in Actions.Values)
+            {
+                if (!action.IsCompleted && action.Type != ActionType.CANCEL_ORDER) action.Cancel();
+            }
+        }
+        else if (Actions.TryGetValue(actionId, out RobotAction? robotAction) && robotAction is not null) robotAction.Cancel();
+    }
+
+    public void FinishAction(string actionId)
+    {
+        if (Actions.TryGetValue(actionId, out RobotAction? robotAction) && robotAction is not null)
+        {
+            robotAction.Finish();
+        }
+    }
+
+    public void PauseActions()
+    {
+        foreach (var action in Actions.Values)
+        {
+            action.Pause();
+        }
+    }
+
+    public void ResumeActions()
+    {
+        foreach (var action in Actions.Values)
+        {
+            action.Resume();
+        }
+    }
+
+    private void ActionHandler()
+    {
+        if (_isClearing) return;
+
+        while (!ActionQueue.IsEmpty)
+        {
+            if (!ActionQueue.TryDequeue(out var result)) continue;
+            if (Actions.ContainsKey(result.action.ActionId)) continue;
+
+            RobotAction? robotAction = null;
+            try
+            {
+                if (EnumHelper.TryParse(result.action.ActionType, out ActionType actionType))
+                {
+                    robotAction = RobotActionProvider.GetRobotAction(actionType);
+                    if (robotAction is not null)
+                    {
+                        robotAction.Initialize(result.scope, result.action);
+                        robotAction.SequenceNumber = Interlocked.Increment(ref _sequenceCounter);
+                        Actions.TryAdd(result.action.ActionId, robotAction);
+                        if (result.scope == ActionScope.INSTANT) robotAction.Start();
+                    }
+                }
+                else
+                {
+                    var error = new RobotError
+                    {
+                        ErrorType = "actionTypeInvalid",
+                        ErrorLevel = RobotNet.VDA5050.Type.ErrorLevel.WARNING,
+                        ErrorDescription = $"ActionType không hợp lệ: {result.action.ActionType}",
+                        ErrorReferences = [new() { ReferenceKey = "actionId", ReferenceValue = result.action.ActionId }]
+                    };
+                    ErrorManager.AddError(error, TimeSpan.FromSeconds(10));
+                    Logger.LogWarning("ActionType không hợp lệ: {ActionType} cho action {ActionId}", result.action.ActionType, result.action.ActionId);
+                }
+            }
+            catch (Exception ex)
+            {
+                var errorMsg = ex is RobotException rex && rex.Error is not null ? rex.Error.ErrorDescription : ex.Message;
+
+                // Nếu robotAction đã tạo, mark FAILED và add vào Actions để FM nhận được trạng thái
+                if (robotAction is not null)
+                {
+                    robotAction.Cancel();
+                    robotAction.ResultDescription = $"Khởi tạo action thất bại: {errorMsg}";
+                    robotAction.SequenceNumber = Interlocked.Increment(ref _sequenceCounter);
+                    Actions.TryAdd(result.action.ActionId, robotAction);
+                }
+
+                var error = new RobotError
+                {
+                    ErrorType = "actionInitializationFailed",
+                    ErrorLevel = RobotNet.VDA5050.Type.ErrorLevel.WARNING,
+                    ErrorDescription = $"Action {result.action.ActionId} ({result.action.ActionType}) khởi tạo thất bại: {errorMsg}",
+                    ErrorReferences = [new() { ReferenceKey = "actionId", ReferenceValue = result.action.ActionId }]
+                };
+                ErrorManager.AddError(error, TimeSpan.FromSeconds(10));
+                Logger.LogWarning("Action {ActionId} (type: {ActionType}) initialization failed: {Error}", result.action.ActionId, result.action.ActionType, errorMsg);
+            }
+        }
+
+        // Cleanup completed actions periodically to prevent memory leak
+        _cleanupCounter++;
+        if (_cleanupCounter >= CleanupIntervalIterations)
+        {
+            _cleanupCounter = 0;
+            CleanupCompletedActions();
+        }
+    }
+
+    private void CleanupCompletedActions()
+    {
+        try
+        {
+            long currentTime = Stopwatch.GetTimestamp() * 1000 / Stopwatch.Frequency;
+            var actionsToRemove = Actions.Where(kvp =>
+                kvp.Value.IsCompleted &&
+                kvp.Value.CompletionTime > 0 &&
+                (currentTime - kvp.Value.CompletionTime) > CompletedActionRetentionMs
+            ).Select(kvp => kvp.Key).ToList();
+
+            foreach (var actionId in actionsToRemove)
+            {
+                if (Actions.TryGetValue(actionId, out var action))
+                {
+                    _ = action.DisposeAsync(); // Fire and forget disposal
+                    Actions.TryRemove(actionId, out _);
+                    Logger.LogDebug($"Cleaned up completed action: {actionId} (Type: {action.Type})");
+                }
+            }
+
+            if (actionsToRemove.Count > 0)
+            {
+                Logger.LogInformation($"Cleaned up {actionsToRemove.Count} completed actions");
+            }
+        }
+        catch (Exception ex)
+        {
+            Logger.LogWarning($"Error during action cleanup: {ex.Message}");
+        }
+    }
+
+    public async Task ClearActions()
+    {
+        _isClearing = true;
+        ActionQueue.Clear();
+        var disposeTasks = Actions.Values.Select(action => action.DisposeAsync().AsTask()).ToList();
+        await Task.WhenAll(disposeTasks).ConfigureAwait(false);
+        Actions.Clear();
+        ActionQueue.Clear(); // Clear lần nữa phòng trường hợp có action mới enqueue trong lúc dispose
+        Interlocked.Exchange(ref _sequenceCounter, 0);
+        _isClearing = false;
+    }
+
+    protected override async Task ExecuteAsync(CancellationToken stoppingToken)
+    {
+        await Task.Yield();
+        HandlerTimer = new(HandlerInterval, ActionHandler, Logger);
+        HandlerTimer.Start();
+    }
+
+    public override Task StopAsync(CancellationToken cancellationToken)
+    {
+        HandlerTimer?.Dispose();
+        HandlerTimer = null;
+        return base.StopAsync(cancellationToken);
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/RobotController.cs

@@ -0,0 +1,732 @@
+using RobotNet.VDA5050.Type;
+using RobotNet10.Common;
+using RobotNet10.RobotApp.Devices;
+using RobotNet10.RobotApp.Events;
+using RobotNet10.RobotApp.Events.Events;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Modules;
+using RobotNet10.RobotApp.Motion;
+using RobotNet10.RobotApp.Services.ConfigManager;
+using RobotNet10.RobotApp.Services.Exceptions;
+using RobotNet10.RobotApp.Services.Robot.Connection;
+using RobotNet10.RobotApp.Services.State;
+
+namespace RobotNet10.RobotApp.Services.Robot;
+
+public partial class RobotController(IOrder OrderManager,
+                             INavigation NavigationManager,
+                             IAction ActionManager,
+                             IPlcController PlcController,
+                             IDeviceProvider DeviceProvider,
+                             IConfiguration Configuration,
+                             IError ErrorManager,
+                             Logger<RobotController> Logger,
+                             IRobotConnectionsService RobotConnectionsService,
+                             IRobotEventBus RobotEventBus,
+                             RobotStateMachine StateManager,
+                             ManualControlService RFControl,
+                             PS5ControllerService Ps5Controller,
+                             IRobotConfiguration RobotConfiguration,
+                             RobotStates StateService,
+                             RobotVisualization VisualizationService,
+                             ILiftModule LiftModule,
+                             ILocalization Localization,
+                             IRotationModule RotateModule,
+                             IInverseKinematics? InverseKinematics = null) : BackgroundService, IRobotController
+{
+    private readonly Mutex NewOrderMutex = new();
+    private readonly Mutex NewInstanceMutex = new();
+    private readonly Lock _stateTransitionLock = new();
+    private WatchThread<RobotController>? _watchTimer;
+    private bool _rfHandleHasPriority = false;
+    private IBattery? Battery;
+    private OperatingMode _previousPlcMode = OperatingMode.SERVICE;
+    private double _batteryLowThresholdPercent = 20.0;
+
+    protected override async Task ExecuteAsync(CancellationToken stoppingToken)
+    {
+        await Task.Yield();
+        await StateManager.InitializeAsync();
+        while (!stoppingToken.IsCancellationRequested)
+        {
+            if (StateManager.CurrentState == RobotStateType.Standby) break;
+            await Task.Delay(1000, stoppingToken);
+        }
+
+        // Subscribe to PLC events
+        PlcController.OnPeripheralModeChanged += OnPlcModeChanged;
+        PlcController.OnStop += OnStop;
+        PlcController.OnButtonPressed += OnButtonPressed;
+
+        // Subscribe to RF Handle mode changes (via RobotController for PLC sync)
+        RFControl.OnRfModeChanged += OnRfModeChanged;
+
+        // Subscribe to fatal errors
+        ErrorManager.OnNewFatalError += OnNewFatalError;
+
+        // Start WatchThread at 5Hz (200ms)
+        _watchTimer = new WatchThread<RobotController>(200, WatchThreadCallback, null);
+        _watchTimer.Start();
+
+        var deviceBattery = DeviceProvider.GetDeviceByType(Client.Shared.Devices.DeviceType.Battery);
+        if(deviceBattery is IBattery battery) Battery = battery;
+        _batteryLowThresholdPercent = ResolveBatteryLowThresholdPercent();
+
+        // Initial mode switch based on current PLC mode
+        _previousPlcMode = PlcController.PeripheralMode;
+        SwitchModeChanged(PlcController.PeripheralMode);
+        PlcController.SetRFMode(RFMode.None);
+        while (!stoppingToken.IsCancellationRequested)
+        {
+            if(RFControl.IsRunning)
+            {
+                RFControl.Start();
+                break;
+            }
+            await Task.Delay(2000);
+        }
+    }
+
+    public override Task StopAsync(CancellationToken cancellationToken)
+    {
+        StopHandler();
+        return base.StopAsync(cancellationToken);
+    }
+
+    public async Task ModuleInitializeAsync()
+    {
+        while (true)
+        {
+            if (StateManager.IsInitialized) break;
+            await Task.Delay(500);
+        }
+
+        // Start MQTT independently so connection topics are available even if hardware init is slow.
+        _ = RobotConnectionsService.StartAsync(CancellationToken.None);
+        // Start VDA5050 publishers early so state/visualization topics keep updating.
+        StateService.Start();
+        VisualizationService.Start();
+
+        while (true)
+        {
+            try
+            {
+                if (!RobotConfiguration.GetSimulationConfig().IsEnable)
+                {
+                    Logger.Info("Checking hardware...");
+
+                    await PlcController.Start(CancellationToken.None);
+
+                    while (!PlcController.IsReady || !DeviceProvider.AreDevicesConnected)
+                    {
+                        // (!DeviceProvider.AreDevicesConnected) Logger.Info(" - Devices service not ready");
+                        //if (!PlcController.IsReady) Logger.Info(" - Peripheral service not ready");
+                        if (PlcController.IsReady) PlcController.SetSystemState(SystemState.INIT);
+                        // if (!LiftModule.IsReady) Logger.Info(" - LiftModule service not ready");
+                        // if (!RotateModule.IsReady) Logger.Info(" - RotateModule service not ready");
+                        await Task.Delay(3000);
+                    }
+                }
+                Logger.Info("Hardware modules ready");
+
+                // Start software modules independently
+                NavigationManager.Start();
+
+                StateManager.Fire(RobotEventType.InitializeCompleted);
+                Logger.Info("Initialization completed");
+                break;
+            }
+            catch (Exception ex)
+            {
+                Logger.Warning($"Robot initialize failed: {ex.Message}");
+                await Task.Delay(2000);
+            }
+        }
+    }
+
+    public void StopHandler()
+    {
+        _watchTimer?.Dispose();
+        _watchTimer = null;
+
+        if (RobotConnectionsService.IsConnected)
+        {
+            var pubOffline = RobotConnectionsService.PublishConnectionStateAsync(ConnectionState.OFFLINE);
+            pubOffline.Wait();
+        }
+
+        var stopConnection = RobotConnectionsService.StopAsync();
+        stopConnection.Wait();
+        RobotEventBus.OrderMessageReceived -= NewOrderUpdated;
+        RobotEventBus.InstantActionReceived -= NewInstantActionUpdated;
+        NavigationManager.Stop();
+        PlcController.Stop();
+        PlcController.OnPeripheralModeChanged -= OnPlcModeChanged;
+        PlcController.OnStop -= OnStop;
+        PlcController.OnButtonPressed -= OnButtonPressed;
+        RFControl.OnRfModeChanged -= OnRfModeChanged;
+        ErrorManager.OnNewFatalError -= OnNewFatalError;
+    }
+
+    public void NewOrderUpdated(object? sender, OrderChangedEvent e)
+    {
+        if (NewOrderMutex.WaitOne(2000))
+        {
+            try
+            {
+                var orderMsg = e.OrderMessage;
+                if (!StateManager.IsInState(RobotStateType.Auto)) throw new OrderException(RobotErrors.Error1006(StateManager.CurrentState.ToString()));
+                if (!Localization.IsReady) throw new OrderException(RobotErrors.Error3001());
+                OrderManager.UpdateOrder(orderMsg);
+            }
+            catch (RobotException orEx)
+            {
+                if (orEx.Error is not null)
+                {
+                    ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10));
+                    Logger.Warning($"New order error: {orEx.Error.ErrorDescription}");
+                }
+                else Logger.Warning($"New order error: {orEx.Message}");
+            }
+            catch (Exception ex)
+            {
+                Logger.Warning($"Order processing error: {ex.Message}");
+            }
+            finally
+            {
+                NewOrderMutex.ReleaseMutex();
+            }
+        }
+    }
+
+    public void NewInstantActionUpdated(object? sender, InstantActionChangedEvent e)
+    {
+        if (NewInstanceMutex.WaitOne(2000))
+        {
+            try
+            {
+                var instantAction = e.InstantActionMessage;
+
+                // VDA5050: Filter instant actions based on current robot state
+                var filteredActions = FilterInstantActionsByState(instantAction.Actions);
+
+                if (filteredActions.Length > 0)
+                {
+                    ActionManager.AddInstantAction(filteredActions);
+                }
+            }
+            catch (RobotException acEx)
+            {
+                if (acEx.Error is not null)
+                {
+                    ErrorManager.AddError(acEx.Error, TimeSpan.FromSeconds(10));
+                    Logger.Warning($"InstantAction error: {acEx.Error.ErrorDescription}");
+                }
+                else Logger.Warning($"InstantAction error: {acEx.Message}");
+            }
+            catch (Exception ex)
+            {
+                Logger.Warning($"InstantAction processing error: {ex.Message}");
+            }
+            finally
+            {
+                NewInstanceMutex.ReleaseMutex();
+            }
+        }
+    }
+
+    /// <summary>
+    /// Filter instant actions based on current robot state for security and safety
+    /// </summary>
+    private RobotNet.VDA5050.InstantAction.Action[] FilterInstantActionsByState(RobotNet.VDA5050.InstantAction.Action[] actions)
+    {
+        var currentState = StateManager.CurrentState;
+        var allowedActions = new List<RobotNet.VDA5050.InstantAction.Action>();
+
+        foreach (var action in actions)
+        {
+            bool isAllowed = IsActionAllowedInState(action.ActionType, currentState);
+
+            if (isAllowed)
+            {
+                allowedActions.Add(action);
+            }
+            else
+            {
+                // VDA5050: Report rejected instant action as error
+                var error = new RobotError
+                {
+                    ErrorType = "instantActionRejected",
+                    ErrorLevel = RobotNet.VDA5050.Type.ErrorLevel.WARNING,
+                    ErrorDescription = $"Instant action '{action.ActionType}' rejected - not allowed in state '{currentState}'",
+                    ErrorReferences = [
+                        new() { ReferenceKey = "actionId", ReferenceValue = action.ActionId },
+                        new() { ReferenceKey = "actionType", ReferenceValue = action.ActionType },
+                        new() { ReferenceKey = "robotState", ReferenceValue = currentState.ToString() }
+                    ]
+                };
+                ErrorManager.AddError(error, TimeSpan.FromSeconds(10));
+                Logger.Warning($"Instant action {action.ActionId} (type: {action.ActionType}) rejected - not allowed in state {currentState}");
+            }
+        }
+
+        return [.. allowedActions];
+    }
+
+    /// <summary>
+    /// Define which instant actions are allowed in each robot state
+    /// </summary>
+    private static bool IsActionAllowedInState(string actionType, RobotStateType state)
+    {
+        // Actions allowed in ALL states (read-only or critical control)
+        var alwaysAllowedActions = new HashSet<string>
+        {
+            "cancelOrder",       // VDA5050: Must work in all states
+            "stateRequest",      // Read-only
+            "factsheetRequest",  // Read-only
+        };
+
+        if (alwaysAllowedActions.Contains(actionType)) return true;
+
+        // Actions allowed only in Auto state
+        if (state == RobotStateType.Auto ||
+            state == RobotStateType.Idle ||
+            state == RobotStateType.Executing ||
+            state == RobotStateType.Paused ||
+            state == RobotStateType.Canceling)
+        {
+            return true;  // All actions allowed in Auto mode
+        }
+
+        // Shared set of maintenance/setup actions (used in Service, Manual, System, Standby)
+        var maintenanceAllowedActions = new HashSet<string>
+        {
+            "initPosition",
+            "pick",
+            "drop",
+            "rotate",
+            "liftRotate",
+            "homingCamera",
+            "liftCameraByHeight",
+            "controlLight",
+            "cameraLightOn",
+            "cameraLightOff",
+            "mutedBaseOn",
+            "mutedBaseOff",
+            "mutedLoadOn",
+            "mutedLoadOff",
+            "dockTo",
+            "moveStraightToCoor",
+            "moveStraightWithDistance"
+        };
+
+        // Actions allowed in Service/Override/Manual states (maintenance/manual control)
+        if (state == RobotStateType.Service ||
+            state == RobotStateType.Remote_Override ||
+            state == RobotStateType.Manual)
+        {
+            return maintenanceAllowedActions.Contains(actionType);
+        }
+
+        // After ReleaseStop robot goes to System/Standby - allow maintenance actions so operator can e.g. lift camera before switching mode
+        if (state == RobotStateType.System || state == RobotStateType.Standby)
+        {
+            return maintenanceAllowedActions.Contains(actionType);
+        }
+
+        // Stop and Fault states: only critical control actions
+        if (state == RobotStateType.Stop || state == RobotStateType.Fault)
+        {
+            // Already handled by alwaysAllowedActions above
+            return false;
+        }
+
+        // Default: reject
+        return false;
+    }
+
+    public void Pause()
+    {
+        OrderManager.PauseOrder();
+        ActionManager.PauseActions();
+    }
+
+    public void Resume()
+    {
+        OrderManager.ResumeOrder();
+        ActionManager.ResumeActions();
+    }
+
+    public bool TryClearFault()
+    {
+        lock (_stateTransitionLock)
+        {
+            if (!StateManager.IsInState(RobotStateType.Fault)) return false;
+
+            if (PlcController.IsReady && !PlcController.IsDisconected)
+                ErrorManager.DeleteErrorId(2003);
+
+            ErrorManager.ClearFatalErrors();
+
+            if (!ErrorManager.HasFatalError)
+            {
+                Logger.Info("TryClearFault: Exiting Fault");
+                StateManager.Fire(RobotEventType.ExitFault);
+                return true;
+            }
+            return false;
+        }
+    }
+
+    private void OnPlcModeChanged(OperatingMode mode)
+    {
+        lock (_stateTransitionLock)
+        {
+            if (_rfHandleHasPriority)
+            {
+                Logger.Info($"PLC mode change to {mode} ignored - RF Handle has priority");
+                return;
+            }
+            // Khi chuyển từ Lock (SERVICE) sang Auto hoặc Manual: ghi M815 xuống PLC, reset fault, enable động cơ
+            if (_previousPlcMode == OperatingMode.SERVICE && (mode == OperatingMode.AUTOMATIC || mode == OperatingMode.MANUAL))
+            {
+                Logger.Info($"PLC Lock -> {mode}: áp dụng ApplyResetFromPlc (M815 + fault reset + enable)");
+                ApplyResetFromPlc();
+            }
+            _previousPlcMode = mode;
+            SwitchModeChanged(mode);
+        }
+    }
+
+    /// <summary>Reset theo PLC (M815): ghi M815 xuống PLC (pulse), clear fault robot, reset fault động cơ, enable lại động cơ (retry đến khi OperationEnabled).</summary>
+    private void ApplyResetFromPlc()
+    {
+        Logger.Info("ApplyResetFromPlc: bắt đầu (M815 pulse, clear fault, fault reset + enable drive)");
+        try { PlcController.WriteAlarmResetM815(); } catch (Exception ex) { Logger.Warning($"WriteAlarmResetM815: {ex.Message}"); }
+        TryClearFault();
+        try
+        {
+            InverseKinematics?.FaultReset();
+            // Đợi servo thoát Fault (CiA402 có thể cần >1s để cập nhật statusword)
+            Thread.Sleep(1500);
+            InverseKinematics?.FaultReset();
+            Thread.Sleep(800);
+            // Enable 2 động cơ giống enable bằng tay trên device: gửi lệnh trực tiếp, await từng bước
+            if (InverseKinematics != null)
+            {
+                InverseKinematics.EnableAsync(CancellationToken.None).GetAwaiter().GetResult();
+                if (InverseKinematics.IsOperationEnabled)
+                    Logger.Info("ApplyResetFromPlc: 2 động cơ đã enable (OperationEnabled)");
+                else
+                    Logger.Warning("ApplyResetFromPlc: động cơ chưa lên OperationEnabled sau EnableAsync");
+            }
+        }
+        catch (Exception ex) { Logger.Warning($"FaultReset/Enable drive: {ex.Message}"); }
+    }
+
+    private void SwitchModeChanged(OperatingMode mode)
+    {
+        // Pause order when leaving Auto mode
+        if (StateManager.IsInState(RobotStateType.Auto) && mode != OperatingMode.AUTOMATIC)
+        {
+            Pause();
+            RobotEventBus.OrderMessageReceived -= NewOrderUpdated;
+            // Keep InstantActionReceived subscription - instant actions (e.g. cancelOrder) must work in all states
+        }
+
+        switch (mode)
+        {
+            case OperatingMode.AUTOMATIC:
+                Ps5Controller.Disable();
+                StateManager.Fire(RobotEventType.EnterAuto);
+                // Prevent duplicate subscriptions
+                RobotEventBus.OrderMessageReceived -= NewOrderUpdated;
+                RobotEventBus.OrderMessageReceived += NewOrderUpdated;
+                RobotEventBus.InstantActionReceived -= NewInstantActionUpdated;
+                RobotEventBus.InstantActionReceived += NewInstantActionUpdated;
+                Resume();
+                break;
+            case OperatingMode.MANUAL:
+                Ps5Controller.Enable();
+                StateManager.Fire(RobotEventType.EnterManual);
+                break;
+            case OperatingMode.SERVICE:
+                Ps5Controller.Disable();
+                StateManager.Fire(RobotEventType.EnterService);
+                break;
+            default:
+                break;
+        }
+    }
+
+    private void OnStop(StopStateType state)
+    {
+        lock (_stateTransitionLock)
+        {
+            if (state != StopStateType.None)
+            {
+                _rfHandleHasPriority = false; // Safety overrides RF Handle
+                if (!StateManager.IsInState(RobotStateType.Stop))
+                {
+                    Pause();
+                    StateManager.Fire(RobotEventType.EnterStop);
+                }
+            }
+            else
+            {
+                // No physical Start button: leave Stop as soon as PLC reports all safety inputs clear.
+                TryReleaseStopAfterSafetyClear();
+            }
+        }
+    }
+
+    /// <summary>
+    /// Exit Stop when EMC/bumper are released. Previously required a Start button; this robot has none.
+    /// </summary>
+    private void TryReleaseStopAfterSafetyClear()
+    {
+        if (!StateManager.IsInState(RobotStateType.Stop))
+            return;
+        if (PlcController.Emergency || PlcController.Bumper)
+            return;
+
+        Logger.Info("Robot Controller: Safety cleared; releasing Stop (auto, no Start button)");
+        StateManager.Fire(RobotEventType.ReleaseStop);
+    }
+
+    private void OnButtonPressed(PeripheralButton button)
+    {
+        lock (_stateTransitionLock)
+        {
+            if (button == PeripheralButton.Reset)
+            {
+                // M815 Reset: clear robot fault + reset fault động cơ
+                ApplyResetFromPlc();
+            }
+        }
+    }
+
+    private void OnNewFatalError()
+    {
+        lock (_stateTransitionLock)
+        {
+            if (!StateManager.IsInState(RobotStateType.Fault))
+            {
+                _rfHandleHasPriority = false; // Fault overrides RF Handle
+                Pause();
+                StateManager.Fire(RobotEventType.EnterFault);
+            }
+        }
+    }
+
+    private void OnRfModeChanged(RFMode rfMode)
+    {
+        lock (_stateTransitionLock)
+        {
+            // Ignore RF mode changes while in Stop or Fault — safety overrides everything
+            // RF mode will be re-evaluated when returning to Standby via WatchThread
+            if (StateManager.IsInState(RobotStateType.Stop) || StateManager.IsInState(RobotStateType.Fault))
+            {
+                Logger.Info($"RF mode change to {rfMode} ignored - robot in {StateManager.CurrentState}");
+                return;
+            }
+
+            switch (rfMode)
+            {
+                case RFMode.Maintenance:
+                    // RF Handle requests Service mode
+                    _rfHandleHasPriority = true;
+                    if (StateManager.IsInState(RobotStateType.Auto))
+                    {
+                        Pause();
+                        RobotEventBus.OrderMessageReceived -= NewOrderUpdated;
+                        // Keep InstantActionReceived subscription - instant actions must work in Service mode
+                    }
+                    StateManager.Fire(RobotEventType.EnterService);
+                    break;
+
+                case RFMode.Override:
+                    // RF Handle requests Remote Override
+                    _rfHandleHasPriority = true;
+                    if (StateManager.IsInState(RobotStateType.Auto))
+                    {
+                        Pause();
+                        RobotEventBus.OrderMessageReceived -= NewOrderUpdated;
+                        // Keep InstantActionReceived subscription - instant actions must work in Override mode
+                    }
+                    StateManager.Fire(RobotEventType.RemoteOverride);
+                    break;
+
+                case RFMode.Default:
+                case RFMode.None:
+                    // RF Handle released control or disconnected - return to PLC-determined mode
+                    _rfHandleHasPriority = false;
+                    PlcController.SetRFEStop(false);
+                    if (StateManager.IsInState(RobotStateType.Service) || StateManager.IsInState(RobotStateType.Remote_Override))
+                    {
+                        SwitchModeChanged(PlcController.PeripheralMode);
+                    }
+                    break;
+            }
+        }
+    }
+
+    private void WatchThreadCallback()
+    {
+        lock (_stateTransitionLock)
+        {
+            // 1. Fatal error detection
+            if (ErrorManager.HasFatalError && !StateManager.IsInState(RobotStateType.Fault))
+            {
+                Logger.Warning("Robot Controller: Fatal error detected, transitioning to Fault state");
+                Pause();
+                StateManager.Fire(RobotEventType.EnterFault);
+                return;
+            }
+
+            // 2. In Stop: release automatically when PLC shows safety clear (backup if OnStop edge was missed)
+            if (StateManager.IsInState(RobotStateType.Stop))
+            {
+                TryReleaseStopAfterSafetyClear();
+                return;
+            }
+
+            // 2b. Fault auto-recovery
+            if (StateManager.IsInState(RobotStateType.Fault))
+            {
+                if (PlcController.IsReady && !PlcController.IsDisconected)
+                    ErrorManager.DeleteErrorId(2003);
+
+                if (!ErrorManager.HasFatalError)
+                {
+                    Logger.Info("Robot Controller: Fatal errors resolved, auto-recovering from Fault");
+                    StateManager.Fire(RobotEventType.ExitFault);
+                }
+                return;
+            }
+
+            var plcMode = PlcController.PeripheralMode;
+
+            // 3. If in Standby, trigger mode switch (e.g., after ReleaseStop or initialization)
+            if (StateManager.CurrentState == RobotStateType.Standby)
+            {
+                // Check if RF Handle has an active mode that should take priority
+                // (RF mode preserved on PLC during Stop/Fault, re-evaluated here after release)
+                var rfMode = PlcController.CurrentRFMode;
+                if (rfMode == RFMode.Maintenance)
+                {
+                    Logger.Info("Robot Controller: Standby → RF Handle Maintenance detected, entering Service");
+                    _rfHandleHasPriority = true;
+                    StateManager.Fire(RobotEventType.EnterService);
+                }
+                else if (rfMode == RFMode.Override)
+                {
+                    Logger.Info("Robot Controller: Standby → RF Handle Override detected, entering Remote_Override");
+                    _rfHandleHasPriority = true;
+                    StateManager.Fire(RobotEventType.RemoteOverride);
+                }
+                else
+                {
+                    SwitchModeChanged(plcMode);
+                }
+                return;
+            }
+
+            // 4. PLC mode mismatch check — ONLY when RF Handle does NOT have priority
+            if (!_rfHandleHasPriority)
+            {
+                var currentModeMatch = plcMode switch
+                {
+                    OperatingMode.AUTOMATIC => StateManager.IsInState(RobotStateType.Auto),
+                    OperatingMode.MANUAL => StateManager.IsInState(RobotStateType.Manual),
+                    OperatingMode.SERVICE => StateManager.IsInState(RobotStateType.Service),
+                    _ => true
+                };
+
+                if (!currentModeMatch)
+                {
+                    Logger.Warning($"Robot Controller: PLC mode mismatch. PLC: {plcMode}, State: {StateManager.CurrentState}");
+                    SwitchModeChanged(plcMode);
+                }
+            }
+
+            // 5. Backup stop detection (ALWAYS runs, even when RF Handle has priority)
+            bool hasSafetyStop = PlcController.Emergency || PlcController.Bumper;
+
+            if (hasSafetyStop && !StateManager.IsInState(RobotStateType.Stop))
+            {
+                Logger.Warning("Robot Controller: Safety stop detected from PLC properties");
+                _rfHandleHasPriority = false; // Safety overrides RF Handle
+                Pause();
+                StateManager.Fire(RobotEventType.EnterStop);
+            }
+
+            // 6. Check has load
+            if (LiftModule.IsReady && LiftModule.Position == LiftPosition.Top) PlcController.SetHasLoad(true);
+            else PlcController.SetHasLoad(false);
+
+            // 7. Check Pin: set M929 when battery percentage is below configured threshold.
+            if(Battery != null
+               && Battery.CurrentBatteryState.HasValue
+               && !double.IsNaN(Battery.CurrentBatteryState.Value.Percentage)
+               && Battery.CurrentBatteryState.Value.Percentage < _batteryLowThresholdPercent)
+            {
+                PlcController.SetBatteryLow(true);
+            }
+            else PlcController.SetBatteryLow(false);
+
+            // 8. Check PLC connection
+            if (PlcController.IsDisconected)
+            {
+                ErrorManager.AddError(RobotErrors.Error2003());
+            }
+            else
+            {
+                ErrorManager.DeleteErrorId(2003);
+            }
+        }
+    }
+
+    private double ResolveBatteryLowThresholdPercent()
+    {
+        const double defaultThreshold = 20.0;
+        try
+        {
+            if (Battery is not DeviceBase batteryDevice)
+            {
+                return defaultThreshold;
+            }
+
+            var devicesSection = Configuration.GetSection("Devices");
+            foreach (var section in devicesSection.GetChildren())
+            {
+                var deviceId = section.GetValue<string>("DeviceId");
+                if (!string.Equals(deviceId, batteryDevice.DeviceId, StringComparison.OrdinalIgnoreCase))
+                {
+                    continue;
+                }
+
+                var threshold = section.GetSection("Connection").GetValue<double?>("LowBatteryThresholdPercent");
+                if (!threshold.HasValue)
+                {
+                    return defaultThreshold;
+                }
+
+                var clamped = Math.Clamp(threshold.Value, 0.0, 100.0);
+                if (Math.Abs(clamped - threshold.Value) > double.Epsilon)
+                {
+                    Logger.Warning($"Battery low threshold {threshold.Value} out of range [0..100], clamped to {clamped}");
+                }
+                Logger.Info($"Battery low threshold loaded from config: {clamped}%");
+                return clamped;
+            }
+
+            return defaultThreshold;
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"Failed to resolve battery low threshold from config, fallback {defaultThreshold}%: {ex.Message}");
+            return defaultThreshold;
+        }
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/RobotErrors.cs

@@ -0,0 +1,136 @@
+using RobotNet.VDA5050.State;
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Interfaces;
+
+namespace RobotNet10.RobotApp.Services.Robot;
+
+public class RobotError : Error
+{
+    public int Id { get; set; }
+}
+
+public class RobotErrors() : IError
+{
+    public Error[] ErrorsState { get { lock (Errors) { return [.. Errors]; } } }
+    public bool HasFatalError { get { lock (Errors) { return Errors.Any(e => e.ErrorLevel == ErrorLevel.FATAL); } } }
+    public event System.Action? OnNewFatalError;
+
+    private readonly List<RobotError> Errors = [];
+
+    public void AddError(RobotError error, TimeSpan? clearAfter = null)
+    {
+        bool isFatal = false;
+        lock (Errors)
+        {
+            if (Errors.Any(e => e.Id == error.Id)) return;
+            Errors.Add(error);
+            isFatal = error.ErrorLevel == ErrorLevel.FATAL;
+        }
+        if (isFatal) OnNewFatalError?.Invoke();
+        if (clearAfter is not null && clearAfter.HasValue)
+        {
+            if (clearAfter.Value < TimeSpan.Zero) throw new ArgumentOutOfRangeException(nameof(clearAfter), "TimeSpan cannot be negative.");
+            _ = Task.Run(async () =>
+            {
+                await Task.Delay(clearAfter.Value);
+                lock (Errors)
+                {
+                    Errors.RemoveAll(e => e.Id == error.Id);
+                }
+            });
+        }
+    }
+
+    public void DeleteErrorType(string errorType)
+    {
+        lock (Errors)
+        {
+            Errors.RemoveAll(e => e.ErrorType == errorType);
+        }
+    }
+
+    public void DeleteErrorId(int id)
+    {
+        lock (Errors)
+        {
+            Errors.RemoveAll(e => e.Id == id);
+        }
+    }
+
+    public void ClearAllErrors()
+    {
+        lock (Errors)
+        {
+            Errors.Clear();
+        }
+    }
+
+    public void ClearFatalErrors()
+    {
+        lock (Errors) { Errors.RemoveAll(e => e.ErrorLevel == ErrorLevel.FATAL); }
+    }
+
+    private static RobotError CreateError(int id, ErrorType type, string hint, ErrorLevel level, string description)
+    {
+        return new RobotError()
+        {
+            Id = id,
+            ErrorType = type.ToString(),
+            ErrorLevel = level,
+            ErrorDescription = description,
+            ErrorHint = hint,
+            ErrorReferences = []
+        };
+    }
+
+    public static RobotError Error1001(string oldOrderId, string newOrderId)
+        => CreateError(1001, ErrorType.ORDER_ERROR, "Vui lòng kiểm tra lại OrderId", ErrorLevel.WARNING, $"Có order đang được thực hiện. OrderId: {oldOrderId}, OrderId mới: {newOrderId}");
+    public static RobotError Error1002(int nodesLength)
+        => CreateError(1002, ErrorType.ORDER_ERROR, "Vui lòng kiểm tra lại kích thước Nodes", ErrorLevel.WARNING, $"Order Nodes không hợp lệ. Kích thước: {nodesLength}");
+    public static RobotError Error1003(int oldOrderUpdateId, int newOrderUpdateId)
+        => CreateError(1003, ErrorType.ORDER_UPDATE_ERROR, "Vui lòng kiểm tra lại OrderUpdateId", ErrorLevel.WARNING, $"OrderUpdateId {newOrderUpdateId} nhận được nhỏ hơn OrderUpdateId hiện tại là {oldOrderUpdateId}");
+    public static RobotError Error1004(int nodesLength, int edgesLength)
+        => CreateError(1004, ErrorType.ORDER_ERROR, "Vui lòng kiểm tra lại kích thước giữa Nodes và Edges", 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 RobotError Error1005()
+        => CreateError(1005, ErrorType.ORDER_ERROR, "Vui lòng kiểm tra lại OrderId", ErrorLevel.WARNING, $"Không có order đang được thực hiện.");
+    public static RobotError Error1006(string rootState)
+    => CreateError(1006, ErrorType.INITIALIZE_ORDER, "Vui lòng chờ robot sẵn sàng", ErrorLevel.WARNING, $"Robot chưa sẵn sàng để nhận Order. Trạng thái hiện tại {rootState}");
+    public static RobotError Error1007()
+        => CreateError(1007, ErrorType.VALIDATION_ERROR, "Vui lòng chờ hoàn thành các action hiện tại.", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi có action đang thực hiện.");
+    public static RobotError Error1008(string edgeId, string nodeId)
+        => CreateError(1008, ErrorType.ORDER_ERROR, "Vui lòng kiểm tra lại order", ErrorLevel.WARNING, $"Order có  edge {edgeId} tồn tại startNode {nodeId} không nằm trong danh sách nodes");
+    public static RobotError Error1009(string edgeId, string nodeId)
+        => CreateError(1009, ErrorType.ORDER_ERROR, "Vui lòng kiểm tra lại order", ErrorLevel.WARNING, $"Order có  edge {edgeId} tồn tại endNode {nodeId} không nằm trong danh sách nodes");
+    public static RobotError Error1010(string lastNodeId, string newStartNodeId)
+        => CreateError(1010, ErrorType.ORDER_ERROR, "Vui lòng kiểm tra lại order", ErrorLevel.WARNING, $"Order mới nhận được không phải là nối tiếp của order khi lastNodeId: {lastNodeId} mà node đầu tiên của order mới là: {newStartNodeId}");
+    public static RobotError Error1011(int lastNodeSequenceId, int newStartNodeSequenceId)
+        => CreateError(1011, ErrorType.ORDER_ERROR, "Vui lòng kiểm tra lại order", ErrorLevel.WARNING, $"Order mới nhận được không phải là nối tiếp của order khi LastNodeSequenceId: {lastNodeSequenceId} mà node đầu tiên của order mới có sequence: {newStartNodeSequenceId}");
+    public static RobotError Error1012(string nodeId, int sequenceId, int correctIndex)
+        => CreateError(1012, ErrorType.ORDER_ERROR, "Vui lòng kiểm tra lại order node sequence", ErrorLevel.WARNING, $"Order Nodes không đúng thứ tự. NodeId: {nodeId}, SequenceId: {sequenceId}, Vị trí đúng: {correctIndex}");
+    public static RobotError Error1013(string edgeId, int sequenceId, int correctIndex)
+       => CreateError(1013, ErrorType.ORDER_ERROR, "Vui lòng kiểm tra lại order edge sequence", ErrorLevel.WARNING, $"Order Edges không đúng thứ tự. EdgeId: {edgeId}, SequenceId: {sequenceId}, Vị trí đúng: {correctIndex}");
+    public static RobotError Error1014()
+       => CreateError(1014, ErrorType.ORDER_ERROR, "", ErrorLevel.WARNING, "Order kết thúc không thành công do module Navigation có lỗi xảy ra");
+    public static RobotError Error1015(string nodeId)
+       => CreateError(1015, ErrorType.ORDER_ERROR, "Vui lòng kiểm tra lại order", ErrorLevel.WARNING, $"Order node {nodeId} yêu cầu phải có NodePosition");
+    public static RobotError Error1016(string nodeId, double distance, double allowedDeviation)
+       => CreateError(1016, ErrorType.ORDER_ERROR, "Robot quá xa node bắt đầu", ErrorLevel.WARNING, $"Robot cách node bắt đầu {nodeId} quá xa. Khoảng cách: {distance:F2}m, cho phép: {allowedDeviation:F2}m");
+    public static RobotError Error1017(string nodeId, double distance, double allowedDeviation)
+       => CreateError(1017, ErrorType.ORDER_ERROR, "Robot đã ở node đích", ErrorLevel.WARNING, $"Robot đã ở tại hoặc quá gần node đích {nodeId}. Khoảng cách: {distance:F2}m, tối thiểu: {allowedDeviation:F2}m");
+    public static RobotError Error1018(string edgeId)
+       => CreateError(1018, ErrorType.ORDER_ERROR, "Vui lòng kiểm tra lại trajectory", ErrorLevel.WARNING, $"Edge {edgeId} có trajectory không hợp lệ: knotVector size phải bằng controlPoints + degree + 1");
+    public static RobotError Error1019(string edgeId)
+       => CreateError(1019, ErrorType.ORDER_ERROR, "Vui lòng kiểm tra lại trajectory", ErrorLevel.WARNING, $"Edge {edgeId} có trajectory không hợp lệ: knotVector phải là dãy tăng dần từ 0 đến 1");
+    public static RobotError Error1020(string edgeId)
+       => CreateError(1020, ErrorType.ORDER_ERROR, "Vui lòng kiểm tra lại trajectory", ErrorLevel.WARNING, $"Edge {edgeId} có trajectory không hợp lệ: cần ít nhất 2 controlPoints (điểm bắt đầu và kết thúc)");
+
+    public static RobotError Error2001()
+        => CreateError(2001, ErrorType.PERIPHERAL_ERROR, "", ErrorLevel.FATAL, "Có lỗi xảy ra trong quá trình đọc tín hiệu từ hệ thống ngoại vi(PLC)");
+    public static RobotError Error2002()
+        => CreateError(2002, ErrorType.PERIPHERAL_ERROR, "", ErrorLevel.FATAL, "Có lỗi xảy ra trong quá trình gửi tín hiệu tới hệ thống ngoại vi(PLC)");
+    public static RobotError Error2003()
+        => CreateError(2003, ErrorType.PERIPHERAL_ERROR, "", ErrorLevel.FATAL, "Mất kết nối với hệ thống ngoại vi(PLC)");
+
+    public static RobotError Error3001()
+        => CreateError(3001, ErrorType.LOCALIZATION_ERROR, "", ErrorLevel.WARNING, "Trạng thái định vị chưa sẵn sàng");
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/RobotExtensions.cs

@@ -0,0 +1,85 @@
+using RobotNet10.FleetManager.Events;
+using RobotNet10.RobotApp.Events;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Services.ConfigManager;
+using RobotNet10.RobotApp.Services.Navigation;
+using RobotNet10.RobotApp.Services.Navigation.CSharp;
+using RobotNet10.RobotApp.Services.Robot.Actions;
+using RobotNet10.RobotApp.Services.Robot.Connection;
+using RobotNet10.RobotApp.Services.Robot.Modules;
+using RobotNet10.RobotApp.Services.State;
+using System.Diagnostics.CodeAnalysis;
+
+namespace RobotNet10.RobotApp.Services.Robot;
+
+public static class RobotExtensions
+{
+    public static IServiceCollection AddRobot(this IServiceCollection services)
+    {
+        services.AddSingleton<RobotStateMachine>();
+        services.AddSingleton<RobotStateMachineExecute>();
+        services.AddSingleton<RobotVisualization>();
+
+        services.AddInterfaceServiceSingleton<IRobotConfiguration, RobotConfiguration>();
+        services.AddInterfaceServiceSingleton<IConnectionConfig, ConnectionConfig>();
+        services.AddInterfaceServiceSingleton<INavigationConfig, ConfigManager.NavigationConfig>();
+        services.AddInterfaceServiceSingleton<IRobotConnectionsService, RobotConnectionsService>();
+        services.AddInterfaceServiceSingleton<IRobotEventBus, RobotEventBus>();
+        services.AddInterfaceServiceSingleton<IError, RobotErrors>();
+        services.AddInterfaceServiceSingleton<IInfomation, RobotInfomations>();
+        services.AddInterfaceServiceSingleton<INavigation, RobotNavigation>();
+        services.AddInterfaceServiceSingleton<IOrder, RobotOrderController>();
+        services.AddInterfaceServiceSingleton<ILoad, RobotLoads>();
+        services.AddInterfaceServiceSingleton<ILocalization, RobotLocalization>();
+        services.AddInterfaceServiceSingleton<IState, RobotStates>();
+        services.AddInterfaceServiceSingleton<IPlcController, RobotPlcController>();
+
+        services.AddInterfaceServiceSingleton<IVelocityController, VelocityController>();
+        services.AddInterfaceServiceSingleton<IFactsheet, RobotFactsheet>();
+
+        services.AddHostedInterfaceServiceSingleton<IAction, RobotActionController>();
+        services.AddHostedInterfaceServiceSingleton<IRobotActionProvider, RobotActionProvider>();
+
+        services.AddHostedInterfaceServiceSingleton<IRobotController, RobotController>();
+        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 AddInterfacesServiceSingleton<TService1, TService2, [DynamicallyAccessedMembers(DynamicallyAccessedMemberTypes.PublicConstructors)] TImplementation>(this IServiceCollection services) where TService1 : class where TService2 : class where TImplementation : class, TService1, TService2
+    {
+        services.AddSingleton<TImplementation>();
+        services.AddSingleton<TService1>(sp => sp.GetRequiredService<TImplementation>());
+        services.AddSingleton<TService2>(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;
+    }
+
+    public static IServiceCollection AddHostedInterfaceServiceSingleton<TService1, TService2, [DynamicallyAccessedMembers(DynamicallyAccessedMemberTypes.PublicConstructors)] THostedService>(this IServiceCollection services) where TService1 : class where TService2 : class where THostedService : class, IHostedService, TService1, TService2
+    {
+        services.AddSingleton<THostedService>();
+        services.AddSingleton<TService1>(sp => sp.GetRequiredService<THostedService>());
+        services.AddSingleton<TService2>(sp => sp.GetRequiredService<THostedService>());
+        services.AddHostedService(sp => sp.GetRequiredService<THostedService>());
+        return services;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/RobotFactsheet.cs

@@ -0,0 +1,34 @@
+using RobotNet.VDA5050;
+using RobotNet.VDA5050.Factsheet;
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Services.ConfigManager;
+using RobotNet10.RobotApp.Services.Robot.Connection;
+
+namespace RobotNet10.RobotApp.Services.Robot;
+
+public class RobotFactsheet(IConnectionConfig ConnectionConfig,
+                            IRobotConnectionsService RobotConnection,
+                            Logger<RobotFactsheet> Logger) : IFactsheet
+{
+    public async Task PubFactsheet()
+    {
+        try
+        {
+            if (!RobotConnection.IsConnected) return;
+
+            var vdaConfig = ConnectionConfig.GetVDA5050Config();
+            FactSheetMsg factSheet = new()
+            {
+                SerialNumber = vdaConfig.SerialNumber,
+                Manufacturer = vdaConfig.Manufacturer,
+                Version = vdaConfig.Version,
+            };
+            await RobotConnection.PublishFactsheetAsync(factSheet);
+        }
+        catch (Exception ex)
+        {
+            Logger.Error($"Error publishing factsheet: {ex.Message}");
+        }
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/RobotInfomations.cs

@@ -0,0 +1,34 @@
+using RobotNet.VDA5050.State;
+using RobotNet10.RobotApp.Interfaces;
+
+namespace RobotNet10.RobotApp.Services.Robot;
+
+public class RobotInfomations() : IInfomation
+{
+    public Information[] InformationState => [.. Infors];
+    private readonly List<Information> Infors = [];
+    public void AddInfo(Information infor)
+    {
+        if (Infors.Any(e => e.InfoType == infor.InfoType)) return;
+        lock (Infors)
+        {
+            Infors.Add(infor);
+        }
+    }
+
+    public void DeleteInfoType(string infoType)
+    {
+        lock (Infors)
+        {
+            Infors.RemoveAll(e => e.InfoType == infoType);
+        }
+    }
+
+    public void ClearAllInfos()
+    {
+        lock (Infors)
+        {
+            Infors.Clear();
+        }
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/RobotLoads.cs

@@ -0,0 +1,42 @@
+using RobotNet.VDA5050.State;
+using RobotNet10.RobotApp.Interfaces;
+
+namespace RobotNet10.RobotApp.Services.Robot;
+
+public class RobotLoads() : ILoad
+{
+    public Load[] Load { get; private set; } = [];
+
+    private static Load GetLoad()
+    {
+        return new()
+        {
+            LoadId = Guid.NewGuid().ToString(),
+            LoadDimensions = new RobotNet.VDA5050.Factsheet.LoadDimensions
+            {
+                Length = 0.5,
+                Width = 0.5,
+                Height = 0.5
+            },
+            LoadPosition = "on_top",
+            LoadType = "box",
+            BoundingBoxReference = new RobotNet.VDA5050.Factsheet.BoundingBoxReference
+            {
+                X = 0,
+                Y = 0,
+                Z = 0,
+            },
+            Weight = 999
+        };
+    }
+
+    public void AddLoad(Load load)
+    {
+        Load = [.. Load, GetLoad()];
+    }
+
+    public void ClearLoad()
+    {
+        Load = [];
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/RobotOrderController.cs

@@ -0,0 +1,969 @@
+using RobotNet.VDA5050.Order;
+using RobotNet.VDA5050.State;
+using RobotNet.VDA5050.Type;
+using RobotNet10.Common;
+using RobotNet10.RobotApp.Client.Pages;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Services.ConfigManager;
+using RobotNet10.RobotApp.Services.Exceptions;
+using RobotNet10.RobotApp.Services.State;
+using System.Collections.Concurrent;
+using System.Data;
+using Action = RobotNet.VDA5050.InstantAction.Action;
+
+namespace RobotNet10.RobotApp.Services.Robot;
+
+public class RobotOrderController(INavigation NavigationManager,
+                                  ILocalization Localization,
+                                  IAction ActionManager,
+                                  IError ErrorManager,
+                                  IPlcController SafetyManager,
+                                  RobotStateMachine StateManager,
+                                  INavigationConfig NavigationConfig,
+                                  ILogger<RobotOrderController> Logger) : IOrder
+{
+    public string OrderId { get; private set; } = string.Empty;
+    public int OrderUpdateId { get; private set; }
+    public NodeState[] NodeStates { get; private set; } = [];
+    public EdgeState[] EdgeStates { get; private set; } = [];
+    public string LastNodeId => LastNode is null ? "" : LastNode.NodeId;
+    public int LastNodeSequenceId => LastNode is null ? 0 : LastNode.SequenceId;
+    public bool NewBaseRequest { get; private set; }
+    public double DistanceSinceLastNode { get; private set; }
+    public bool IsPaused { get; private set; } = false;
+
+    private const int CycleHandlerMilliseconds = 100;
+    private WatchThread<RobotOrderController>? OrderTimer;
+
+    private readonly Dictionary<string, Action[]> OrderActions = []; // Node actions keyed by NodeId
+    private readonly ConcurrentQueue<Action> ActionWaitingRunning = [];
+
+    private OrderMsg? NewOrder;
+    private OrderMsg? _currentActiveOrder;
+    private Node[] Nodes = [];
+    private Edge[] Edges = [];
+    private Node? CurrentBaseNode;
+    private Node? LastNode;
+    private Edge? CurrentEdge; // Track current edge for EDGE action lifecycle
+    private readonly ConcurrentBag<string> RunningEdgeActionIds = []; // Track running EDGE action IDs (thread-safe)
+
+    private readonly Lock LockObject = new();
+
+    private bool IsCancelOrder = false;
+    private bool IsCancelSentToNavigation = false;
+    private bool IsActionRunning = false;
+    private bool IsWaitingPaused = false;
+    private bool IsNavigationFinished = false;
+    private bool HasNewOrder = false;
+    private Action? ActionHard = null;
+    private NavigationState NavState = NavigationState.None;
+
+    private double SafetySpeed = 0.0;
+    private double EdgeSpeed = 0.0;
+    private double CurrentSpeed = 0.0;
+    private Navigation.NavigationConfig? CachedNavConfig = null;
+
+    public void UpdateOrder(OrderMsg order)
+    {
+        bool shouldStart = false;
+        lock (LockObject)
+        {
+            NewOrder = order;
+            if (OrderTimer is null)
+            {
+                shouldStart = true;
+            }
+        }
+        if (shouldStart) HandleOrderStart();
+    }
+
+    public void StopOrder()
+    {
+        if (NodeStates.Length > 0 || OrderTimer is not null)
+        {
+            IsCancelOrder = true;
+            IsCancelSentToNavigation = false;
+        }
+    }
+
+    public void PauseOrder()
+    {
+        IsPaused = true;
+        NavigationManager.Pause();
+        ActionManager.PauseActions();
+    }
+
+    public void ResumeOrder()
+    {
+        IsPaused = false;
+        NavigationManager.Resume();
+        ActionManager.ResumeActions();
+    }
+
+    private void HandleOrderStart()
+    {
+        // Console.WriteLine("HandleOrderStart called");
+        OrderTimer = new(CycleHandlerMilliseconds, OrderHandler, Logger);
+        OrderTimer.Start();
+    }
+
+    private void HandleOrderStop()
+    {
+        OrderTimer?.Dispose();
+        OrderTimer = null;
+        OrderActions.Clear();
+        ActionWaitingRunning.Clear();
+        ActionManager.StopOrderAction(); // Stop all running order actions
+        // Reset state flags
+        IsCancelOrder = false;
+        IsCancelSentToNavigation = false;
+        IsNavigationFinished = false;
+        IsActionRunning = false;
+        IsWaitingPaused = false;
+        IsPaused = false;
+        ActionHard = null;
+        CurrentBaseNode = null;
+        Nodes = [];
+        Edges = [];
+        _currentActiveOrder = null;
+        // Reset EDGE action tracking
+        CurrentEdge = null;
+        RunningEdgeActionIds.Clear();
+        // Reset speed tracking
+        SafetySpeed = 0.0;
+        EdgeSpeed = 0.0;
+        CurrentSpeed = 0.0;
+        CachedNavConfig = null;
+        UpdateState();
+        SafetyManager.OnSafetySpeedChanged -= OnSafetySpeedChanged;
+        NavigationManager.OnNavigationFinished -= NavigationFinished;
+        StateManager.Fire(RobotEventType.CompleteExecution);
+    }
+
+    /// <summary>
+    /// VDA5050 Compliance: Normalize angle to range [-π, π]
+    /// </summary>
+    private static double NormalizeAngle(double angle)
+    {
+        while (angle > Math.PI) angle -= 2 * Math.PI;
+        while (angle < -Math.PI) angle += 2 * Math.PI;
+        return angle;
+    }
+
+    private Node? GetCurrentNode()
+    {
+        Node? inNode = null;
+        double minDistance = double.MaxValue;
+        foreach (var node in Nodes)
+        {
+            var distance = Localization.DistanceTo(node.NodePosition?.X ?? 0, node.NodePosition?.Y ?? 0);
+            var nodeMin = node.NodePosition?.AllowedDeviationXY == 0.0 ? 0.5 : node.NodePosition?.AllowedDeviationXY ?? 0.3;
+
+            bool positionMatch = distance <= nodeMin;
+            bool orientationMatch = true;
+
+            // VDA5050 Compliance: Check theta if specified
+            if (node.NodePosition?.Theta is not null)
+            {
+                var currentTheta = Localization.Theta;
+                var targetTheta = node.NodePosition.Theta.Value;
+                var allowedThetaDev = node.NodePosition.AllowedDeviationTheta ?? Math.PI; // Default: any orientation
+
+                var thetaDiff = Math.Abs(NormalizeAngle(currentTheta - targetTheta));
+                orientationMatch = thetaDiff <= allowedThetaDev;
+            }
+
+            if (positionMatch && orientationMatch)
+            {
+                // Exclude last node - it's handled separately in HandleOrder() after navigation completes (lines 392-399)
+                // This ensures intermediate node actions are processed during navigation
+                if (distance < minDistance && node.NodeId != Nodes[^1].NodeId)
+                {
+                    minDistance = distance;
+                    inNode = node;
+                }
+            }
+        }
+        return inNode;
+    }
+
+    private void NavigationFinished(NavigationState state)
+    {
+        NavState = state;
+        IsNavigationFinished = true;
+    }
+
+    /// <summary>
+    /// Xử lý sự kiện thay đổi safety speed từ PLC
+    /// </summary>
+    private void OnSafetySpeedChanged(SafetySpeed safetySpeed)
+    {
+        if (TryGetSafetySpeedFromPlcSignal(safetySpeed, out double safeSpeed))
+        {
+            if (safetySpeed == Interfaces.SafetySpeed.Very_Fast)
+            {
+                SafetySpeed = 0.0; // 0 = không giới hạn từ safety; edge/config quyết định trong UpdateNavigationSpeed
+                UpdateNavigationSpeed();
+                Logger.LogInformation("SafetySpeed released: robot speed now follows edge/config limit");
+            }
+            else
+            {
+                SafetySpeed = safeSpeed;
+                UpdateNavigationSpeed();
+            }
+        }
+        else
+        {
+            Logger.LogWarning("Cannot map PLC SafetySpeed {SafetySpeed} to navigation speed value", safetySpeed);
+        }
+    }
+
+    private bool TryGetSafetySpeedFromPlcSignal(SafetySpeed safetySpeed, out double speed)
+    {
+        // PLC IO tốc độ: 825/2873, 826/2874, 827/2875, 828/2876.
+        // Ưu tiên map cứng theo yêu cầu vận hành để không phụ thuộc config.
+        switch (safetySpeed)
+        {
+            case Interfaces.SafetySpeed.Very_Slow:
+                speed = 1.0;
+                return true;
+            case Interfaces.SafetySpeed.Slow:
+                speed = 0.3;
+                return true;
+            case Interfaces.SafetySpeed.Normal:
+                speed = 0.6;
+                return true;
+            case Interfaces.SafetySpeed.Medium:
+                speed = 0.9;
+                return true;
+            case Interfaces.SafetySpeed.Very_Fast:
+                speed = 1.5; 
+                return true;
+        }
+
+        // Các mức còn lại vẫn theo cấu hình hiện tại.
+        CachedNavConfig ??= NavigationConfig.GetNavigationConfig();
+        if (CachedNavConfig.SafetySpeedMap.TryGetValue(safetySpeed, out double configSpeed))
+        {
+            speed = configSpeed;
+            return true;
+        }
+
+        speed = 0.0;
+        return false;
+    }
+
+    /// <summary>
+    /// VDA5050 Compliance: Tính toán và áp dụng tốc độ giới hạn cho navigation
+    /// Kết hợp 3 nguồn tốc độ: Edge, Safety, Config Max
+    /// Sử dụng giá trị MIN để đảm bảo an toàn
+    /// </summary>
+    private void UpdateNavigationSpeed()
+    {
+        if (Nodes.Length <= 0 || IsNavigationFinished)
+            return;
+
+        CachedNavConfig ??= NavigationConfig.GetNavigationConfig();
+        double maxConfigSpeed = CachedNavConfig.MaxLinearVelocity;
+        if (maxConfigSpeed <= 0)
+            return;
+
+        // Bắt đầu từ max config; EdgeSpeed/SafetySpeed = 0 nghĩa là không giới hạn từ nguồn đó
+        double targetSpeed = maxConfigSpeed;
+
+        if (EdgeSpeed > 0)
+            targetSpeed = Math.Min(targetSpeed, EdgeSpeed);
+
+        if (SafetySpeed > 0)
+            targetSpeed = Math.Min(targetSpeed, SafetySpeed);
+
+        targetSpeed = Math.Max(targetSpeed, CachedNavConfig.MinLinearVelocity);
+
+        if (Math.Abs(CurrentSpeed - targetSpeed) < 0.01)
+            return;
+
+        CurrentSpeed = targetSpeed;
+        NavigationManager.SetSpeed(CurrentSpeed);
+
+        Logger.LogInformation(
+            "Speed updated: {CurrentSpeed:F2} m/s [Edge: {EdgeSpeed:F2}, Safety: {SafetySpeed:F2}, Max: {MaxSpeed:F2}]",
+            CurrentSpeed,
+            EdgeSpeed > 0 ? EdgeSpeed : maxConfigSpeed,
+            SafetySpeed > 0 ? SafetySpeed : maxConfigSpeed,
+            maxConfigSpeed);
+    }
+
+    private void UpdateState()
+    {
+        NodeStates = [.. Nodes.Select(n => new NodeState
+                    {
+                        NodeId = n.NodeId,
+                        Released = n.Released,
+                        SequenceId = n.SequenceId,
+                        NodeDescription = n.NodeDescription,
+                        NodePosition = n.NodePosition is null ? null : new()
+                        {
+                            X = n.NodePosition.X,
+                            Y = n.NodePosition.Y,
+                            Theta = n.NodePosition.Theta,
+                            MapId = n.NodePosition.MapId
+                        }
+                    })];
+        EdgeStates = [.. Edges.Select(e => new EdgeState
+                    {
+                        EdgeId = e.EdgeId,
+                        Released = e.Released,
+                        EdgeDescription = e.EdgeDescription,
+                        SequenceId = e.SequenceId,
+                        Trajectory = e.Trajectory
+                    })];
+    }
+
+    private async Task ClearOldOrder()
+    {
+        OrderActions.Clear();
+        await ActionManager.ClearActions();
+        IsNavigationFinished = false;
+        IsCancelOrder = false;
+        IsActionRunning = false;
+        IsWaitingPaused = false;
+        ActionHard = null;
+    }
+
+    private void AddAction(Action[] actions, Node node)
+    {
+        foreach (var item in actions)
+        {
+            item.ActionDescription += $".On Node: {(string.IsNullOrEmpty(node.NodeDescription) ? node.NodeId : node.NodeDescription)}";
+        }
+        if (OrderActions.TryGetValue(node.NodeId, out Action[]? oldActions) && oldActions is not null)
+        {
+            OrderActions[node.NodeId] = [.. oldActions, .. actions];
+        }
+        else OrderActions.Add(node.NodeId, actions);
+    }
+
+    private void AddEdgeAction(Action[] actions, Edge edge)
+    {
+        foreach (var item in actions)
+        {
+            item.ActionDescription += $".On Edge: {(string.IsNullOrEmpty(edge.EdgeDescription) ? edge.EdgeId : edge.EdgeDescription)}";
+        }
+        if (OrderActions.TryGetValue(edge.EdgeId, out Action[]? oldActions) && oldActions is not null)
+        {
+            OrderActions[edge.EdgeId] = [.. oldActions, .. actions];
+        }
+        else OrderActions.Add(edge.EdgeId, actions);
+    }
+
+    private void ValidateNodes(Node[] nodes, int currentSequence)
+    {
+        for (int i = 0; i < nodes.Length; i++)
+        {
+            int correctSequence = i * 2 + currentSequence;
+            if (nodes[i].SequenceId != correctSequence) throw new OrderException(RobotErrors.Error1012(nodes[i].NodeId, nodes[i].SequenceId, correctSequence));
+            if (nodes[i].NodePosition is null) throw new OrderException(RobotErrors.Error1015(nodes[i].NodeId));
+            if (i == 0)
+            {
+                if (nodes[i].Released)
+                {
+                    if (nodes[i].Actions != null && nodes[i].Actions.Length > 0) AddAction(nodes[i].Actions, nodes[i]);
+                }
+            }
+        }
+    }
+
+    private static void ValidateTrajectory(Edge edge, Node startNode, Node endNode)
+    {
+        // VDA5050 Compliance: Validate NURBS trajectory structure
+        if (edge.Trajectory is not null)
+        {
+            var traj = edge.Trajectory;
+
+            // Validate controlPoints count (minimum 2: start and end)
+            if (traj.ControlPoints is null || traj.ControlPoints.Length < 2)
+            {
+                throw new OrderException(RobotErrors.Error1020(edge.EdgeId));
+            }
+
+            // Validate knotVector size: must equal controlPoints.Length + degree + 1
+            if (traj.KnotVector is not null)
+            {
+                int expectedSize = traj.ControlPoints.Length + traj.Degree + 1;
+                if (traj.KnotVector.Length != expectedSize)
+                {
+                    throw new OrderException(RobotErrors.Error1018(edge.EdgeId));
+                }
+
+                // Validate knotVector is monotonically increasing from 0 to 1
+                for (int j = 0; j < traj.KnotVector.Length; j++)
+                {
+                    if (traj.KnotVector[j] < 0 || traj.KnotVector[j] > 1)
+                    {
+                        throw new OrderException(RobotErrors.Error1019(edge.EdgeId));
+                    }
+                    if (j > 0 && traj.KnotVector[j] < traj.KnotVector[j - 1])
+                    {
+                        throw new OrderException(RobotErrors.Error1019(edge.EdgeId));
+                    }
+                }
+            }
+        }
+        else
+        {
+            // VDA5050 Compliance: Create valid default linear trajectory
+            edge.Trajectory = new Trajectory()
+            {
+                Degree = 1,
+                ControlPoints =
+                [
+                    new ControlPoint()
+                    {
+                        X = startNode.NodePosition?.X ?? 0,
+                        Y = startNode.NodePosition?.Y ?? 0,
+                        Weight = 1.0
+                    },
+                    new ControlPoint()
+                    {
+                        X = endNode.NodePosition?.X ?? 0,
+                        Y = endNode.NodePosition?.Y ?? 0,
+                        Weight = 1.0
+                    }
+                ],
+                KnotVector = [0, 0, 1, 1]
+            };
+        }
+    }
+
+    private void ValidateEdges(Edge[] edges, Node[] nodes, int currentSequence)
+    {
+        for (int i = 0; i < edges.Length; i++)
+        {
+            var startNode = nodes.FirstOrDefault(n => n.NodeId == edges[i].StartNodeId) ??
+                throw new OrderException(RobotErrors.Error1008(edges[i].EdgeId, edges[i].StartNodeId));
+            var endNode = nodes.FirstOrDefault(n => n.NodeId == edges[i].EndNodeId) ??
+                throw new OrderException(RobotErrors.Error1009(edges[i].EdgeId, edges[i].StartNodeId));
+
+            int correctSequence = i * 2 + 1 + currentSequence;
+            if (edges[i].SequenceId != correctSequence) throw new OrderException(RobotErrors.Error1013(edges[i].EdgeId, edges[i].SequenceId, correctSequence));
+
+            // VDA5050 Compliance: Validate or create proper trajectory
+            ValidateTrajectory(edges[i], startNode, endNode);
+
+            if (edges[i].Released)
+            {
+                if (endNode.Released)
+                {
+                    CurrentBaseNode = endNode;
+                    if (endNode.Actions != null && endNode.Actions.Length > 0) AddAction(endNode.Actions, endNode);
+                    if (edges[i].Actions != null && edges[i].Actions.Length > 0) AddEdgeAction(edges[i].Actions, edges[i]);
+                }
+            }
+        }
+    }
+
+    private async Task HandleNewOrder(OrderMsg order)
+    {
+        if (order.OrderId == OrderId)
+        {
+            if (order.OrderUpdateId < OrderUpdateId) throw new OrderException(RobotErrors.Error1003(OrderUpdateId, order.OrderUpdateId));
+            if (order.OrderUpdateId == OrderUpdateId) return;
+            if (order.Nodes[0].NodeId != LastNodeId)
+            {
+                throw new OrderException(RobotErrors.Error1010(LastNodeId, order.Nodes[0].NodeId));
+            }
+            if (order.Nodes[0].SequenceId != LastNodeSequenceId)
+            {
+                throw new OrderException(RobotErrors.Error1011(LastNodeSequenceId, order.Nodes[0].SequenceId));
+            }
+        }
+
+        // xử lí order mới
+        // Validate Nodes, Edges
+        await ClearOldOrder();
+        ValidateNodes(order.Nodes, order.OrderId == OrderId ? LastNodeSequenceId : 0);
+        ValidateEdges(order.Edges, order.Nodes, order.OrderId == OrderId ? LastNodeSequenceId : 0);
+
+        // Add actions to ActionManager with correct scope
+        if (OrderActions.Count > 0)
+        {
+            foreach (var actions in OrderActions)
+            {
+                ActionManager.AddOrderActions(actions.Value, order.Edges.Any(e => e.EdgeId == actions.Key) ? ActionScope.EDGE : ActionScope.NODE);
+            }
+        }
+
+        if (order.Nodes.Length <= 1 || order.Edges.Length == 0)
+        {
+            if (order.Nodes.Length == 1 && order.Nodes[0].Actions.Length == 0) return;
+            NavigationFinished(NavigationState.Completed);
+        }
+
+        OrderId = order.OrderId;
+        OrderUpdateId = order.OrderUpdateId;
+        Nodes = order.Nodes;
+        Edges = order.Edges;
+        _currentActiveOrder = order;
+        ErrorManager.DeleteErrorType(ErrorType.VALIDATION_ERROR.ToString());
+        ErrorManager.DeleteErrorType(ErrorType.ORDER_ERROR.ToString());
+        ErrorManager.DeleteErrorType(ErrorType.ORDER_UPDATE_ERROR.ToString());
+        UpdateState();
+        HasNewOrder = true;
+    }
+
+    private void ClearLastNode()
+    {
+        if (LastNode is null) return;
+        var currentLastNodeIndex = Array.FindIndex(Nodes, n => n.NodeId == LastNode.NodeId);
+        if (currentLastNodeIndex != -1 && currentLastNodeIndex < Nodes.Length - 1)
+        {
+            Nodes = [.. Nodes.Skip(currentLastNodeIndex + 1)];
+            Edges = [.. Edges.Skip(currentLastNodeIndex + 1)];
+            UpdateState();
+        }
+    }
+
+    private void HandleUpdateOrder(OrderMsg order)
+    {
+        if (order.OrderId != OrderId) throw new OrderException(RobotErrors.Error1001(OrderId, order.OrderId));
+        if (order.OrderUpdateId < OrderUpdateId) throw new OrderException(RobotErrors.Error1003(OrderUpdateId, order.OrderUpdateId));
+        if (order.OrderUpdateId == OrderUpdateId) return;
+
+        if (CurrentBaseNode is not null && order.Nodes[0].NodeId != CurrentBaseNode.NodeId)
+        {
+            throw new OrderException(RobotErrors.Error1010(LastNodeId, order.Nodes[0].NodeId));
+        }
+        if (CurrentBaseNode is not null && order.Nodes[0].SequenceId != CurrentBaseNode.SequenceId)
+        {
+            throw new OrderException(RobotErrors.Error1011(LastNodeSequenceId, order.Nodes[0].SequenceId));
+        }
+
+        IsNavigationFinished = false;
+
+        Node[] baseNodes = CurrentBaseNode is null ? [] : [.. Nodes.TakeWhile(n => n != CurrentBaseNode).Append(CurrentBaseNode)];
+        Edge[] baseEdges = CurrentBaseNode is null ? [] : [.. Edges.ToList().GetRange(0, baseNodes.Length - 1)];
+
+        ValidateNodes(order.Nodes, baseNodes.Length > 0 ? baseNodes[^1].SequenceId : 0);
+        ValidateEdges(order.Edges, order.Nodes, baseNodes.Length > 0 ? baseNodes[^1].SequenceId : 0);
+
+        if (OrderActions.Count > 0)
+        {
+            foreach(var actions in OrderActions)
+            {
+                ActionManager.AddOrderActions(actions.Value, order.Edges.Any(e => e.EdgeId == actions.Key) ? ActionScope.EDGE : ActionScope.NODE);
+            }
+        }
+
+        OrderUpdateId = order.OrderUpdateId;
+        Nodes = [.. baseNodes, .. order.Nodes.Skip(1)];
+        Edges = [.. baseEdges, .. order.Edges];
+        _currentActiveOrder = new OrderMsg
+        {
+            HeaderId = order.HeaderId,
+            Timestamp = order.Timestamp,
+            Version = order.Version,
+            Manufacturer = order.Manufacturer,
+            SerialNumber = order.SerialNumber,
+            OrderId = order.OrderId,
+            OrderUpdateId = order.OrderUpdateId,
+            ZoneSetId = order.ZoneSetId,
+            Nodes = Nodes,
+            Edges = Edges
+        };
+
+        ErrorManager.DeleteErrorType(ErrorType.VALIDATION_ERROR.ToString());
+        ErrorManager.DeleteErrorType(ErrorType.ORDER_ERROR.ToString());
+        ErrorManager.DeleteErrorType(ErrorType.ORDER_UPDATE_ERROR.ToString());
+        UpdateState();
+    }
+
+    private void StartActionTerminal(Node node)
+    {
+        var action = node.Actions[0];
+        var robotAction = ActionManager[action.ActionId];
+        if (robotAction is null)
+        {
+            if (!ActionManager.HasActionWaitting && node.Actions.Length > 0) node.Actions = [.. node.Actions.Skip(1)];
+            return;
+        }
+        if (robotAction.IsCompleted) node.Actions = [.. node.Actions.Skip(1)];
+        if (robotAction.Status == ActionStatus.WAITING) ActionManager.StartOrderAction(action.ActionId);
+    }
+
+    private void HandleOrder()
+    {
+        if (Nodes.Length <= 0)
+        {
+            HandleOrderStop();
+            return;
+        }
+
+        if (HasNewOrder)
+        {
+            if (ActionManager.HasActionWaitting) return;
+            if (Nodes.Length > 1 && Edges.Length >= 0)
+            {
+                if (Nodes[0].Actions.Length > 0)
+                {
+                    // VDA5050: Check if robot is on Node[0] before triggering actions
+                    var startNode = Nodes[0];
+                    var nodeDeviation = startNode.NodePosition?.AllowedDeviationXY ?? 0.5;
+                    if (nodeDeviation == 0.0) nodeDeviation = 0.5;
+                    var distance = Localization.DistanceTo(startNode.NodePosition?.X ?? 0, startNode.NodePosition?.Y ?? 0);
+
+                    if (distance <= nodeDeviation)
+                    {
+                        // VDA5050: Separate NONE from blocking actions on Node[0]
+                        var noneActions = startNode.Actions.Where(a => a.BlockingType == BlockingType.NONE).ToArray();
+                        var blockingActions = startNode.Actions.Where(a => a.BlockingType != BlockingType.NONE).ToArray();
+
+                        // Start NONE actions immediately - they must not delay navigation
+                        foreach (var action in noneActions)
+                        {
+                            ActionManager.StartOrderAction(action.ActionId);
+                        }
+
+                        startNode.Actions = blockingActions;
+
+                        if (blockingActions.Length > 0)
+                        {
+                            // Robot is on Node[0] - trigger blocking actions sequentially
+                            StartActionTerminal(Nodes[0]);
+                            return;
+                        }
+                        // All were NONE → fall through to start navigation
+                    }
+                    // else: Robot not on Node[0] - let navigation start, actions will trigger when node is traversed
+                }
+                else
+                // Start navigation (no blocking actions on Node[0], or Node[0] has no actions)
+                {
+                    IsCancelSentToNavigation = false;
+                    NavigationManager.OnNavigationFinished += NavigationFinished;
+                    SafetyManager.OnSafetySpeedChanged += OnSafetySpeedChanged;
+
+                    // Cache NavigationConfig để tránh load lại mỗi lần speed change
+                    CachedNavConfig = NavigationConfig.GetNavigationConfig();
+
+                    // VDA5050: Đọc initial safety speed khi bắt đầu navigation
+                    var currentSafetySpeed = SafetyManager.SafetySpeed;
+                    if (TryGetSafetySpeedFromPlcSignal(currentSafetySpeed, out double safeSpeed))
+                    {
+                        SafetySpeed = safeSpeed;
+                        Logger.LogInformation("Initial safety speed: {SafetySpeed:F2} m/s (level: {Level})", SafetySpeed, currentSafetySpeed);
+                    }
+
+                    // VDA5050: Set initial edge speed (edge đầu tiên)
+                    if (Edges.Length > 0 && Edges[0].MaxSpeed.HasValue && Edges[0].MaxSpeed is double speed)
+                    {
+                        EdgeSpeed = speed;
+                        Logger.LogInformation("Initial edge speed: {EdgeSpeed:F2} m/s (edge: {EdgeId})", EdgeSpeed, Edges[0].EdgeId);
+                    }
+                    else
+                    {
+                        EdgeSpeed = 0.0; // Không giới hạn
+                    }
+
+                    // VDA5050: Áp dụng tốc độ ban đầu trước khi bắt đầu navigation
+                    UpdateNavigationSpeed();
+
+                    // chỗ này có thể sẽ phải sửa lại theo interface của a Hiệp
+                    NavigationManager.Move(_currentActiveOrder!, SafetyManager.SetHasLoadValue);
+                    if (CurrentBaseNode is not null
+                        && CurrentBaseNode.NodeId != Nodes[0].NodeId
+                        && CurrentBaseNode.NodeId != Nodes[^1].NodeId
+                        && Nodes.Length > 1)
+                    {
+                        NavigationManager.UpdateOrder(CurrentBaseNode.NodeId);
+                    }
+                    if (StateManager.CurrentState != RobotStateType.Executing) StateManager.Fire(RobotEventType.StartExecution);
+                    if(OrderActions.ContainsKey(Nodes[0].NodeId)) OrderActions.Remove(Nodes[0].NodeId);
+                    HasNewOrder = false;
+                }
+            }
+        }
+
+        if (IsCancelOrder && !IsCancelSentToNavigation)
+        {
+            NavigationManager.CancelMovement();
+            IsCancelSentToNavigation = true;
+        }
+
+        if (IsNavigationFinished)
+        {
+            if (IsCancelOrder && !ActionManager.HasActionRunning)
+            {
+                HandleOrderStop();
+                Logger.LogInformation("Order {OrderId} is canceled", OrderId);
+            }
+            else if (NavState == NavigationState.Completed)
+            {
+                if (Nodes.Length > 0 && Nodes[^1].Actions.Length > 0) StartActionTerminal(Nodes[^1]);
+                else if (ActionManager.HasActionRunning) return;
+                else
+                {
+                    LastNode = Nodes[^1];
+                    HandleOrderStop();
+                    Logger.LogInformation("Order {OrderId} is finished", OrderId);
+                }
+            }
+            else
+            {
+                if (NavState == NavigationState.Error) ErrorManager.AddError(RobotErrors.Error1014());
+                HandleOrderStop();
+                Logger.LogInformation("Order {OrderId} is error", OrderId);
+            }
+            return;
+        }
+
+        var currentNode = GetCurrentNode();
+        if (currentNode is not null && currentNode.NodeId != LastNode?.NodeId)
+        {
+            LastNode = currentNode;
+
+            // VDA5050 Section 6.10.2: Finish EDGE actions from previous edge when leaving it
+            if (CurrentEdge is not null && !RunningEdgeActionIds.IsEmpty)
+            {
+                Logger.LogInformation("Finishing {Count} EDGE actions from edge {EdgeId}", RunningEdgeActionIds.Count, CurrentEdge.EdgeId);
+                foreach (var actionId in RunningEdgeActionIds.ToList())
+                {
+                    ActionManager.FinishAction(actionId);
+                }
+                RunningEdgeActionIds.Clear();
+            }
+
+            // VDA5050: Cập nhật edge speed và start EDGE actions khi robot vào edge tiếp theo
+            // Khi đến node i, robot sẽ bắt đầu đi trên edge i (từ node i → node i+1)
+            var currentNodeIndex = Array.FindIndex(Nodes, n => n.NodeId == currentNode.NodeId);
+            if (currentNodeIndex >= 0 && currentNodeIndex < Edges.Length)
+            {
+                var nextEdge = Edges[currentNodeIndex];
+                CurrentEdge = nextEdge;
+
+                // Update edge speed
+                if (nextEdge.MaxSpeed.HasValue && nextEdge.MaxSpeed.Value > 0)
+                {
+                    EdgeSpeed = nextEdge.MaxSpeed.Value;
+                    Logger.LogInformation("Edge speed updated: {EdgeSpeed:F2} m/s (edge: {EdgeId}, node: {NodeId})",
+                        EdgeSpeed, nextEdge.EdgeId, currentNode.NodeId);
+                }
+                else
+                {
+                    EdgeSpeed = 0.0; // Không giới hạn
+                    Logger.LogInformation("Edge speed limit removed (edge: {EdgeId}, node: {NodeId})",
+                        nextEdge.EdgeId, currentNode.NodeId);
+                }
+
+                UpdateNavigationSpeed();
+
+                // VDA5050: Start EDGE actions for this edge
+                // (Actions already added to ActionManager during HandleNewOrder)
+                if (nextEdge.Actions.Length > 0)
+                {
+                    Logger.LogInformation("Starting {Count} EDGE actions for edge {EdgeId}", nextEdge.Actions.Length, nextEdge.EdgeId);
+
+                    // Separate NONE from blocking EDGE actions
+                    var noneActions = nextEdge.Actions.Where(a => a.BlockingType == BlockingType.NONE).ToArray();
+                    var blockingActions = nextEdge.Actions.Where(a => a.BlockingType == BlockingType.SOFT || a.BlockingType == BlockingType.HARD).ToArray();
+
+                    // Start NONE actions immediately
+                    foreach (var action in noneActions)
+                    {
+                        ActionManager.StartOrderAction(action.ActionId);
+                        RunningEdgeActionIds.Add(action.ActionId);
+                    }
+
+                    // Pause navigation and enqueue blocking actions
+                    if (blockingActions.Length > 0)
+                    {
+                        NavigationManager.Pause();
+                        IsWaitingPaused = true;
+
+                        foreach (var action in blockingActions)
+                        {
+                            ActionWaitingRunning.Enqueue(action);
+                            RunningEdgeActionIds.Add(action.ActionId);
+                        }
+                    }
+                }
+            }
+            else
+            {
+                // No next edge - clear current edge
+                CurrentEdge = null;
+            }
+
+            if (OrderActions.TryGetValue(currentNode.NodeId, out Action[]? actions) && actions is not null && actions.Length > 0)
+            {
+                // VDA5050 Compliance: Separate NONE actions from blocking actions
+                var noneActions = actions.Where(a => a.BlockingType == BlockingType.NONE).ToArray();
+                var blockingActions = actions.Where(a => a.BlockingType == BlockingType.SOFT || a.BlockingType == BlockingType.HARD).ToArray();
+
+                // Start NONE actions immediately - they can run during movement
+                foreach (var action in noneActions)
+                {
+                    ActionManager.StartOrderAction(action.ActionId);
+                }
+
+                // Pause navigation only if there are SOFT/HARD actions
+                if (blockingActions.Length > 0)
+                {
+                    NavigationManager.Pause();
+                    IsWaitingPaused = true;
+
+                    // Enqueue blocking actions for sequential execution
+                    foreach (var action in blockingActions)
+                    {
+                        ActionWaitingRunning.Enqueue(action);
+                    }
+                }
+            }
+
+            ClearLastNode();
+        }
+
+        UpdateNavigationSpeed();
+
+        // VDA5050: Improved blocking logic for parallel SOFT actions
+        if (ActionHard is not null)
+        {
+            var robotAction = ActionManager[ActionHard.ActionId];
+            if (robotAction is null) return;
+            if (robotAction is not null && robotAction.IsCompleted) ActionHard = null;
+        }
+        else
+        {
+            if (!ActionWaitingRunning.IsEmpty)
+            {
+                IsActionRunning = !IsWaitingPaused || (IsWaitingPaused && NavigationManager.State == NavigationState.Paused);
+                if (IsActionRunning)
+                {
+                    // VDA5050: Check if there are running SOFT actions (both NODE and EDGE)
+                    var runningSoftActions = ActionManager.GetRunningActions()
+                        .Where(a => (a.ActionScope == ActionScope.NODE || a.ActionScope == ActionScope.EDGE) &&
+                                     a.BlockingType == BlockingType.SOFT)
+                        .ToList();
+
+                    // Try to start next action(s) from queue
+                    while (!ActionWaitingRunning.IsEmpty)
+                    {
+                        if (ActionWaitingRunning.TryPeek(out Action? action) && action is not null)
+                        {
+                            var robotAction = ActionManager[action.ActionId];
+                            if (robotAction is null)
+                            {
+                                // Action not found - dequeue and skip it
+                                ActionWaitingRunning.TryDequeue(out _);
+                                Logger.LogWarning($"Action {action.ActionId} (type: {action.ActionType}) not found in ActionManager - skipping action");
+                                continue;
+                            }
+
+                            // VDA5050: Check if action can start based on blocking type
+                            if (action.BlockingType == BlockingType.HARD)
+                            {
+                                // HARD can only start if no actions are running
+                                if (runningSoftActions.Count > 0)
+                                {
+                                    // Wait for SOFT actions to complete
+                                    break;
+                                }
+                                // Start HARD action and set flag
+                                ActionWaitingRunning.TryDequeue(out _);
+                                ActionManager.StartOrderAction(action.ActionId);
+                                ActionHard = action;
+                                break; // Only one HARD action at a time
+                            }
+                            else if (action.BlockingType == BlockingType.SOFT)
+                            {
+                                // SOFT can start in parallel with other SOFT actions
+                                ActionWaitingRunning.TryDequeue(out _);
+                                ActionManager.StartOrderAction(action.ActionId);
+                                runningSoftActions.Add(robotAction);
+                                // Continue to potentially start more SOFT actions
+                            }
+                            else
+                            {
+                                // NONE should have been started already, but handle it anyway
+                                ActionWaitingRunning.TryDequeue(out _);
+                                ActionManager.StartOrderAction(action.ActionId);
+                            }
+                        }
+                        else
+                        {
+                            break;
+                        }
+                    }
+                }
+            }
+            else
+            {
+                if (IsWaitingPaused)
+                {
+                    IsWaitingPaused = false;
+                    NavigationManager.Resume();
+                    if (CurrentBaseNode is not null
+                        && CurrentBaseNode.NodeId != Nodes[0].NodeId
+                        && CurrentBaseNode.NodeId != Nodes[^1].NodeId
+                        && Nodes.Length > 1)
+                    {
+                        NavigationManager.UpdateOrder(CurrentBaseNode.NodeId);
+                    }
+                }
+            }
+        }
+    }
+
+    private async void OrderHandler()
+    {
+        try
+        {
+            if (NewOrder is not null)
+            {
+                OrderMsg NewOrderHandler;
+                lock (LockObject)
+                {
+                    NewOrderHandler = NewOrder;
+                    NewOrder = null;
+                }
+
+                if (NewOrderHandler.Nodes.Length == 0) throw new OrderException(RobotErrors.Error1002(NewOrderHandler.Nodes.Length));
+                if (NewOrderHandler.Edges.Length != NewOrderHandler.Nodes.Length - 1) throw new OrderException(RobotErrors.Error1004(NewOrderHandler.Nodes.Length, NewOrderHandler.Edges.Length));
+
+                if (NodeStates.Length != 0 || EdgeStates.Length != 0) HandleUpdateOrder(NewOrderHandler);
+                else
+                {
+                    if (ActionManager.HasActionRunning) return;
+                    // Kiểm tra robot có nằm trên node đầu tien không
+                    Node startNode = NewOrderHandler.Nodes[0];
+                    var nodeDeviation = startNode.NodePosition?.AllowedDeviationXY == 0.0 ? NewOrderHandler.Nodes.Length == 1 ? 0.3 : 0.5 : startNode.NodePosition?.AllowedDeviationXY ?? 0.5;
+                    var distance = Localization.DistanceTo(startNode.NodePosition?.X ?? 0, startNode.NodePosition?.Y ?? 0);
+                    if (distance > nodeDeviation) throw new OrderException(RobotErrors.Error1016(startNode.NodeId, distance, nodeDeviation));
+
+                    if (NewOrderHandler.Nodes.Length > 1)
+                    {
+                        Node endNode = NewOrderHandler.Nodes[^1];
+                        nodeDeviation = endNode.NodePosition?.AllowedDeviationXY == 0.0 ? 0.2 : endNode.NodePosition?.AllowedDeviationXY ?? 0.2;
+                        distance = Localization.DistanceTo(endNode.NodePosition?.X ?? 0, endNode.NodePosition?.Y ?? 0);
+                        if (distance < nodeDeviation) throw new OrderException(RobotErrors.Error1017(endNode.NodeId, distance, nodeDeviation));
+                    }
+                    await HandleNewOrder(NewOrderHandler);
+                }
+            }
+            HandleOrder();
+        }
+        catch (RobotException orEx)
+        {
+            if (orEx.Error is not null)
+            {
+                ErrorManager.AddError(orEx.Error);
+                Logger.LogWarning("Order processing error: {orEx.Error.ErrorDescription}", orEx.Error.ErrorDescription);
+            }
+            else Logger.LogWarning("Order processing error: {orEx.Message}", orEx.Message);
+        }
+        catch (Exception ex)
+        {
+            Logger.LogWarning("Order processing error: {ex.Message}", ex.Message);
+        }
+    }
+
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/RobotPhysicalConfig.cs

@@ -0,0 +1,14 @@
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Shared.Enums;
+
+namespace RobotNet10.RobotApp.Services.Robot;
+
+public class RobotPhysicalConfig
+{
+    public double WheelBase { get; set; }
+    public double WheelRadius { get; set; }
+    public double Width { get; set; }
+    public double Length { get; set; } 
+    public double Height { get; set; }
+    public NavigationType NavigationType { get; set; }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/RobotStates.cs

@@ -0,0 +1,142 @@
+using RobotNet.VDA5050;
+using RobotNet.VDA5050.State;
+using RobotNet.VDA5050.Type;
+using RobotNet10.Common;
+using RobotNet10.RobotApp.Devices;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Motion;
+using RobotNet10.RobotApp.Services.ConfigManager;
+using RobotNet10.RobotApp.Services.Navigation;
+using RobotNet10.RobotApp.Services.Robot.Connection;
+using RobotNet10.RobotApp.Services.State;
+
+namespace RobotNet10.RobotApp.Services.Robot;
+
+public class RobotStates(IConnectionConfig ConnectionConfig,
+                         IRobotConnectionsService RobotConnectionsService,
+                         RobotStateMachine StateManager,
+                         ILogger<RobotStates> Logger,
+                         IOrder OrderManager,
+                         IAction ActionManager,
+                         IPlcController PeripheralManager,
+                         IInfomation InfoManager,
+                         IError ErrorManager, 
+                         ILocalization LocalizationManager,
+                         IDeviceProvider DeviceProvider,
+                         ILoad LoadManager,
+                         INavigation NavigationManager,
+                         IVelocityController VelocityController) : IState
+{
+    private uint HeaderId = 0;
+
+    private WatchTimerAsync<RobotStates>? UpdateStateTimer;
+    private const int UpdateStateInterval = 1000;
+
+    public async Task PubState()
+    {
+        try
+        {
+            if (!RobotConnectionsService.IsConnected) return;
+            await RobotConnectionsService.PublishStateAsync(GetStateMsg());
+        }
+        catch { }
+    }
+
+    private StateMsg GetStateMsg()
+    {
+        var vdaConfig = ConnectionConfig.GetVDA5050Config();
+        var batteryDevice = DeviceProvider.GetDevice("battery-varta-001");
+        RobotNet10.Shared.Sensor.BatteryState batteryState = new();
+        if (batteryDevice is IBattery battery && battery.CurrentBatteryState.HasValue && battery.CurrentBatteryState is RobotNet10.Shared.Sensor.BatteryState state)
+        {
+            batteryState = state;
+        }
+        return new StateMsg
+        {
+            HeaderId = HeaderId++,
+            Manufacturer = vdaConfig.Manufacturer,
+            Version = vdaConfig.Version,
+            SerialNumber = vdaConfig.SerialNumber,
+            Maps = [],
+            OrderId = OrderManager.OrderId,
+            OrderUpdateId = OrderManager.OrderUpdateId,
+            ZoneSetId = LocalizationManager.CurrentActiveMap,
+            LastNodeId = OrderManager.LastNodeId,
+            LastNodeSequenceId = OrderManager.LastNodeSequenceId,
+            Driving = NavigationManager.Driving,
+            Paused = OrderManager.IsPaused,
+            NewBaseRequest = OrderManager.NewBaseRequest,
+            DistanceSinceLastNode = OrderManager.DistanceSinceLastNode,
+            OperatingMode = PeripheralManager.PeripheralMode.ToString(),
+            NodeStates = OrderManager.NodeStates,
+            EdgeStates = OrderManager.EdgeStates,
+            ActionStates = ActionManager.ActionStates,
+            Information = [General, .. InfoManager.InformationState],
+            Errors = ErrorManager.ErrorsState,
+            AgvPosition = new()
+            {
+                X = LocalizationManager.X,
+                Y = LocalizationManager.Y,
+                Theta = LocalizationManager.Theta,
+                LocalizationScore = LocalizationManager.LocalizationScore,
+                MapId = LocalizationManager.CurrentActiveMap,
+                DeviationRange = LocalizationManager.DeviationRange,
+                PositionInitialized = LocalizationManager.PositionInitialized,
+            },
+            BatteryState = new()
+            {
+                Charging = batteryState.Current > 0,
+                BatteryHealth = 100,
+                Reach = 0,
+                BatteryVoltage = batteryState.Voltage is double.NaN ? 0 : batteryState.Voltage,
+                BatteryCharge = batteryState.Percentage is double.NaN ? 0 : batteryState.Percentage,
+            },
+            Loads = LoadManager.Load,
+            Velocity = new()
+            {
+                Vx = VelocityController.ActualVelocity.Linear,
+                Vy = 0,
+                Omega = VelocityController.ActualVelocity.Angular,
+            },
+            SafetyState = new()
+            {
+                FieldViolation = !PeripheralManager.LidarBackProtectField || !PeripheralManager.LidarFrontProtectField || PeripheralManager.LidarFrontTimProtectField,
+                EStop = PeripheralManager.Emergency || PeripheralManager.Bumper ? EStop.AUTOACK : EStop.NONE,
+            }
+        };
+    }
+
+    private Information General => new()
+    {
+        InfoType = InformationType.GENERAL.ToJsonString(),
+        InfoDescription = "Thông tin chung của robot",
+        InfoLevel = InfoLevel.INFO,
+        InfoReferences =
+        [
+            new InfomationReference
+            {
+                ReferenceKey = InformationReferencesKey.STATE.ToJsonString(),
+                ReferenceValue = StateManager.CurrentState.ToString(),
+            },
+        ],
+    };
+
+    private async Task UpdateStateHandler()
+    {
+        await PubState();
+    }
+
+    public void Start()
+    {
+        if (UpdateStateTimer is not null) Stop();
+
+        UpdateStateTimer = new(UpdateStateInterval, UpdateStateHandler, Logger);
+        UpdateStateTimer.Start();
+    }
+
+    public void Stop()
+    {
+        UpdateStateTimer?.Dispose();
+        UpdateStateTimer = null;
+    }   
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/RobotVisualization.cs

@@ -0,0 +1,67 @@
+using RobotNet.VDA5050.Visualization;
+using RobotNet10.Common;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Services.ConfigManager;
+using RobotNet10.RobotApp.Services.Robot.Connection;
+
+namespace RobotNet10.RobotApp.Services.Robot;
+
+public class RobotVisualization(ILocalization Localization,
+                                INavigation Navigation,
+                                IConnectionConfig ConnectionConfig,
+                                IRobotConnectionsService RobotConnectionsService,
+                                ILogger<RobotVisualization> Logger)
+{
+    private uint HeaderId;
+
+    private WatchThread<RobotVisualization>? UpdateTimer;
+    private const int UpdateInterval = 100;
+    private VisualizationMsg GetVisualizationMsg()
+    {
+        var vdaConfig = ConnectionConfig.GetVDA5050Config();
+        return new VisualizationMsg()
+        {
+            HeaderId = HeaderId++,
+            Manufacturer = vdaConfig.Manufacturer,
+            Version = vdaConfig.Version,
+            SerialNumber = vdaConfig.SerialNumber,
+            AgvPosition = new AgvPosition()
+            {
+                X = Localization.X,
+                Y = Localization.Y,
+                Theta = Localization.Theta
+            },
+            Velocity = new Velocity()
+            {
+                Vx = Navigation.VelocityX,
+                Vy = Navigation.VelocityY,
+                Omega = Navigation.Omega
+            }
+        };
+    }
+
+    private void UpdateHandler()
+    {
+        try
+        {
+            if (!RobotConnectionsService.IsConnected) return;
+            var publish = RobotConnectionsService.PublishVisualizationAsync(GetVisualizationMsg());
+            publish.ConfigureAwait(false);
+        }
+        catch { }
+    }
+
+    public void Start()
+    {
+        if (UpdateTimer is not null) Stop();
+
+        UpdateTimer = new(UpdateInterval, UpdateHandler, Logger, ThreadPriority.Normal);
+        UpdateTimer.Start();
+    }
+
+    public void Stop()
+    {
+        UpdateTimer?.Dispose();
+        UpdateTimer = null;
+    }
+}