Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/ConfigManager/ConnectionConfig.cs

@@ -0,0 +1,369 @@
+using RobotNet10.CustomConfiguration.Events;
+using RobotNet10.CustomConfiguration.Models;
+using RobotNet10.CustomConfiguration.Services;
+using RobotNet10.MqttConnection;
+using RobotNet10.RobotApp.Services.Robot.Connection.Models;
+using System.Reflection;
+using System.Text.Json;
+
+namespace RobotNet10.RobotApp.Services.ConfigManager;
+
+/// <summary>
+/// Service implementation for managing application configurations
+/// </summary>
+public class ConnectionConfig : IConnectionConfig
+{
+    private readonly IConfigManager _configManager;
+    private readonly Logger<ConnectionConfig> _logger;
+
+    private readonly Lock _lockObject = new();
+    private VDA5050ProtocolConfig? _vda5050Config;
+    private MQTTConfig? _mqttConfig;
+    private bool _vda5050ConfigLoaded = false;
+    private bool _mqttConfigLoaded = false;
+
+    private const string VDA5050_PROTOCOL_CONFIG_TYPE = "VDA5050ProtocolConfig";
+    private const string MQTT_CONFIG_TYPE = "MQTTConfig";
+    private const string VDA5050_CONFIG_FILE_NAME = "VDA5050ProtocolConfig.config.json";
+    private const string MQTT_CONFIG_FILE_NAME = "MQTTConfig.config.json";
+
+    public ConnectionConfig(IConfigManager configManager, Logger<ConnectionConfig> logger)
+    {
+        _configManager = configManager ?? throw new ArgumentNullException(nameof(configManager));
+        _logger = logger;
+        
+        // Subscribe to config changes
+        _configManager.ConfigChanged += OnConfigChanged;
+    }
+
+    public VDA5050ProtocolConfig GetVDA5050Config()
+    {
+        if (!_vda5050ConfigLoaded)
+        {
+            lock (_lockObject)
+            {
+                if (!_vda5050ConfigLoaded)
+                {
+                    LoadVDA5050ConfigAsync().GetAwaiter().GetResult();
+                }
+            }
+        }
+        return _vda5050Config ?? throw new InvalidOperationException("VDA5050 configuration not loaded");
+    }
+
+    public MQTTConfig GetMqttConfig()
+    {
+        if (!_mqttConfigLoaded)
+        {
+            lock (_lockObject)
+            {
+                if (!_mqttConfigLoaded)
+                {
+                    LoadMqttConfigAsync().GetAwaiter().GetResult();
+                }
+            }
+        }
+        return _mqttConfig ?? throw new InvalidOperationException("MQTT configuration not loaded");
+    }
+
+    private async Task LoadVDA5050ConfigAsync()
+    {
+        try
+        {
+            var configFile = await _configManager.GetConfigByTypeAsync(VDA5050_PROTOCOL_CONFIG_TYPE);
+            
+            if (configFile == null)
+            {
+                if (!TryLoadVda5050ConfigFromLocalFile(out var vdaConfig))
+                {
+                    _logger.Warning("VDA5050 Protocol configuration not found, using defaults");
+                    _vda5050Config = new VDA5050ProtocolConfig
+                    {
+                        Manufacturer = "RobotNet",
+                        Version = "2.1.0",
+                        TopicPrefix = "uagv/v2",
+                        SerialNumber = "Robot-Dev"
+                    };
+                }
+                else
+                {
+                    _vda5050Config = vdaConfig;
+                }
+            }
+            else
+            {
+                _vda5050Config = MapConfigVariablesToObject<VDA5050ProtocolConfig>(configFile.Variables);
+                _logger.Info("VDA5050 Protocol configuration loaded successfully");
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error loading VDA5050 Protocol configuration, using defaults: {ex.Message}");
+            _vda5050Config = new VDA5050ProtocolConfig
+            {
+                Manufacturer = "RobotNet",
+                Version = "2.1.0",
+                TopicPrefix = "uagv/v2"
+            };
+        }
+        finally
+        {
+            _vda5050ConfigLoaded = true;
+        }
+    }
+
+    private async Task LoadMqttConfigAsync()
+    {
+        var configFile = await _configManager.GetConfigByTypeAsync(MQTT_CONFIG_TYPE);
+        if (configFile is not null)
+        {
+            _mqttConfig = MapConfigVariablesToObject<MQTTConfig>(configFile.Variables);
+        }
+        else if (TryLoadMqttConfigFromLocalFile(out var localMqttConfig))
+        {
+            _mqttConfig = localMqttConfig;
+        }
+        else
+        {
+            throw new InvalidOperationException($"MQTT configuration (ConfigType: {MQTT_CONFIG_TYPE}) not found");
+        }
+
+        if (string.IsNullOrEmpty(_mqttConfig?.Host))
+        {
+            throw new InvalidOperationException("MQTT configuration: Host is required");
+        }
+
+        if (string.IsNullOrEmpty(_mqttConfig.ClientId))
+        {
+            throw new InvalidOperationException("MQTT configuration: ClientId is required");
+        }
+
+        _mqttConfigLoaded = true;
+        _logger.Info("MQTT configuration loaded successfully");
+    }
+
+    private bool TryLoadVda5050ConfigFromLocalFile(out VDA5050ProtocolConfig config)
+    {
+        config = new VDA5050ProtocolConfig();
+        foreach (var path in GetLocalConfigCandidates(VDA5050_CONFIG_FILE_NAME))
+        {
+            try
+            {
+                if (!File.Exists(path))
+                {
+                    continue;
+                }
+
+                using var doc = JsonDocument.Parse(File.ReadAllText(path));
+                config = new VDA5050ProtocolConfig
+                {
+                    Manufacturer = GetStringVariable(doc.RootElement, "Manufacturer", "RobotNet"),
+                    Version = GetStringVariable(doc.RootElement, "Version", "2.1.0"),
+                    TopicPrefix = GetStringVariable(doc.RootElement, "TopicPrefix", "uagv/v2"),
+                    SerialNumber = GetStringVariable(doc.RootElement, "SerialNumber", "Robot-Dev")
+                };
+
+                _logger.Info($"VDA5050 Protocol configuration loaded from local file: {path}");
+                return true;
+            }
+            catch (Exception ex)
+            {
+                _logger.Warning($"Failed reading VDA5050 config file '{path}': {ex.Message}");
+            }
+        }
+
+        return false;
+    }
+
+    private bool TryLoadMqttConfigFromLocalFile(out MQTTConfig config)
+    {
+        config = new MQTTConfig();
+        foreach (var path in GetLocalConfigCandidates(MQTT_CONFIG_FILE_NAME))
+        {
+            try
+            {
+                if (!File.Exists(path))
+                {
+                    continue;
+                }
+
+                using var doc = JsonDocument.Parse(File.ReadAllText(path));
+                config = new MQTTConfig
+                {
+                    Host = GetStringVariable(doc.RootElement, "Host", string.Empty),
+                    Port = GetIntVariable(doc.RootElement, "Port", 1883),
+                    ClientId = GetStringVariable(doc.RootElement, "ClientId", string.Empty),
+                    Username = GetStringVariable(doc.RootElement, "Username", string.Empty),
+                    Password = GetStringVariable(doc.RootElement, "Password", string.Empty),
+                    EnablePassword = GetBoolVariable(doc.RootElement, "EnablePassword", false),
+                    EnableTls = GetBoolVariable(doc.RootElement, "EnableTls", false),
+                    EnableCA = GetBoolVariable(doc.RootElement, "EnableCA", false),
+                    CaCertificatesPath = GetStringVariable(doc.RootElement, "CaCertificatesPath", string.Empty),
+                    ClientCertificatePath = GetStringVariable(doc.RootElement, "ClientCertificatePath", string.Empty),
+                    ClientKeyPath = GetStringVariable(doc.RootElement, "ClientKeyPath", string.Empty),
+                    PublishRepeat = GetIntVariable(doc.RootElement, "PublishRepeat", 2)
+                };
+
+                _logger.Info($"MQTT configuration loaded from local file: {path}");
+                return true;
+            }
+            catch (Exception ex)
+            {
+                _logger.Warning($"Failed reading MQTT config file '{path}': {ex.Message}");
+            }
+        }
+
+        return false;
+    }
+
+    private static IEnumerable<string> GetLocalConfigCandidates(string fileName)
+    {
+        yield return Path.Combine(Directory.GetCurrentDirectory(), "RobotAppCustomConfigs", "configs", fileName);
+        yield return Path.Combine(AppContext.BaseDirectory, "RobotAppCustomConfigs", "configs", fileName);
+    }
+
+    private static string GetStringVariable(JsonElement root, string variableName, string fallback)
+    {
+        if (!root.TryGetProperty("variables", out var variables) || variables.ValueKind != JsonValueKind.Array)
+        {
+            return fallback;
+        }
+
+        foreach (var variable in variables.EnumerateArray())
+        {
+            if (!variable.TryGetProperty("name", out var nameElement) || !string.Equals(nameElement.GetString(), variableName, StringComparison.Ordinal))
+            {
+                continue;
+            }
+
+            if (variable.TryGetProperty("value", out var valueElement) && valueElement.ValueKind != JsonValueKind.Null)
+            {
+                return valueElement.ToString() ?? fallback;
+            }
+        }
+
+        return fallback;
+    }
+
+    private static int GetIntVariable(JsonElement root, string variableName, int fallback)
+    {
+        var rawValue = GetStringVariable(root, variableName, fallback.ToString());
+        return int.TryParse(rawValue, out var value) ? value : fallback;
+    }
+
+    private static bool GetBoolVariable(JsonElement root, string variableName, bool fallback)
+    {
+        var rawValue = GetStringVariable(root, variableName, fallback.ToString());
+        return bool.TryParse(rawValue, out var value) ? value : fallback;
+    }
+
+    private void OnConfigChanged(object? sender, ConfigChangedEventArgs e)
+    {
+        // Reload config if it's one of our config types
+        if (e.ConfigType == VDA5050_PROTOCOL_CONFIG_TYPE)
+        {
+            lock (_lockObject)
+            {
+                _vda5050ConfigLoaded = false;
+                _vda5050Config = null;
+            }
+            _logger.Info("VDA5050 Protocol configuration changed, will reload on next access");
+        }
+        else if (e.ConfigType == MQTT_CONFIG_TYPE)
+        {
+            lock (_lockObject)
+            {
+                _mqttConfigLoaded = false;
+                _mqttConfig = null;
+            }
+            _logger.Info("MQTT configuration changed, will reload on next access");
+        }
+    }
+
+    private T MapConfigVariablesToObject<T>(List<ConfigVariable> variables) where T : new()
+    {
+        var obj = new T();
+        var properties = typeof(T).GetProperties(BindingFlags.Public | BindingFlags.Instance | BindingFlags.SetProperty);
+
+        foreach (var property in properties)
+        {
+            // Find variable by exact name match (case-sensitive)
+            var variable = variables.FirstOrDefault(v => v.Name == property.Name);
+            
+            if (variable != null && variable.Value != null)
+            {
+                try
+                {
+                    var convertedValue = ConvertValue(variable.Value, property.PropertyType, variable.Type);
+                    property.SetValue(obj, convertedValue);
+                }
+                catch (Exception ex)
+                {
+                    _logger.Warning($"Failed to set property {property.Name} from variable {variable.Name}: {ex.Message}");
+                }
+            }
+        }
+
+        return obj;
+    }
+
+    private object? ConvertValue(object? value, Type targetType, ConfigVariableType variableType)
+    {
+        if (value == null)
+            return targetType.IsValueType ? Activator.CreateInstance(targetType) : null;
+
+        // If value is already of the correct type, return it
+        if (targetType.IsInstanceOfType(value))
+            return value;
+
+        try
+        {
+            // Handle nullable types
+            var underlyingType = Nullable.GetUnderlyingType(targetType) ?? targetType;
+
+            // Convert based on target type
+            if (underlyingType == typeof(string))
+            {
+                return value.ToString() ?? string.Empty;
+            }
+            else if (underlyingType == typeof(int))
+            {
+                if (value is int i) return i;
+                if (int.TryParse(value.ToString(), out var parsed)) return parsed;
+                if (value is double d) return (int)d;
+                throw new InvalidCastException($"Cannot convert {value.GetType()} to int");
+            }
+            else if (underlyingType == typeof(double))
+            {
+                if (value is double d) return d;
+                if (double.TryParse(value.ToString(), out var parsed)) return parsed;
+                if (value is int i) return i;
+                throw new InvalidCastException($"Cannot convert {value.GetType()} to double");
+            }
+            else if (underlyingType == typeof(bool))
+            {
+                if (value is bool b) return b;
+                if (bool.TryParse(value.ToString(), out var parsed)) return parsed;
+                // Handle numeric values: 0/1, "0"/"1", etc.
+                if (value.ToString()?.Trim() == "0" || value.ToString()?.Trim().ToLower() == "false") return false;
+                if (value.ToString()?.Trim() == "1" || value.ToString()?.Trim().ToLower() == "true") return true;
+                throw new InvalidCastException($"Cannot convert {value.GetType()} to bool");
+            }
+            else if (underlyingType.IsEnum)
+            {
+                var enumValue = Enum.Parse(underlyingType, value.ToString() ?? "", true);
+                return enumValue;
+            }
+            else
+            {
+                // Try standard conversion
+                return Convert.ChangeType(value, underlyingType);
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger.Warning($"Error converting value {value} of type {value.GetType()} to {targetType}: {ex.Message}");
+            throw;
+        }
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/ConfigManager/IConnectionConfig.cs

@@ -0,0 +1,20 @@
+using RobotNet10.MqttConnection;
+using RobotNet10.RobotApp.Services.Robot.Connection.Models;
+
+namespace RobotNet10.RobotApp.Services.ConfigManager;
+
+/// <summary>
+/// Service for managing application configurations
+/// </summary>
+public interface IConnectionConfig
+{
+    /// <summary>
+    /// Get VDA5050 Protocol configuration
+    /// </summary>
+    VDA5050ProtocolConfig GetVDA5050Config();
+
+    /// <summary>
+    /// Get MQTT configuration
+    /// </summary>
+    MQTTConfig GetMqttConfig();
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/ConfigManager/INavigationConfig.cs

@@ -0,0 +1,17 @@
+using RobotNet10.RobotApp.Services.Navigation.CSharp;
+
+namespace RobotNet10.RobotApp.Services.ConfigManager;
+public interface INavigationConfig
+{
+    PurePursuitConfig GetPurepursuitConfig();
+    VelocitySignalProcessingConfig GetVelocitySignalProcessingConfig();
+    VelocityEstimatorConfig GetVelocityEstimatorConfig();
+    MotorDynamicsConfig GetMotorDynamicsConfig();
+    PIDConfig GetMovePidConfig();
+    PIDConfig GetRotatePidConfig();
+    Navigation.NavigationConfig GetNavigationConfig();
+    NavigationTune.Shared.Models.StanleyConfig GetStanleyConig();
+    DockToConfig GetDockToConfig();
+    DockToConfig GetMoveStraightConfig();
+    LocalPlannerConfig GetLocalPlannerConfig();
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/ConfigManager/IRobotConfiguration.cs

@@ -0,0 +1,20 @@
+using RobotNet10.RobotApp.Services.Robot;
+using RobotNet10.RobotApp.Services.Simulation;
+
+namespace RobotNet10.RobotApp.Services.ConfigManager;
+
+/// <summary>
+/// Service for managing robot configurations
+/// </summary>
+public interface IRobotConfiguration
+{
+    /// <summary>
+    /// Get Robot Physical configuration
+    /// </summary>
+    RobotPhysicalConfig GetRobotPhysicalConfig();
+
+    /// <summary>
+    /// Get Simulation configuration
+    /// </summary>
+    SimulationConfig GetSimulationConfig();
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/ConfigManager/RobotConfiguration.cs

@@ -0,0 +1,456 @@
+using RobotNet10.CustomConfiguration.Events;
+using RobotNet10.CustomConfiguration.Models;
+using RobotNet10.CustomConfiguration.Services;
+using RobotNet10.RobotApp.Services.Robot;
+using RobotNet10.RobotApp.Services.Simulation;
+using RobotNet10.RobotApp.Shared.Enums;
+using System.Reflection;
+
+namespace RobotNet10.RobotApp.Services.ConfigManager;
+
+/// <summary>
+/// Service implementation for managing robot configurations
+/// </summary>
+public class RobotConfiguration : IRobotConfiguration
+{
+    private readonly IConfigManager _configManager;
+    private readonly Logger<RobotConfiguration> _logger;
+
+    private readonly Lock _lockObject = new();
+    private RobotPhysicalConfig? _robotPhysicalConfig;
+    private SimulationConfig? _simulationConfig;
+    private bool _robotPhysicalConfigLoaded = false;
+    private bool _simulationConfigLoaded = false;
+
+    private const string ROBOT_PHYSICAL_CONFIG_TYPE = "RobotPhysicalConfig";
+    private const string SIMULATION_CONFIG_TYPE = "SimulationConfig";
+
+    public RobotConfiguration(IConfigManager configManager, Logger<RobotConfiguration> logger)
+    {
+        _configManager = configManager ?? throw new ArgumentNullException(nameof(configManager));
+        _logger = logger;
+        
+        // Subscribe to config changes
+        _configManager.ConfigChanged += OnConfigChanged;
+    }
+
+    public RobotPhysicalConfig GetRobotPhysicalConfig()
+    {
+        if (!_robotPhysicalConfigLoaded)
+        {
+            lock (_lockObject)
+            {
+                if (!_robotPhysicalConfigLoaded)
+                {
+                    LoadRobotPhysicalConfigAsync().GetAwaiter().GetResult();
+                }
+            }
+        }
+        return _robotPhysicalConfig ?? throw new InvalidOperationException("Robot Physical configuration not loaded");
+    }
+
+    public SimulationConfig GetSimulationConfig()
+    {
+        if (!_simulationConfigLoaded)
+        {
+            lock (_lockObject)
+            {
+                if (!_simulationConfigLoaded)
+                {
+                    LoadSimulationConfigAsync().GetAwaiter().GetResult();
+                }
+            }
+        }
+        return _simulationConfig ?? throw new InvalidOperationException("Simulation configuration not loaded");
+    }
+
+    private async Task LoadRobotPhysicalConfigAsync()
+    {
+        try
+        {
+            var configFile = await _configManager.GetConfigByTypeAsync(ROBOT_PHYSICAL_CONFIG_TYPE);
+            
+            if (configFile == null)
+            {
+                _logger.Warning("Robot Physical configuration not found, using defaults");
+                _robotPhysicalConfig = new RobotPhysicalConfig
+                {
+                    WheelRadius = 0.1,
+                    WheelBase = 0.6,
+                    Width = 0.606,
+                    Length = 1.106,
+                    Height = 0.5,
+                    NavigationType = NavigationType.Differential
+                };
+            }
+            else
+            {
+                _robotPhysicalConfig = MapConfigVariablesToRobotPhysicalConfig(configFile.Variables);
+                _logger.Info("Robot Physical configuration loaded successfully");
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error loading Robot Physical configuration, using defaults: {ex.Message}");
+            _robotPhysicalConfig = new RobotPhysicalConfig
+            {
+                WheelRadius = 0.1,
+                WheelBase = 0.6,
+                Width = 0.606,
+                Length = 1.106,
+                Height = 0.5,
+                NavigationType = NavigationType.Differential
+            };
+        }
+        finally
+        {
+            _robotPhysicalConfigLoaded = true;
+        }
+    }
+
+    private async Task LoadSimulationConfigAsync()
+    {
+        try
+        {
+            var configFile = await _configManager.GetConfigByTypeAsync(SIMULATION_CONFIG_TYPE);
+            
+            if (configFile == null)
+            {
+                _logger.Warning("Simulation configuration not found, using defaults");
+                _simulationConfig = new SimulationConfig
+                {
+                    IsEnable = false,
+                    MaxVelocity = 1.5,
+                    MaxAngularVelocity = 0.5,
+                    Acceleration = 2.0,
+                    Deceleration = 10.0
+                };
+            }
+            else
+            {
+                _simulationConfig = MapConfigVariablesToObject<SimulationConfig>(configFile.Variables);
+                _logger.Info("Simulation configuration loaded successfully");
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error loading Simulation configuration, using defaults: {ex.Message}");
+            _simulationConfig = new SimulationConfig
+            {
+                IsEnable = false,
+                MaxVelocity = 1.5,
+                MaxAngularVelocity = 0.5,
+                Acceleration = 2.0,
+                Deceleration = 10.0
+            };
+        }
+        finally
+        {
+            _simulationConfigLoaded = true;
+        }
+    }
+
+    private void OnConfigChanged(object? sender, ConfigChangedEventArgs e)
+    {
+        // Reload config if it's one of our config types
+        if (e.ConfigType == ROBOT_PHYSICAL_CONFIG_TYPE)
+        {
+            lock (_lockObject)
+            {
+                _robotPhysicalConfigLoaded = false;
+                _robotPhysicalConfig = null;
+            }
+            _logger.Info("Robot Physical configuration changed, will reload on next access");
+        }
+        else if (e.ConfigType == SIMULATION_CONFIG_TYPE)
+        {
+            lock (_lockObject)
+            {
+                _simulationConfigLoaded = false;
+                _simulationConfig = null;
+            }
+            _logger.Info("Simulation configuration changed, will reload on next access");
+        }
+    }
+
+    private RobotPhysicalConfig MapConfigVariablesToRobotPhysicalConfig(List<ConfigVariable> variables)
+    {
+        var config = new RobotPhysicalConfig();
+        
+        // Get properties with reflection to handle private setters
+        // Use BindingFlags to get properties with private setters
+        var properties = typeof(RobotPhysicalConfig).GetProperties(BindingFlags.Public | BindingFlags.Instance);
+
+        foreach (var property in properties)
+        {
+            // Find variable by exact name match (case-sensitive)
+            var variable = variables.FirstOrDefault(v => v.Name == property.Name);
+            
+            if (variable != null && variable.Value != null)
+            {
+                try
+                {
+                    var convertedValue = ConvertValue(variable.Value, property.PropertyType, variable.Type);
+                    property.SetValue(config, convertedValue);
+                }
+                catch (Exception ex)
+                {
+                    _logger.Warning($"Failed to set property {property.Name} from variable {variable.Name}: {ex.Message}");
+                }
+            }
+        }
+
+        return config;
+    }
+
+    private T MapConfigVariablesToObject<T>(List<ConfigVariable> variables) where T : new()
+    {
+        var obj = new T();
+        var properties = typeof(T).GetProperties(BindingFlags.Public | BindingFlags.Instance | BindingFlags.SetProperty);
+
+        foreach (var property in properties)
+        {
+            // Find variable by exact name match (case-sensitive)
+            var variable = variables.FirstOrDefault(v => v.Name == property.Name);
+            
+            if (variable != null && variable.Value != null)
+            {
+                try
+                {
+                    var convertedValue = ConvertValue(variable.Value, property.PropertyType, variable.Type);
+                    property.SetValue(obj, convertedValue);
+                }
+                catch (Exception ex)
+                {
+                    _logger.Warning($"Failed to set property {property.Name} from variable {variable.Name}: {ex.Message}");
+                }
+            }
+        }
+
+        return obj;
+    }
+
+    private object? ConvertValue(object? value, Type targetType, ConfigVariableType variableType)
+    {
+        if (value == null)
+            return targetType.IsValueType ? Activator.CreateInstance(targetType) : null;
+
+        // If value is already of the correct type, return it
+        if (targetType.IsInstanceOfType(value))
+            return value;
+
+        try
+        {
+            // Handle nullable types
+            var underlyingType = Nullable.GetUnderlyingType(targetType) ?? targetType;
+
+            // Convert based on target type
+            if (underlyingType == typeof(string))
+            {
+                return value.ToString() ?? string.Empty;
+            }
+            else if (underlyingType == typeof(int))
+            {
+                if (value is int i) return i;
+                if (int.TryParse(value.ToString(), out var parsed)) return parsed;
+                if (value is double d) return (int)d;
+                throw new InvalidCastException($"Cannot convert {value.GetType()} to int");
+            }
+            else if (underlyingType == typeof(double))
+            {
+                if (value is double d) return d;
+                if (double.TryParse(value.ToString(), out var parsed)) return parsed;
+                if (value is int i) return i;
+                throw new InvalidCastException($"Cannot convert {value.GetType()} to double");
+            }
+            else if (underlyingType == typeof(bool))
+            {
+                if (value is bool b) return b;
+                if (bool.TryParse(value.ToString(), out var parsed)) return parsed;
+                // Handle numeric values: 0/1, "0"/"1", etc.
+                if (value.ToString()?.Trim() == "0" || value.ToString()?.Trim().ToLower() == "false") return false;
+                if (value.ToString()?.Trim() == "1" || value.ToString()?.Trim().ToLower() == "true") return true;
+                throw new InvalidCastException($"Cannot convert {value.GetType()} to bool");
+            }
+            else if (underlyingType.IsEnum)
+            {
+                var enumValue = Enum.Parse(underlyingType, value.ToString() ?? "", true);
+                return enumValue;
+            }
+            // Handle Dictionary types (e.g., Dictionary<SafetySpeed, double>)
+            else if (targetType.IsGenericType && targetType.GetGenericTypeDefinition() == typeof(Dictionary<,>))
+            {
+                return ConvertToDictionary(value, targetType);
+            }
+            else
+            {
+                // Try standard conversion
+                return Convert.ChangeType(value, underlyingType);
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger.Warning($"Error converting value {value} of type {value.GetType()} to {targetType}: {ex.Message}");
+            throw;
+        }
+    }
+
+    /// <summary>
+    /// Convert value to Dictionary<TKey, TValue>
+    /// Handles conversion from Dictionary<string, object> (JSON) to typed dictionaries
+    /// Supports enum keys (e.g., Dictionary<SafetySpeed, double>)
+    /// </summary>
+    private object ConvertToDictionary(object value, Type targetDictionaryType)
+    {
+        // Get key and value types from Dictionary<TKey, TValue>
+        var genericArgs = targetDictionaryType.GetGenericArguments();
+        var keyType = genericArgs[0];
+        var valueType = genericArgs[1];
+
+        // Create dictionary instance
+        var dictionaryType = typeof(Dictionary<,>).MakeGenericType(keyType, valueType);
+        var dictionary = Activator.CreateInstance(dictionaryType);
+        var addMethod = dictionaryType.GetMethod("Add") ?? throw new InvalidOperationException("Cannot find Add method on Dictionary");
+
+        // Handle source Dictionary<string, object> from JSON
+        if (value is Dictionary<string, object> sourceDict)
+        {
+            foreach (var kvp in sourceDict)
+            {
+                // Convert key (usually from string to enum)
+                object convertedKey;
+                if (keyType.IsEnum)
+                {
+                    // Parse enum from string
+                    convertedKey = Enum.Parse(keyType, kvp.Key, ignoreCase: true);
+                }
+                else if (keyType == typeof(string))
+                {
+                    convertedKey = kvp.Key;
+                }
+                else
+                {
+                    convertedKey = Convert.ChangeType(kvp.Key, keyType);
+                }
+
+                // Convert value
+                object? convertedValue = ConvertDictionaryValue(kvp.Value, valueType);
+
+                // Add to dictionary
+                addMethod.Invoke(dictionary, [convertedKey, convertedValue]);
+            }
+
+            return dictionary ?? new Dictionary<object, object>();
+        }
+        // Handle JsonElement (alternative JSON representation)
+        else if (value is System.Text.Json.JsonElement jsonElement && jsonElement.ValueKind == System.Text.Json.JsonValueKind.Object)
+        {
+            foreach (var property in jsonElement.EnumerateObject())
+            {
+                // Convert key
+                object convertedKey;
+                if (keyType.IsEnum)
+                {
+                    convertedKey = Enum.Parse(keyType, property.Name, ignoreCase: true);
+                }
+                else if (keyType == typeof(string))
+                {
+                    convertedKey = property.Name;
+                }
+                else
+                {
+                    convertedKey = Convert.ChangeType(property.Name, keyType);
+                }
+
+                // Convert value from JsonElement
+                object? convertedValue = ConvertJsonElementValue(property.Value, valueType);
+
+                // Add to dictionary
+                addMethod.Invoke(dictionary, [convertedKey, convertedValue]);
+            }
+
+            return dictionary ?? new Dictionary<object, object>();
+        }
+        else
+        {
+            throw new InvalidCastException($"Cannot convert {value.GetType()} to {targetDictionaryType}");
+        }
+    }
+
+    /// <summary>
+    /// Convert dictionary value to target type
+    /// </summary>
+    private object? ConvertDictionaryValue(object? value, Type valueType)
+    {
+        if (value == null)
+            return valueType.IsValueType ? Activator.CreateInstance(valueType) : null;
+
+        if (valueType == typeof(double))
+        {
+            if (value is double d) return d;
+            if (double.TryParse(value.ToString(), out var parsed)) return parsed;
+            if (value is int i) return (double)i;
+            return Convert.ChangeType(value, valueType);
+        }
+        else if (valueType == typeof(int))
+        {
+            if (value is int i) return i;
+            if (int.TryParse(value.ToString(), out var parsed)) return parsed;
+            if (value is double d) return (int)d;
+            return Convert.ChangeType(value, valueType);
+        }
+        else if (valueType == typeof(string))
+        {
+            return value.ToString() ?? string.Empty;
+        }
+        else if (valueType == typeof(bool))
+        {
+            if (value is bool b) return b;
+            if (bool.TryParse(value.ToString(), out var parsed)) return parsed;
+            return Convert.ChangeType(value, valueType);
+        }
+        else if (valueType.IsEnum)
+        {
+            return Enum.Parse(valueType, value.ToString() ?? "", ignoreCase: true);
+        }
+        else
+        {
+            return Convert.ChangeType(value, valueType);
+        }
+    }
+
+    /// <summary>
+    /// Convert JsonElement value to target type
+    /// </summary>
+    private object? ConvertJsonElementValue(System.Text.Json.JsonElement element, Type valueType)
+    {
+        if (element.ValueKind == System.Text.Json.JsonValueKind.Null)
+            return valueType.IsValueType ? Activator.CreateInstance(valueType) : null;
+
+        if (valueType == typeof(double))
+        {
+            return element.GetDouble();
+        }
+        else if (valueType == typeof(int))
+        {
+            return element.GetInt32();
+        }
+        else if (valueType == typeof(string))
+        {
+            return element.GetString();
+        }
+        else if (valueType == typeof(bool))
+        {
+            return element.GetBoolean();
+        }
+        else if (valueType.IsEnum)
+        {
+            return Enum.Parse(valueType, element.GetString() ?? "", ignoreCase: true);
+        }
+        else
+        {
+            var rawValue = element.GetRawText();
+            return System.Text.Json.JsonSerializer.Deserialize(rawValue, valueType);
+        }
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Exceptions/ActionException.cs

@@ -0,0 +1,15 @@
+using RobotNet.VDA5050.State;
+using RobotNet10.RobotApp.Services.Robot;
+
+namespace RobotNet10.RobotApp.Services.Exceptions;
+
+public class ActionException : RobotException
+{
+    public ActionException(string message) : base(message) { }
+    public ActionException(string message, Exception inner) : base(message, inner) { }
+    public ActionException() : base() { }
+    public ActionException(RobotError error) : base()
+    {
+        Error = error;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Exceptions/ModbusException.cs

@@ -0,0 +1,15 @@
+using RobotNet.VDA5050.State;
+using RobotNet10.RobotApp.Services.Robot;
+
+namespace RobotNet10.RobotApp.Services.Exceptions;
+
+public class ModbusException : RobotException
+{
+    public ModbusException(string message) : base(message) { }
+    public ModbusException(string message, Exception inner) : base(message, inner) { }
+    public ModbusException() : base() { }
+    public ModbusException(RobotError error) : base()
+    {
+        Error = error;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Exceptions/NavigationException.cs

@@ -0,0 +1,14 @@
+using RobotNet10.RobotApp.Services.Robot;
+
+namespace RobotNet10.RobotApp.Services.Exceptions;
+
+public class NavigationException : RobotException
+{
+    public NavigationException(string message) : base(message) { }
+    public NavigationException(string message, Exception inner) : base(message, inner) { }
+    public NavigationException() : base() { }
+    public NavigationException(RobotError error) : base()
+    {
+        Error = error;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Exceptions/OrderException.cs

@@ -0,0 +1,15 @@
+using RobotNet.VDA5050.State;
+using RobotNet10.RobotApp.Services.Robot;
+
+namespace RobotNet10.RobotApp.Services.Exceptions;
+
+public class OrderException : RobotException
+{
+    public OrderException(string message) : base(message) { }
+    public OrderException(string message, Exception inner) : base(message, inner) { }
+    public OrderException() : base() { }
+    public OrderException(RobotError error) : base()
+    {
+        Error = error;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Exceptions/PathPlannerException.cs

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

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Exceptions/RobotException.cs

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

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Exceptions/SimulationException.cs

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

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Logger.cs

@@ -0,0 +1,108 @@
+namespace RobotNet10.RobotApp.Services;
+
+public class Logger<T>(ILogger<T> Logger) where T : class
+{
+    public event Action? LoggerUpdate;
+    public void Write(string message, LogLevel level)
+    {
+        switch (level)
+        {
+            case LogLevel.Trace:
+                Logger.LogTrace("{mes}", message);
+                break;
+            case LogLevel.Debug:
+                Logger.LogDebug("{mes}", message);
+                break;
+            case LogLevel.Information:
+                Logger.LogInformation("{mes}", message);
+                break;
+            case LogLevel.Warning:
+                Logger.LogWarning("{mes}", message);
+                break;
+            case LogLevel.Error:
+                Logger.LogError("{mes}", message);
+                break;
+            case LogLevel.Critical:
+                Logger.LogCritical("{mes}", message);
+                break;
+        }
+        LoggerUpdate?.Invoke();
+    }
+
+    public void Write(string message)
+    {
+        Write(message, LogLevel.Information);
+    }
+
+    public async Task WriteAsync(string message)
+    {
+        var write = Task.Run(() => Write(message));
+        await write.WaitAsync(CancellationToken.None);
+    }
+
+    public async Task TraceAsync(string message)
+    {
+        var write = Task.Run(() => Write(message, LogLevel.Trace));
+        await write.WaitAsync(CancellationToken.None);
+    }
+
+    public async Task DebugAsync(string message)
+    {
+        var write = Task.Run(() => Write(message, LogLevel.Debug));
+        await write.WaitAsync(CancellationToken.None);
+    }
+
+    public async Task InfoAsync(string message)
+    {
+        var write = Task.Run(() => Write(message, LogLevel.Information));
+        await write.WaitAsync(CancellationToken.None);
+    }
+
+    public async Task WarningAsync(string message)
+    {
+        var write = Task.Run(() => Write(message, LogLevel.Warning));
+        await write.WaitAsync(CancellationToken.None);
+    }
+
+    public async Task ErrorAsync(string message)
+    {
+        var write = Task.Run(() => Write(message, LogLevel.Error));
+        await write.WaitAsync(CancellationToken.None);
+    }
+
+    public async Task CriticalAsync(string message)
+    {
+        var write = Task.Run(() => Write(message, LogLevel.Critical));
+        await write.WaitAsync(CancellationToken.None);
+    }
+
+    public void Trace(string message)
+    {
+        Write(message, LogLevel.Trace);
+    }
+
+    public void Debug(string message)
+    {
+        Write(message, LogLevel.Debug);
+    }
+
+    public void Info(string message)
+    {
+        Write(message, LogLevel.Information);
+    }
+
+    public void Warning(string message)
+    {
+        Write(message, LogLevel.Warning);
+    }
+
+    public void Error(string message)
+    {
+        Write(message, LogLevel.Error);
+    }
+
+    public void Critical(string message)
+    {
+        Write(message, LogLevel.Critical);
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Navigation/CPlusNavigation.cs

@@ -0,0 +1,5 @@
+namespace RobotNet10.RobotApp.Services.Navigation;
+
+public class CPlusNavigation
+{
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Navigation/CSharp/CircularBuffer.cs

@@ -0,0 +1,59 @@
+using RobotNet10.Shared.Numbers;
+using SysNum = System.Numerics;
+
+namespace RobotNet10.RobotApp.Services.Navigation.CSharp;
+
+/// <summary>
+/// Circular buffer để lưu lịch sử (fixed size)
+/// </summary>
+public class CircularBuffer<T>(int capacity) where T : SysNum.INumber<T>
+{
+    private readonly T[] _buffer = new T[capacity];
+    private int _head = 0;
+    private int _count = 0;
+    private readonly int _capacity = capacity;
+
+    public int Count => _count;
+    public int Capacity => _capacity;
+
+    public void Add(T item)
+    {
+        _buffer[_head] = item;
+        _head = (_head + 1) % _capacity;
+
+        if (_count < _capacity)
+            _count++;
+    }
+
+    public void Clear()
+    {
+        _head = 0;
+        _count = 0;
+        Array.Clear(_buffer, 0, _capacity);
+    }
+
+    public T[] ToArray()
+    {
+        T[] result = new T[_count];
+        for (int i = 0; i < _count; i++)
+        {
+            int index = (_head - _count + i + _capacity) % _capacity;
+            result[i] = _buffer[index];
+        }
+        return result;
+    }
+
+    public double Average()
+    {
+        if (_count == 0) return 0;
+
+        T sum = T.Zero;
+        for (int i = 0; i < _count; i++)
+        {
+            int index = (_head - _count + i + _capacity) % _capacity;
+            sum += _buffer[index];
+        }
+        return double.CreateChecked(sum) / _count;
+    }
+}
+

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Navigation/CSharp/DockToController.cs

@@ -0,0 +1,625 @@
+using RobotNet10.Common;
+using RobotNet10.Common.Models;
+using RobotNet10.RobotApp.Services.Simulation;
+using RobotDirection = RobotNet10.RobotApp.Shared.Enums.RobotDirection;
+
+namespace RobotNet10.RobotApp.Services.Navigation.CSharp;
+
+public class DockToConfig
+{
+    #region Core Stanley Parameters
+
+    /// <summary>
+    /// Cross-track error gain (K)
+    /// Default: 2.5
+    ///
+    /// Meaning: How aggressively to correct lateral position error
+    /// Formula: δ = ψ + arctan(K × e / (v + Ks))
+    ///
+    /// ↑ Increase (3.0-5.0):
+    ///   ✓ Faster correction of cross-track error
+    ///   ✓ Tighter path following
+    ///   ✗ May cause oscillation
+    ///   ✗ Less smooth on noisy paths
+    ///
+    /// ↓ Decrease (1.5-2.0):
+    ///   ✓ Smoother motion
+    ///   ✓ Less oscillation
+    ///   ✗ Slower error correction
+    ///   ✗ Larger cross-track error
+    ///
+    /// Tuning Tips:
+    /// - Start: 2.5 for general use
+    /// - High precision: 3.0-4.0
+    /// - Smooth priority: 1.5-2.0
+    /// - Check stability by observing steering oscillation
+    /// </summary>
+    public double K { get; set; } = 2.5;
+
+    /// <summary>
+    /// Softening constant (Ks) - meters/second
+    /// Default: 0.1 m/s
+    ///
+    /// Meaning: Added to velocity denominator to prevent division by zero at low speeds
+    /// Formula: δ = ψ + arctan(K × e / (v + Ks))
+    ///
+    /// ↑ Increase (0.15-0.2):
+    ///   ✓ Less aggressive correction at low speed
+    ///   ✓ Smoother motion when starting
+    ///   ✗ Slower error correction at low speed
+    ///
+    /// ↓ Decrease (0.05-0.08):
+    ///   ✓ More responsive at low speed
+    ///   ✗ May cause oscillation when slow
+    ///   ✗ Risk of instability near zero velocity
+    ///
+    /// Tuning Tips:
+    /// - Should be ~10% of typical operating velocity
+    /// - If robot oscillates when slow: increase to 0.15-0.2
+    /// - If too sluggish at startup: decrease to 0.05-0.08
+    /// </summary>
+    public double Ks { get; set; } = 0.1;
+
+    #endregion
+
+    #region Vehicle Parameters
+
+    /// <summary>
+    /// Wheelbase (L) - distance between front and rear axles (meters)
+    /// Default: 0.5m
+    ///
+    /// Meaning: Distance from rear axle (robot center) to virtual front axle
+    /// Used to calculate front axle position and convert steering angle to angular velocity
+    ///
+    /// IMPORTANT: Must match actual robot geometry
+    ///
+    /// Formula: ω = (v × tan(δ)) / L
+    /// </summary>
+    public double WheelBase { get; set; } = 0.6;
+
+    /// <summary>
+    /// Maximum steering angle (radians)
+    /// Default: 0.5 rad (≈28.6°)
+    ///
+    /// Meaning: Physical limit of equivalent steering angle
+    ///
+    /// ↑ Increase (0.6-0.8 rad ≈ 34-46°):
+    ///   ✓ Sharper turns possible
+    ///   ✗ May exceed robot's turning capability
+    ///
+    /// ↓ Decrease (0.3-0.4 rad ≈ 17-23°):
+    ///   ✓ Safer, gentler turns
+    ///   ✗ Cannot track sharp curves
+    ///
+    /// Tuning Tips:
+    /// - Test robot's max practical turn rate
+    /// - Calculate: δ_max = arctan(L × ω_max / v_typical)
+    /// - Example: L=0.5m, ω_max=2rad/s, v=1m/s → δ_max = 0.785 rad (45°)
+    /// - Conservative: 0.4-0.5 rad
+    /// </summary>
+    public double MaxSteeringAngle { get; set; } = 0.5;
+
+    #endregion
+
+    #region Goal Approach Parameters
+
+    /// <summary>
+    /// Distance to start increasing K gain near goal (meters)
+    /// Default: 1.0m
+    ///
+    /// Meaning: When within this distance, K gain increases linearly
+    /// to improve tracking accuracy during final approach
+    ///
+    /// ↑ Increase (1.5-2.0m):
+    ///   ✓ Earlier tightening, smoother transition
+    ///   ✗ May be too aggressive on long approach
+    ///
+    /// ↓ Decrease (0.5-0.8m):
+    ///   ✓ Only tighten very close to goal
+    ///   ✗ Less time to correct errors
+    ///
+    /// Tuning Tips:
+    /// - Should be larger than GoalTolerance × 10
+    /// - Typical: 0.8-1.5m
+    /// </summary>
+    public double GoalApproachDistance { get; set; } = 1.0;
+
+    /// <summary>
+    /// K gain multiplier at goal position
+    /// Default: 2.0 (K doubles when at goal)
+    ///
+    /// Meaning: At goal, effective K = K × GoalGainMultiplier
+    /// Linearly interpolated from 1.0 at GoalApproachDistance to this value at goal
+    ///
+    /// ↑ Increase (2.5-3.0):
+    ///   ✓ Much tighter tracking near goal
+    ///   ✗ Risk of oscillation
+    ///
+    /// ↓ Decrease (1.3-1.5):
+    ///   ✓ Gentler increase
+    ///   ✗ Less improvement near goal
+    ///
+    /// Tuning Tips:
+    /// - Start at 2.0
+    /// - If oscillating near goal: decrease to 1.5
+    /// - If still drifting: increase to 2.5
+    /// </summary>
+    public double GoalGainMultiplier { get; set; } = 2.0;
+
+    public double ReachedRadius { get; set; } = 0.03;
+    #endregion
+
+    #region Angular Velocity Limit
+
+    /// <summary>
+    /// Maximum angular velocity during dock-to approach (rad/s)
+    /// Default: 0.8 rad/s
+    ///
+    /// Meaning: Clamps the angular velocity output of Stanley controller
+    /// to prevent excessive rotation during docking.
+    ///
+    /// ↑ Increase (1.0-1.5):
+    ///   ✓ Faster heading correction
+    ///   ✗ May overshoot or oscillate during docking
+    ///
+    /// ↓ Decrease (0.3-0.5):
+    ///   ✓ Smoother, more precise docking
+    ///   ✗ Slower heading correction
+    ///
+    /// Tuning Tips:
+    /// - Should be lower than PurePursuit/Stanley MaxAngularVelocity for precise docking
+    /// - Start at 0.8, decrease if robot oscillates during dock
+    /// </summary>
+    public double MaxAngularVelocity { get; set; } = 0.05;
+
+    #endregion
+
+    #region Path Resolution
+
+    /// <summary>
+    /// Waypoint spacing for path sampling (meters)
+    /// Default: 0.05m (5cm)
+    ///
+    /// Meaning: Distance between interpolated path points
+    /// Same as PurePursuit.ResolutionSplit for consistency
+    /// </summary>
+    public double ResolutionSplit { get; set; } = 0.05;
+
+    #endregion
+
+    public RobotDirection DockToDirection { get; set; } = RobotDirection.FORWARD;
+    public double DockToLength { get; set; } = 3;
+
+    #region Fine Positioning
+
+    /// <summary>
+    /// Timeout per FinePositioning attempt (milliseconds).
+    /// Default: 6000ms (6s)
+    /// </summary>
+    public int FinePositioningTimeoutMs { get; set; } = 6000;
+
+    /// <summary>
+    /// Maximum retries for FinePositioning before transitioning to Error.
+    /// Default: 3
+    /// </summary>
+    public int FinePositioningMaxRetries { get; set; } = 3;
+
+    #endregion
+
+    #region Docking Overshoot & FinePositioning Tuning
+
+    /// <summary>
+    /// Distance (meters) from goal at which overshoot detection begins during Docking.
+    /// Default: 0.2m
+    ///
+    /// Meaning: Similar to MovingOvershootDetectionRadius but for Docking phase.
+    /// Smaller default because docking requires higher precision.
+    ///
+    /// ↑ Increase (0.3-0.5):
+    ///   ✓ Earlier overshoot detection
+    ///   ✗ May false-trigger during normal approach deceleration
+    ///
+    /// ↓ Decrease (0.1-0.15):
+    ///   ✓ Fewer false triggers
+    ///   ✗ Robot may overshoot further before detection
+    ///
+    /// Tuning Tips:
+    /// - Should be > ReachedRadius (default 0.03m)
+    /// - If robot frequently enters FinePositioning unnecessarily: increase
+    /// - If robot overshoots too far before FP kicks in: decrease
+    /// </summary>
+    public double DockingOvershootDetectionRadius { get; set; } = 0.2;
+
+    /// <summary>
+    /// Distance (meters) from goal at which PID deceleration begins during Docking.
+    /// Default: 3.0m
+    ///
+    /// Meaning: When distance to goal > this value, robot runs at MaxLinearVelocity.
+    /// Below this distance, PID ramps velocity down proportionally.
+    ///
+    /// ↑ Increase (4-5):
+    ///   ✓ Earlier, smoother deceleration — better for heavy robots
+    ///   ✗ Slower docking approach
+    ///
+    /// ↓ Decrease (1-2):
+    ///   ✓ Faster approach — stays at max speed longer
+    ///   ✗ Risk of overshoot on high-inertia robots
+    ///
+    /// Tuning Tips:
+    /// - Typically smaller than Moving's DecelerationDistance (docking is slower)
+    /// - Should account for DockToMaxSpeed and robot braking capability
+    /// </summary>
+    public double DecelerationDistance { get; set; } = 3.0;
+
+    /// <summary>
+    /// Heading alignment threshold (degrees) for FinePositioning Phase 1 (Align).
+    /// Default: 0.5°
+    ///
+    /// Meaning: Robot must align heading within this tolerance before transitioning
+    /// from Align phase to Advance phase.
+    ///
+    /// ↑ Increase (1-2°):
+    ///   ✓ Faster alignment — less time spent rotating
+    ///   ✗ Robot may drift more during advance due to initial heading error
+    ///
+    /// ↓ Decrease (0.1-0.3°):
+    ///   ✓ More precise heading before advancing
+    ///   ✗ Longer alignment time, may oscillate on noisy heading sensor
+    ///
+    /// Tuning Tips:
+    /// - Must be < ReAlignThresholdDegrees
+    /// - If robot drifts during advance: decrease
+    /// - If alignment takes too long or oscillates: increase
+    /// </summary>
+    public double FineAlignThresholdDegrees { get; set; } = 0.5;
+
+    /// <summary>
+    /// Heading drift threshold (degrees) to trigger re-alignment during FinePositioning Phase 2 (Advance).
+    /// Default: 3.0°
+    ///
+    /// Meaning: If heading error exceeds this during advance, robot stops and re-enters Align phase.
+    ///
+    /// ↑ Increase (5-10°):
+    ///   ✓ Fewer re-alignment interruptions
+    ///   ✗ Robot may approach goal at a large angle, reducing accuracy
+    ///
+    /// ↓ Decrease (1-2°):
+    ///   ✓ Tighter heading control during advance
+    ///   ✗ Frequent re-alignment, slower convergence
+    ///
+    /// Tuning Tips:
+    /// - Must be > FineAlignThresholdDegrees (to avoid immediate re-trigger)
+    /// - Typical ratio: ReAlign ≈ 5-10x FineAlign
+    /// - If robot keeps re-aligning: increase or tune AdvanceHeadingCorrectionGain
+    /// </summary>
+    public double ReAlignThresholdDegrees { get; set; } = 3.0;
+
+    /// <summary>
+    /// P-gain for heading correction during FinePositioning Phase 2 (Advance).
+    /// Default: 1.5
+    ///
+    /// Meaning: Angular velocity correction = headingError × Gain × effectiveLinearVel
+    /// Higher gain → stronger angular correction while advancing.
+    ///
+    /// ↑ Increase (2.0-3.0):
+    ///   ✓ Faster heading correction during advance
+    ///   ✗ May cause oscillation or jerky steering
+    ///
+    /// ↓ Decrease (0.5-1.0):
+    ///   ✓ Smoother advance motion
+    ///   ✗ Heading drift may accumulate, triggering re-alignment
+    ///
+    /// Tuning Tips:
+    /// - Works together with DockToAdvanceMaxAngularVelocity
+    /// - If robot oscillates during advance: decrease
+    /// - If robot drifts and re-aligns too often: increase
+    /// </summary>
+    public double AdvanceHeadingCorrectionGain { get; set; } = 1.5;
+
+    /// <summary>
+    /// Maximum angular velocity (rad/s) for heading correction during FinePositioning Phase 2 (Advance).
+    /// Default: 0.15 rad/s
+    ///
+    /// Meaning: Angular correction during advance is clamped to ±this value.
+    /// Independent of NavigationConfig.MaxAngularVelocity (which is tuned for Moving phase).
+    ///
+    /// ↑ Increase (0.2-0.3):
+    ///   ✓ Stronger heading correction during advance
+    ///   ✗ May cause path deviation or jerky steering
+    ///
+    /// ↓ Decrease (0.05-0.1):
+    ///   ✓ Very smooth, nearly straight-line advance
+    ///   ✗ Cannot correct heading drift effectively, may trigger re-alignment
+    ///
+    /// Tuning Tips:
+    /// - Should be small relative to DockToRetrySpeed to keep motion smooth
+    /// - If advance path is too curved: decrease
+    /// - If heading correction is too weak and triggers frequent re-align: increase
+    /// </summary>
+    public double DockToAdvanceMaxAngularVelocity { get; set; } = 0.15;
+
+    /// <summary>
+    /// Number of consecutive distance-increasing cycles to trigger overshoot during FinePositioning Advance.
+    /// Default: 3
+    ///
+    /// Meaning: During advance, if distance to goal increases for this many consecutive cycles,
+    /// overshoot is declared and a retry is initiated.
+    ///
+    /// ↑ Increase (5-7):
+    ///   ✓ More tolerant of temporary distance fluctuations (sensor noise)
+    ///   ✗ Slower overshoot detection — robot travels further past goal
+    ///
+    /// ↓ Decrease (1-2):
+    ///   ✓ Faster overshoot detection
+    ///   ✗ May false-trigger on sensor noise or minor jitter
+    ///
+    /// Tuning Tips:
+    /// - At 30ms cycle: 3 counts = 90ms detection delay
+    /// - Noisy localization: increase to 5-7
+    /// - Stable localization: 2-3 is sufficient
+    /// </summary>
+    public int FinePositioningOvershootCount { get; set; } = 3;
+
+    #endregion
+
+    #region Continuous Goal Update
+
+    /// <summary>
+    /// Maximum allowed position shift (meters) between consecutive goal updates.
+    /// If new goal is farther than this from old goal, it is rejected.
+    /// Default: 0.5m
+    /// </summary>
+    public double MaxGoalPositionShift { get; set; } = 0.2;
+
+    /// <summary>
+    /// Maximum allowed angle shift (degrees) between consecutive goal updates.
+    /// If new goal orientation differs by more than this from old goal, it is rejected.
+    /// Default: 15 degrees
+    /// </summary>
+    public double MaxGoalAngleShiftDegrees { get; set; } = 5.0;
+
+    #endregion
+
+    public DockToConfig Clone() => (DockToConfig)MemberwiseClone();
+}
+
+public class DockToController(DockToConfig dockConfig)
+{
+    public DockToConfig DockConfig { get; private set; } = dockConfig;
+    public List<NavigationNode> Waypoints_Value = [];
+    public NavigationNode Goal = null!;
+    private int _currentWaypointAheadIndex = 0;
+    public NavigationNode StartNode = null!;
+
+    public DockToController WithPath(NavigationNode startNode, NavigationNode currentGoal)
+    {
+        _currentWaypointAheadIndex = 0;
+        StartNode = startNode;
+        Goal = currentGoal;
+        Waypoints_Value = [..PathSplit(StartNode, Goal)];
+        return this;
+    }
+
+    public void ResetTracking()
+    {
+        _currentWaypointAheadIndex = 0;
+    }
+
+    public void UpdateGoal(NavigationNode currentGoal)
+    {
+        Goal = currentGoal;
+        Waypoints_Value = [.. PathSplit(StartNode, Goal)];
+    }
+
+    private NavigationNode[] PathSplit(NavigationNode startNode, NavigationNode currentGoal)
+    {
+        List<NavigationNode> navigationNode = [startNode];
+            var spaceEdge = new SpaceEdge()
+            {
+                StartX = startNode.X,
+                StartY = startNode.Y,
+                EndX = currentGoal.X,
+                EndY = currentGoal.Y,
+                Degree = 1,
+            };
+
+            double length = SpaceCompute.GetEdgeLength(spaceEdge, DockConfig.ResolutionSplit);
+            if (length <= 0) return [];
+            double step = DockConfig.ResolutionSplit / length;
+
+            for (double t = step; t <= 1 - step; t += step)
+            {
+                (double x, double y) = SpaceCompute.BezierPoint(t, spaceEdge);
+                navigationNode.Add(new()
+                {
+                    Id = Guid.NewGuid(),
+                    NodeId = string.Empty,
+                    X = x,
+                    Y = y,
+                    Theta = null,
+                    Direction = DockConfig.DockToDirection,
+                    Speed = startNode.Speed,
+                });
+            }
+            navigationNode.Add(currentGoal);
+        return [.. navigationNode];
+    }
+
+    /// <summary>
+    /// Calculate distance between two points
+    /// </summary>
+    private static double CalculateDistance(double x1, double y1, double x2, double y2)
+    {
+        double dx = x2 - x1;
+        double dy = y2 - y1;
+        return Math.Sqrt(dx * dx + dy * dy);
+    }
+
+    public (NavigationNode node, int index) GetClosestAheadWaypoint(double x, double y)
+    {
+        if (Waypoints_Value.Count == 0)
+            throw new InvalidOperationException("Path not set");
+
+        double minDistance = double.MaxValue;
+        int closestIndex = 0;
+
+        // Start search from current index for efficiency
+        for (int i = _currentWaypointAheadIndex; i < Waypoints_Value.Count; i++)
+        {
+            double distance = CalculateDistance(x, y, Waypoints_Value[i].X, Waypoints_Value[i].Y);
+
+            if (distance < minDistance)
+            {
+                minDistance = distance;
+                closestIndex = i;
+            }
+        }
+
+        // Also check previous waypoints in case robot moved backwards
+        for (int i = 0; i < _currentWaypointAheadIndex; i++)
+        {
+            double distance = CalculateDistance(x, y, Waypoints_Value[i].X, Waypoints_Value[i].Y);
+
+            if (distance < minDistance)
+            {
+                minDistance = distance;
+                closestIndex = i;
+            }
+        }
+
+        _currentWaypointAheadIndex = closestIndex;
+        return (Waypoints_Value[closestIndex], closestIndex);
+    }
+
+    /// <summary>
+    /// Calculate path heading at given waypoint index (for Stanley)
+    /// Uses current point and next point to determine direction
+    /// </summary>
+    private double CalculatePathHeading(int index)
+    {
+        if (index >= Waypoints_Value.Count - 1)
+        {
+            // Last point - use previous segment direction
+            if (index > 0)
+            {
+                double dx = Waypoints_Value[index].X - Waypoints_Value[index - 1].X;
+                double dy = Waypoints_Value[index].Y - Waypoints_Value[index - 1].Y;
+                return Math.Atan2(dy, dx);
+            }
+            return 0;
+        }
+
+        // Use current to next point
+        double dxNext = Waypoints_Value[index + 1].X - Waypoints_Value[index].X;
+        double dyNext = Waypoints_Value[index + 1].Y - Waypoints_Value[index].Y;
+        return Math.Atan2(dyNext, dxNext);
+    }
+
+    /// <summary>
+    /// Calculate signed cross-track error (for Stanley)
+    /// Positive: front axle is to the left of path
+    /// Negative: front axle is to the right of path
+    /// </summary>
+    private static double CalculateStanleyCrossTrackError(double frontX, double frontY, NavigationNode closestPoint, double pathHeading)
+    {
+        // Vector from closest point to front axle
+        double dx = frontX - closestPoint.X;
+        double dy = frontY - closestPoint.Y;
+
+        // Path direction vector
+        double pathDx = Math.Cos(pathHeading);
+        double pathDy = Math.Sin(pathHeading);
+
+        // Cross product to get signed perpendicular distance
+        // positive = left, negative = right
+        double crossTrackError = dx * pathDy - dy * pathDx;
+
+        return crossTrackError;
+    }
+
+    /// <summary>
+    /// Normalize angle to [-π, π]
+    /// </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;
+    }
+
+    /// <summary>
+    /// Stanley-based final approach controller
+    /// When robot enters goal region (IsApproachGoal), uses Stanley algorithm for precise CTE-based tracking
+    /// </summary>
+    public (double linearVel, double angularVel) FinalApproachController(
+        double robotX,
+        double robotY,
+        double robotTheta,
+        double actualLinearVelocity,
+        double maxLinearVelocity)
+    {
+        if (Goal is null || Waypoints_Value.Count == 0) return (0, 0);
+
+        if (DockConfig.DockToDirection == RobotDirection.BACKWARD) robotTheta += Math.PI;
+        robotTheta = NormalizeAngle(robotTheta);
+
+        // Calculate front axle position
+        double frontX = robotX + DockConfig.WheelBase * Math.Cos(robotTheta);
+        double frontY = robotY + DockConfig.WheelBase * Math.Sin(robotTheta);
+
+        // Find closest point on path to front axle
+        var (closestPoint, closestIndex) = GetClosestAheadWaypoint(frontX, frontY);
+
+        // Calculate heading at closest point (path direction)
+        double pathHeading = CalculatePathHeading(closestIndex);
+
+        // Calculate cross-track error (signed distance from front axle to path)
+        double crossTrackError = CalculateStanleyCrossTrackError(frontX, frontY, closestPoint, pathHeading);
+        if (DockConfig.DockToDirection == RobotDirection.BACKWARD) crossTrackError = -crossTrackError;
+
+        // Calculate heading error (path heading - robot heading)
+        double headingError = NormalizeAngle(pathHeading - robotTheta);
+
+        // Adaptive K gain: increase when close to goal for tighter tracking
+        double distanceToGoal = CalculateDistance(robotX, robotY, Goal.X, Goal.Y);
+        double adaptiveK = DockConfig.K;
+        if (distanceToGoal < DockConfig.GoalApproachDistance)
+        {
+            // Linearly increase K from K to K*GoalGainMultiplier as distance decreases
+            double approachRatio = 1.0 - (distanceToGoal / DockConfig.GoalApproachDistance);
+            adaptiveK = DockConfig.K * (1.0 + approachRatio * (DockConfig.GoalGainMultiplier - 1.0));
+        }
+
+        // Stanley formula: δ = ψ + arctan(K × e / (v + Ks))
+        double crossTrackTerm = Math.Atan2(adaptiveK * crossTrackError, Math.Abs(actualLinearVelocity) + DockConfig.Ks);
+
+        // Total steering angle
+        double steeringAngle = headingError + crossTrackTerm;
+
+        // Clamp to maximum steering angle
+        steeringAngle = Math.Clamp(steeringAngle, -DockConfig.MaxSteeringAngle, DockConfig.MaxSteeringAngle);
+
+        // Convert steering angle to angular velocity using bicycle model
+        double angularVelocity = (actualLinearVelocity * Math.Tan(steeringAngle)) / DockConfig.WheelBase;
+
+        // Clamp angular velocity for dock-to approach
+        angularVelocity = Math.Clamp(angularVelocity, -DockConfig.MaxAngularVelocity, DockConfig.MaxAngularVelocity);
+
+        // Apply direction to velocities
+        double linearVel = maxLinearVelocity;
+        if (DockConfig.DockToDirection == RobotDirection.BACKWARD)
+        {
+            linearVel = -linearVel;
+        }
+
+        // Debug output
+        Console.WriteLine($"DT-Stanley: Front=({frontX:F3},{frontY:F3}), Closest=({closestPoint.X:F3},{closestPoint.Y:F3}), " +
+                         $"Pose=({robotX:F3},{robotY:F3},{robotTheta * 180 / Math.PI:F1}°), " +
+                         $"CTE={crossTrackError:F3}m, HeadErr={headingError * 180 / Math.PI:F1}°, " +
+                         $"SteerAng={steeringAngle * 180 / Math.PI:F1}°, " +
+                         $"K={adaptiveK:F2}, DTG={distanceToGoal:F3}m, " +
+                         $"LVel={linearVel:F3}, AnVel={angularVelocity:F3}");
+
+        return (linearVel, angularVelocity);
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Navigation/CSharp/FuzzyLogic.cs

@@ -0,0 +1,333 @@
+namespace RobotNet10.RobotApp.Services.Navigation.CSharp;
+
+public class FuzzyLogic
+{
+    private double Gain_P = 0.5;
+    private double Gain_I = 0.01;
+    private double piIntegratorState;  // Trạng thái tích phân của PI controller
+
+    // Các tham số cho membership functions hình thang của tín hiệu góc
+    // Negative Large: [-∞, -∞, -1.0, -0.5]
+    private static readonly double[] NegativeLargeAngularParams = [-1.0E+10, -1.0E+10, -1.0, -0.5];
+    // Positive Large: [0.5, 1.0, +∞, +∞]
+    private static readonly double[] PositiveLargeAngularParams = [0.5, 1.0, 1.0E+10, 1.0E+10];
+
+    // Các tham số cho membership functions hình thang của vận tốc
+    // High Velocity: [0.75, 1.0, +∞, +∞]
+    private static readonly double[] HighVelocityParams = [0.75, 1.0, 1.0E+9, 1.0E+9];
+    // Low Velocity: [-∞, -∞, 0.0, 0.25]
+    private static readonly double[] LowVelocityParams = [-1.0E+9, -1.0E+9, 0.0, 0.25];
+
+    // Mảng quy tắc cho bộ điều khiển bánh phải (wr)
+    // 25 phần tử đầu: chỉ số membership function cho tín hiệu góc (input 1)
+    // 25 phần tử sau: chỉ số membership function cho vận tốc (input 2)
+    private static readonly byte[] RightWheelRuleInput1Indices = [ 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 4,
+                         4, 4, 4, 4, 5, 5, 5, 5, 5, 1, 2, 3, 4, 5, 1, 2, 3,
+                         4, 5, 1, 2, 3, 4, 5, 3, 4, 5, 1, 2, 1, 2, 3, 4, 5 ];
+    // Mảng quy tắc output cho bánh phải (25 quy tắc)
+    private static readonly byte[] RightWheelRuleOutputIndices = [1, 1, 2, 1, 1, 2, 3, 5, 1, 4, 5, 5, 5, 5, 5, 2, 1, 1, 1, 1, 5, 5, 5, 5, 5];
+
+    // Mảng quy tắc cho bộ điều khiển bánh trái (wl)
+    private static readonly byte[] LeftWheelRuleInput1Indices = [  1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 4,
+                            4, 4, 4, 4, 5, 5, 5, 5, 5, 1, 2, 3, 4, 5, 4, 1,
+                            2, 3, 5, 3, 1, 2, 4, 5, 1, 2, 3, 4, 5, 1, 2, 4, 5, 3 ];
+    private static readonly byte[] LeftWheelRuleOutputIndices = [5, 5, 5, 5, 5, 1, 2, 3, 5, 4, 2, 1, 1, 1, 1, 5, 5, 5, 5, 5, 1, 1, 1, 1, 2];
+
+    public void SetGainP(double gainP)
+    {
+        Gain_P = gainP;
+    }
+
+    public void SetGainI(double gainI)
+    {
+        Gain_I = gainI;
+    }
+
+    /// <summary>
+    /// Tính toán giá trị membership cho hàm hình thang (trapezoidal membership function).
+    /// </summary>
+    /// <param name="inputValue">Giá trị đầu vào cần tính độ thuộc</param>
+    /// <param name="parameters">Mảng 4 phần tử: [a, b, c, d] trong đó:
+    /// - a: điểm bắt đầu của cạnh tăng (left foot)
+    /// - b: điểm bắt đầu của phần phẳng (left shoulder)
+    /// - c: điểm kết thúc của phần phẳng (right shoulder)
+    /// - d: điểm kết thúc của cạnh giảm (right foot)</param>
+    /// <returns>Giá trị membership trong khoảng [0, 1]</returns>
+    private static double Fuzzy_trapmf(double inputValue, double[] parameters)
+    {
+        // Extract các tham số để dễ đọc
+        double leftFoot = parameters[0];      // a: điểm bắt đầu tăng
+        double leftShoulder = parameters[1];  // b: điểm bắt đầu phẳng
+        double rightShoulder = parameters[2]; // c: điểm kết thúc phẳng
+        double rightFoot = parameters[3];     // d: điểm kết thúc giảm
+
+        // Tính giá trị membership từ cạnh trái (từ a đến b)
+        double leftMembership = 0.0;
+        if (inputValue < leftFoot)
+        {
+            // Ngoài vùng hình thang bên trái
+            leftMembership = 0.0;
+        }
+        else if (inputValue >= leftShoulder)
+        {
+            // Trong vùng phẳng bên trái
+            leftMembership = 1.0;
+        }
+        else if (leftFoot != leftShoulder)
+        {
+            // Trên cạnh tăng (tính toán tuyến tính từ a đến b)
+            leftMembership = (inputValue - leftFoot) / (leftShoulder - leftFoot);
+        }
+
+        // Tính giá trị membership từ cạnh phải (từ c đến d)
+        double rightMembership = 0.0;
+        if (inputValue <= rightShoulder)
+        {
+            // Trong vùng phẳng bên phải
+            rightMembership = 1.0;
+        }
+        else if (inputValue > rightFoot)
+        {
+            // Ngoài vùng hình thang bên phải
+            rightMembership = 0.0;
+        }
+        else if (rightShoulder != rightFoot)
+        {
+            // Trên cạnh giảm (tính toán tuyến tính từ c đến d)
+            rightMembership = (rightFoot - inputValue) / (rightFoot - rightShoulder);
+        }
+
+        // Kết quả là giá trị nhỏ hơn để đảm bảo không vượt quá 1.0
+        return leftMembership < rightMembership ? leftMembership : rightMembership;
+    }
+
+    /// <summary>
+    /// Tính toán giá trị membership cho hàm tam giác (triangular membership function).
+    /// </summary>
+    /// <param name="inputValue">Giá trị đầu vào cần tính độ thuộc</param>
+    /// <param name="parameters">Mảng 3 phần tử: [a, b, c] trong đó:
+    /// - a: điểm bắt đầu (left foot)
+    /// - b: điểm đỉnh (peak) - giá trị membership = 1.0
+    /// - c: điểm kết thúc (right foot)</param>
+    /// <returns>Giá trị membership trong khoảng [0, 1]</returns>
+    private static double Fuzzy_trimf(double inputValue, double[] parameters)
+    {
+        // Extract các tham số để dễ đọc
+        double leftFoot = parameters[0];  // a: điểm bắt đầu
+        double peak = parameters[1];       // b: điểm đỉnh
+        double rightFoot = parameters[2];  // c: điểm kết thúc
+
+        // Kiểm tra nếu giá trị nằm ngoài vùng tam giác
+        if (inputValue < leftFoot || inputValue > rightFoot)
+        {
+            return 0.0;
+        }
+
+        // Nếu giá trị tại đỉnh, membership = 1.0
+        if (inputValue == peak)
+        {
+            return 1.0;
+        }
+
+        // Tính toán membership trên cạnh tăng (từ a đến b)
+        if (leftFoot < inputValue && inputValue < peak && leftFoot != peak)
+        {
+            return (inputValue - leftFoot) / (peak - leftFoot);
+        }
+
+        // Tính toán membership trên cạnh giảm (từ b đến c)
+        if (peak < inputValue && inputValue < rightFoot && peak != rightFoot)
+        {
+            return (rightFoot - inputValue) / (rightFoot - peak);
+        }
+
+        // Trường hợp đặc biệt: nếu không khớp với điều kiện nào
+        return 0.0;
+    }
+
+    /// <summary>
+    /// Tính toán vận tốc bánh trái và bánh phải dựa trên fuzzy logic controller.
+    /// </summary>
+    /// <param name="v">Vận tốc tuyến tính (linear velocity) - giá trị chuẩn hóa [0, 1]</param>
+    /// <param name="w">Vận tốc góc (angular velocity)</param>
+    /// <param name="timeSample">Thời gian mẫu (sampling time) cho tích phân - phải > 0</param>
+    /// <returns>Tuple chứa (wl: vận tốc bánh trái, wr: vận tốc bánh phải) - giá trị chuẩn hóa [0, 1]</returns>
+    /// <exception cref="ArgumentException">Thrown khi timeSample <= 0 hoặc các tham số không hợp lệ</exception>
+    public (double wl, double wr) Fuzzy_step(double v, double w, double timeSample)
+    {
+        // Validation đầu vào
+        if (timeSample <= 0.0 || double.IsNaN(timeSample) || double.IsInfinity(timeSample))
+        {
+            throw new ArgumentException("timeSample must be a positive finite number", nameof(timeSample));
+        }
+        if (double.IsNaN(v) || double.IsNaN(w) || double.IsInfinity(v) || double.IsInfinity(w))
+        {
+            throw new ArgumentException("Input parameters v and w must be finite numbers", nameof(v));
+        }
+        
+        (double wl, double wr) result = new();
+        
+        // Cache cho các giá trị membership của đầu vào (10 membership functions)
+        // [0-4]: membership functions cho tín hiệu góc đã xử lý
+        // [5-9]: membership functions cho vận tốc tuyến tính
+        double[] inputMembershipValues = new double[10];
+        
+        // Cache cho các giá trị membership của đầu ra (5 levels: 0.0, 0.25, 0.5, 0.75, 1.0)
+        double[] outputMembershipValuesRight = new double[5];  // Cho bánh phải (wr)
+        double[] outputMembershipValuesLeft = new double[5];  // Cho bánh trái (wl)
+        
+        // Mảng tạm để chứa tham số cho hàm tam giác (3 phần tử: [a, b, c])
+        double[] triangularParams = new double[3];
+        
+        // Các biến tạm để cache kết quả fuzzification (tránh tính toán lại)
+        double negativeLargeMembership;
+        double positiveLargeMembership;
+        double highVelocityMembership;
+        double lowVelocityMembership;
+        // ========== BƯỚC 1: Xử lý tín hiệu đầu vào bằng PI Controller ==========
+        // Tích phân vận tốc góc để loại bỏ sai số ổn định
+        piIntegratorState += Gain_I * w * timeSample;
+        // Kết hợp thành phần tỷ lệ và tích phân
+        double piControllerOutput = Gain_P * w + piIntegratorState;
+        
+        // ========== BƯỚC 2: Fuzzification - Chuyển đổi đầu vào thành độ thuộc ==========
+        // Tính toán membership values cho tín hiệu góc đã xử lý (5 membership functions)
+        
+        // MF1: Negative Large (hình thang)
+        negativeLargeMembership = Fuzzy_trapmf(piControllerOutput, NegativeLargeAngularParams);
+        inputMembershipValues[0] = negativeLargeMembership;
+        
+        // MF2: Negative (tam giác: -0.5, 0.0, 0.5)
+        triangularParams[0] = -0.5;
+        triangularParams[1] = 0.0;
+        triangularParams[2] = 0.5;
+        inputMembershipValues[1] = Fuzzy_trimf(piControllerOutput, triangularParams);
+        
+        // MF3: Positive Large (hình thang)
+        positiveLargeMembership = Fuzzy_trapmf(piControllerOutput, PositiveLargeAngularParams);
+        inputMembershipValues[2] = positiveLargeMembership;
+        
+        // MF4: Very Negative (tam giác: -1.0, -0.5, 0.0)
+        triangularParams[0] = -1.0;
+        triangularParams[1] = -0.5;
+        triangularParams[2] = 0.0;
+        inputMembershipValues[3] = Fuzzy_trimf(piControllerOutput, triangularParams);
+        
+        // MF5: Positive (tam giác: 0.0, 0.5, 1.0)
+        triangularParams[0] = 0.0;
+        triangularParams[1] = 0.5;
+        triangularParams[2] = 1.0;
+        inputMembershipValues[4] = Fuzzy_trimf(piControllerOutput, triangularParams);
+        
+        // Tính toán membership values cho vận tốc tuyến tính (5 membership functions)
+        
+        // MF6: Low (tam giác: 0.0, 0.25, 0.5)
+        triangularParams[0] = 0.0;
+        triangularParams[1] = 0.25;
+        triangularParams[2] = 0.5;
+        inputMembershipValues[5] = Fuzzy_trimf(v, triangularParams);
+        
+        // MF7: Medium (tam giác: 0.25, 0.5, 0.75)
+        triangularParams[0] = 0.25;
+        triangularParams[1] = 0.5;
+        triangularParams[2] = 0.75;
+        inputMembershipValues[6] = Fuzzy_trimf(v, triangularParams);
+        
+        // MF8: High (hình thang)
+        highVelocityMembership = Fuzzy_trapmf(v, HighVelocityParams);
+        inputMembershipValues[7] = highVelocityMembership;
+        
+        // MF9: Very Low (hình thang)
+        lowVelocityMembership = Fuzzy_trapmf(v, LowVelocityParams);
+        inputMembershipValues[8] = lowVelocityMembership;
+        
+        // MF10: High-Medium (tam giác: 0.5, 0.75, 1.0)
+        triangularParams[0] = 0.5;
+        triangularParams[1] = 0.75;
+        triangularParams[2] = 1.0;
+        inputMembershipValues[9] = Fuzzy_trimf(v, triangularParams);
+        // ========== BƯỚC 3: Tính toán vận tốc bánh phải (wr) ==========
+        // Khởi tạo giá trị membership cho đầu ra (5 mức: 0.0, 0.25, 0.5, 0.75, 1.0)
+        outputMembershipValuesRight[0] = 0.0;
+        outputMembershipValuesRight[1] = 0.25;
+        outputMembershipValuesRight[2] = 0.5;
+        outputMembershipValuesRight[3] = 0.75;
+        outputMembershipValuesRight[4] = 1.0;
+        
+        // Đánh giá 25 quy tắc fuzzy và tính toán defuzzification
+        double totalRuleActivation = 0.0;
+        double weightedOutputSum = 0.0;
+        const int numberOfRules = 25;
+        
+        for (int ruleIndex = 0; ruleIndex < numberOfRules; ruleIndex++)
+        {
+            // Tính độ kích hoạt của quy tắc: product(input1_membership, input2_membership)
+            // Sử dụng phép nhân (product) thay vì min() cho fuzzy AND operation
+            // input1: tín hiệu góc (index từ RightWheelRuleInput1Indices[ruleIndex] - 1, vì mảng bắt đầu từ 0)
+            // input2: vận tốc (index từ RightWheelRuleInput1Indices[ruleIndex + 25] + 4, offset 4 vì vận tốc bắt đầu từ index 5)
+            int angularSignalIndex = RightWheelRuleInput1Indices[ruleIndex] - 1;
+            int velocityIndex = RightWheelRuleInput1Indices[ruleIndex + numberOfRules] + 4;
+            double ruleActivation = inputMembershipValues[velocityIndex] * inputMembershipValues[angularSignalIndex];
+            
+            totalRuleActivation += ruleActivation;
+            
+            // Tính tổng có trọng số cho defuzzification (Center of Gravity)
+            int outputIndex = RightWheelRuleOutputIndices[ruleIndex] - 1;
+            weightedOutputSum += outputMembershipValuesRight[outputIndex] * ruleActivation;
+        }
+        
+        // Defuzzification: Center of Gravity method
+        if (totalRuleActivation == 0.0)
+        {
+            // Nếu không có quy tắc nào được kích hoạt, trả về giá trị mặc định
+            result.wr = 0.5;
+        }
+        else
+        {
+            result.wr = weightedOutputSum / totalRuleActivation;
+        }
+        // ========== BƯỚC 4: Tính toán vận tốc bánh trái (wl) ==========
+        // Sử dụng lại các giá trị membership đã tính ở BƯỚC 2 (không cần tính lại vì không thay đổi)
+        // Các giá trị trong inputMembershipValues[0-9] đã được tính toán và lưu trữ ở BƯỚC 2
+        
+        // Khởi tạo giá trị membership cho đầu ra bánh trái
+        outputMembershipValuesLeft[0] = 0.0;
+        outputMembershipValuesLeft[1] = 0.25;
+        outputMembershipValuesLeft[2] = 0.5;
+        outputMembershipValuesLeft[3] = 0.75;
+        outputMembershipValuesLeft[4] = 1.0;
+        
+        // Đánh giá 25 quy tắc fuzzy cho bánh trái và tính toán defuzzification
+        totalRuleActivation = 0.0;
+        weightedOutputSum = 0.0;
+        
+        for (int ruleIndex = 0; ruleIndex < numberOfRules; ruleIndex++)
+        {
+            // Tính độ kích hoạt của quy tắc cho bánh trái: product(input1_membership, input2_membership)
+            // Sử dụng phép nhân (product) thay vì min() cho fuzzy AND operation
+            // input1: tín hiệu góc (index từ LeftWheelRuleInput1Indices[ruleIndex] - 1)
+            // input2: vận tốc (index từ LeftWheelRuleInput1Indices[ruleIndex + 25] + 4)
+            int angularSignalIndex = LeftWheelRuleInput1Indices[ruleIndex] - 1;
+            int velocityIndex = LeftWheelRuleInput1Indices[ruleIndex + numberOfRules] + 4;
+            double ruleActivation = inputMembershipValues[velocityIndex] * inputMembershipValues[angularSignalIndex];
+            
+            totalRuleActivation += ruleActivation;
+            
+            // Tính tổng có trọng số cho defuzzification
+            int outputIndex = LeftWheelRuleOutputIndices[ruleIndex] - 1;
+            weightedOutputSum += outputMembershipValuesLeft[outputIndex] * ruleActivation;
+        }
+        
+        // Defuzzification: Center of Gravity method cho bánh trái
+        if (totalRuleActivation == 0.0)
+        {
+            // Nếu không có quy tắc nào được kích hoạt, trả về giá trị mặc định
+            result.wl = 0.5;
+        }
+        else
+        {
+            result.wl = weightedOutputSum / totalRuleActivation;
+        }
+        return result;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Navigation/CSharp/LocalPlannerConfig.cs

@@ -0,0 +1,72 @@
+namespace RobotNet10.RobotApp.Services.Navigation.CSharp;
+
+public enum ApproachResult
+{
+    /// <summary>
+    /// Approach waypoints đã sinh và prepend thành công
+    /// </summary>
+    ApproachGenerated,
+
+    /// <summary>
+    /// Robot đã trên path, không cần approach
+    /// </summary>
+    AlreadyOnPath,
+
+    /// <summary>
+    /// Robot quá xa path, fallback Rotate cũ
+    /// </summary>
+    TooFarFromPath,
+
+    /// <summary>
+    /// LocalPlanner tắt hoặc path quá ngắn
+    /// </summary>
+    Disabled
+}
+
+public class LocalPlannerConfig
+{
+    /// <summary>
+    /// Bật/tắt Local Planner
+    /// </summary>
+    public bool Enabled { get; set; } = true;
+
+    /// <summary>
+    /// Khoảng cách dưới mức này coi robot đã trên path, không cần approach (m)
+    /// </summary>
+    public double OnPathThreshold { get; set; } = 0.3;
+
+    /// <summary>
+    /// Khoảng cách trên mức này thì fallback Rotate cũ (m)
+    /// </summary>
+    public double MaxApproachDistance { get; set; } = 3.0;
+
+    /// <summary>
+    /// Hệ số tính merge distance: mergeDistance = MergeDistanceGain × distToPath
+    /// </summary>
+    public double MergeDistanceGain { get; set; } = 2.0;
+
+    /// <summary>
+    /// Merge distance tối thiểu (m)
+    /// </summary>
+    public double MergeDistanceMin { get; set; } = 0.3;
+
+    /// <summary>
+    /// Merge distance tối đa (m)
+    /// </summary>
+    public double MergeDistanceMax { get; set; } = 2.0;
+
+    /// <summary>
+    /// Tỷ lệ control arm phía robot (P1): d1 = ratio × dist(P0, P3)
+    /// </summary>
+    public double ControlArmRatioStart { get; set; } = 0.4;
+
+    /// <summary>
+    /// Tỷ lệ control arm phía path (P2): d2 = ratio × dist(P0, P3)
+    /// </summary>
+    public double ControlArmRatioEnd { get; set; } = 0.4;
+
+    /// <summary>
+    /// Bước sample approach curve (m), nên giống PurePursuitConfig.ResolutionSplit
+    /// </summary>
+    public double ResolutionSplit { get; set; } = 0.05;
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Navigation/CSharp/MotorDynamicsModel.cs

@@ -0,0 +1,109 @@
+namespace RobotNet10.RobotApp.Services.Navigation.CSharp;
+
+public class MotorDynamicsConfig
+{
+    /// <summary>
+    /// Time constant (τ) - thời gian để motor đạt 63.2% của target velocity
+    /// Đơn vị: giây (s)
+    /// Typical: 0.1 - 0.5s cho DC motor với driver PID
+    /// </summary>
+    public double Tau { get; set; }
+
+    /// <summary>
+    /// Pure delay (δ) - độ trễ trước khi motor bắt đầu phản ứng
+    /// Đơn vị: giây (s)
+    /// Bao gồm: communication delay + driver processing
+    /// Typical: 0.02 - 0.1s
+    /// </summary>
+    public double Delta { get; set; }
+}
+
+/// <summary>
+/// Mô hình động học của motor driver
+/// First-order system: v(t) = v_cmd × (1 - e^(-(t-δ)/τ))
+/// </summary>
+public class MotorDynamicsModel
+{
+    /// <summary>
+    /// Time constant (τ) - thời gian để motor đạt 63.2% của target velocity
+    /// Đơn vị: giây (s)3
+    /// Typical: 0.1 - 0.5s cho DC motor với driver PID
+    /// </summary>
+    public double Tau { get; set; }
+
+    /// <summary>
+    /// Pure delay (δ) - độ trễ trước khi motor bắt đầu phản ứng
+    /// Đơn vị: giây (s)
+    /// Bao gồm: communication delay + driver processing
+    /// Typical: 0.02 - 0.1s
+    /// </summary>
+    public double Delta { get; set; }
+
+    /// <summary>
+    /// Constructor với giá trị mặc định
+    /// </summary>
+    public MotorDynamicsModel()
+    {
+        Tau = 0.3;      // 300ms time constant
+        Delta = 0.05f;   // 50ms delay
+    }
+
+    public MotorDynamicsModel(MotorDynamicsConfig cog)
+    {
+        Tau = cog.Tau;
+        Delta = cog.Delta;
+    }
+
+    /// <summary>
+    /// Predict vận tốc tại thời điểm tương lai
+    /// </summary>
+    /// <param name="vCmd">Velocity command đã gửi</param>
+    /// <param name="vActual">Velocity thực tế hiện tại</param>
+    /// <param name="timeAhead">Thời gian dự đoán về tương lai (s)</param>
+    /// <returns>Vận tốc dự đoán</returns>
+    public double PredictVelocity(double vCmd, double vActual, double timeAhead)
+    {
+        // Nếu thời gian dự đoán < delay
+        // → Motor chưa bắt đầu phản ứng
+        if (timeAhead < Delta)
+        {
+            return vActual;
+        }
+
+        // Thời gian hiệu dụng (sau khi trừ delay)
+        double effectiveTime = timeAhead - Delta;
+
+        // First-order system response
+        // response = 1 - e^(-t/τ)
+        double response = 1.0 - Math.Exp(-effectiveTime / Tau);
+
+        // Velocity prediction
+        // v_future = v_actual + (v_cmd - v_actual) × response
+        double vPredicted = vActual + (vCmd - vActual) * response;
+
+        return vPredicted;
+    }
+
+    /// <summary>
+    /// Tính settling time (thời gian để đạt 95% target)
+    /// </summary>
+    public double GetSettlingTime()
+    {
+        // 95% response: t = -τ × ln(0.05) ≈ 3τ
+        return Delta + 3.0 * Tau;
+    }
+
+    /// <summary>
+    /// Tính rise time (thời gian để đạt từ 10% đến 90%)
+    /// </summary>
+    public double GetRiseTime()
+    {
+        // Rise time ≈ 2.2τ
+        return 2.2 * Tau;
+    }
+
+    public override string ToString()
+    {
+        return $"MotorModel(τ={Tau:F3}s, δ={Delta:F3}s, settling={GetSettlingTime():F3}s)";
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Navigation/CSharp/PID.cs

@@ -0,0 +1,80 @@
+namespace RobotNet10.RobotApp.Services.Navigation.CSharp;
+
+public class PIDConfig
+{
+    public double Kp { get; set; }
+    public double Ki { get; set; }
+    public double Kd { get; set; }
+    /// <summary>
+    /// Integral chỉ tích lũy khi |error| &lt;= IntegralZone.
+    /// Giá trị 0 = không giới hạn (integral luôn tích lũy).
+    /// </summary>
+    public double IntegralZone { get; set; }
+}
+
+public class PID(PIDConfig config)
+{
+    private double Kp = config.Kp;
+    private double Ki = config.Ki;
+    private double Kd = config.Kd;
+    private double IntegralZone = config.IntegralZone;
+
+    private double _prevError;
+    private double _integral;
+
+    public PID WithKp(double kp)
+    {
+        Kp = kp;
+        return this;
+    }
+
+    public PID WithKi(double ki)
+    {
+        Ki = ki;
+        return this;
+    }
+
+    public PID WithKd(double kd)
+    {
+        Kd = kd;
+        return this;
+    }
+
+    public PID WithIntegralZone(double integralZone)
+    {
+        IntegralZone = integralZone;
+        return this;
+    }
+
+    public double PID_step(double error, double max, double min, double timeSample)
+    {
+        double integralStep = 0.5 * (error + _prevError) * timeSample;
+
+        // Integral Zone: chỉ tích lũy khi |error| nằm trong vùng cho phép
+        bool inIntegralZone = IntegralZone <= 0 || Math.Abs(error) <= IntegralZone;
+        if (inIntegralZone)
+            _integral += integralStep;
+        else
+            _integral = 0;
+
+        double derivative = (error - _prevError) / timeSample;
+        _prevError = error;
+
+        double Out = Kp * error
+                   + Ki * _integral
+                   + Kd * derivative;
+
+        // Anti-windup: hoàn tác integralStep khi output bị bão hòa
+        double clamped = Math.Clamp(Out, min, max);
+        if (clamped != Out && inIntegralZone)
+            _integral -= integralStep;
+
+        return clamped;
+    }
+
+    public void Reset()
+    {
+        _prevError = 0;
+        _integral = 0;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Navigation/CSharp/PurePursuit.cs

@@ -0,0 +1,929 @@
+using RobotNet.VDA5050.Order;
+using RobotNet10.Common;
+using RobotNet10.Common.Models;
+using RobotNet10.NavigationTune.Shared.Models;
+using RobotNet10.RobotApp.Services.Exceptions;
+using RobotNet10.RobotApp.Services.Robot;
+using RobotNet10.RobotApp.Services.Robot.Helper;
+using RobotNet10.RobotApp.Services.Robot.Models;
+using RobotNet10.RobotApp.Services.Simulation;
+using RobotDirection = RobotNet10.RobotApp.Shared.Enums.RobotDirection;
+
+namespace RobotNet10.RobotApp.Services.Navigation.CSharp;
+
+/// <summary>
+/// Configuration cho Pure Pursuit controller
+/// </summary>
+public class PurePursuitConfig
+{
+    /// <summary>
+    /// Lookahead distance minimum (m)
+    /// </summary>
+    public double LookaheadMin { get; set; } = 0.3;
+
+    /// <summary>
+    /// Hệ số tỷ lệ lookahead với vận tốc (s)
+    /// </summary>
+    public double Kdd { get; set; } = 1.0;
+
+    /// <summary>
+    /// Lookahead distance maximum (m)
+    /// </summary>
+    public double LookaheadMax { get; set; } = 2.0;
+
+    /// <summary>
+    /// [LEGACY] Gain cho curvature (nếu cần scale steering)
+    /// Note: Not used in current implementation.
+    ///       Replaced by adaptive KCurvature for lookahead adjustment.
+    ///       Kept for backward compatibility.
+    /// </summary>
+    public double CurvatureGain { get; set; } = 1.0;
+
+    /// <summary>
+    /// Ngưỡng để coi như đạt waypoint (m)
+    /// </summary>
+    public double WaypointTolerance { get; set; } = 0.1;
+
+    /// <summary>
+    /// Maximum angular velocity during tracking (rad/s)
+    /// </summary>
+    public double MaxAngularVelocity { get; set; } = 1.5;
+
+    /// <summary>
+    /// Path waypoint spacing resolution (meters)
+    /// </summary>
+    public double ResolutionSplit { get; set; } = 0.05;
+
+    #region Adaptive Lookahead Parameters
+
+    /// <summary>
+    /// Goal region distance - start reducing lookahead when closer than this (m)
+    /// Default: 1.5m
+    /// </summary>
+    public double GoalRegionDistance { get; set; } = 1.5;
+
+    /// <summary>
+    /// Curvature adaptation factor (higher = more lookahead reduction on curves)
+    /// Default: 2.0
+    /// </summary>
+    public double KCurvature { get; set; } = 2.0;
+
+    /// <summary>
+    /// Minimum lookahead time ratio (seconds) - for dynamic min limit
+    /// Default: 0.3s
+    /// </summary>
+    public double MinLookaheadTimeRatio { get; set; } = 0.3;
+
+    /// <summary>
+    /// Maximum lookahead time ratio (seconds) - for dynamic max limit
+    /// Default: 2.0s
+    /// </summary>
+    public double MaxLookaheadTimeRatio { get; set; } = 2.0;
+
+    /// <summary>
+    /// Switch to Stanley controller when within this distance to goal (m)
+    /// Default: 0.5m
+    /// </summary>
+    public double FinalApproachThreshold { get; set; } = 1;
+
+    #endregion
+}
+
+public class PurePursuit(PurePursuitConfig PurePursuitConfig, StanleyConfig StanleyConfig)
+{
+    public OrderNode[] OrderNodes = [];
+    public OrderEdge[] OrderEdges = [];
+    public OrderNode? LastOrderNode = null;
+    public List<NavigationNode> Waypoints_Value = [];
+
+    private Dictionary<string, (int start, int end)>? _segmentCache;
+
+    private NavigationNode? Goal;
+
+    private int closesEdgeIndex = 0;
+    private int _currentWaypointAheadIndex = 0; // For Stanley controller
+    private bool _isApproachGoal = false;        // Flag for final approach mode
+
+    // Local Planner: approach curve stored as a virtual edge
+    private int _approachWaypointCount = 0;
+    private int _connectionEdgeIndex = 0;
+    private OrderEdge? _approachOrderEdge;   // Virtual edge for segment cache lookup
+    private SpaceEdge? _approachSpaceEdge;   // Geometry for projection in GetClosesEdges
+
+    public PurePursuit WithPath(Node[] nodes, Edge[] edges, double currentTheta)
+    {
+        if (nodes.Length < 2) throw new SimulationException(RobotErrors.Error1002(nodes.Length));
+        if (edges.Length < 1) throw new SimulationException();
+        if (edges.Length != nodes.Length - 1) throw new SimulationException(RobotErrors.Error1004(nodes.Length, edges.Length));
+        (OrderNodes, OrderEdges) = OrderConverter.Validate(nodes, edges, currentTheta);
+        Waypoints_Value = [.. PathSplit(OrderNodes, OrderEdges)];
+        closesEdgeIndex = 0;
+        _currentWaypointAheadIndex = 0;
+        _isApproachGoal = false;
+        _approachWaypointCount = 0;
+        _connectionEdgeIndex = 0;
+        _approachOrderEdge = null;
+        _approachSpaceEdge = null;
+        BuildSegmentCache();
+        return this;
+    }
+
+    public void ResetTracking()
+    {
+        closesEdgeIndex = 0;
+        _currentWaypointAheadIndex = 0;
+        _isApproachGoal = false;
+    }
+
+    /// <summary>
+    /// Sinh approach waypoints (cubic Bezier) từ vị trí robot đến path rồi prepend vào Waypoints_Value.
+    /// Approach curve CHỈ dựa vào vị trí (X,Y) robot và tangent tại connection point — KHÔNG dùng robotTheta.
+    /// Robot sẽ Rotate tại chỗ đến heading đầu curve trước khi Moving.
+    /// </summary>
+    public ApproachResult GenerateAndPrependApproachPath(
+        double robotX, double robotY, int closestIndex, LocalPlannerConfig config)
+    {
+        if (!config.Enabled || Waypoints_Value.Count < 2)
+            return ApproachResult.Disabled;
+
+        // Bước 1: Tính khoảng cách đến path
+        double distToPath = CalculateDistance(robotX, robotY,
+            Waypoints_Value[closestIndex].X, Waypoints_Value[closestIndex].Y);
+
+        if (distToPath < config.OnPathThreshold)
+            return ApproachResult.AlreadyOnPath;
+
+        if (distToPath > config.MaxApproachDistance)
+            return ApproachResult.TooFarFromPath;
+
+        // Bước 2: Tính mergeDistance (dynamic theo distToPath)
+        double mergeDistance = config.MergeDistanceGain * distToPath;
+        mergeDistance = Math.Clamp(mergeDistance, config.MergeDistanceMin, config.MergeDistanceMax);
+
+        // Bước 3: Tìm connection point trên path
+        int connectionIndex = closestIndex;
+        double accDist = 0;
+        while (accDist < mergeDistance && connectionIndex < Waypoints_Value.Count - 2)
+        {
+            double segLen = CalculateDistance(
+                Waypoints_Value[connectionIndex].X, Waypoints_Value[connectionIndex].Y,
+                Waypoints_Value[connectionIndex + 1].X, Waypoints_Value[connectionIndex + 1].Y);
+            accDist += segLen;
+            connectionIndex++;
+        }
+        var connectionPoint = Waypoints_Value[connectionIndex];
+
+        // Bước 4: Tính path tangent tại connection point
+        double pathTangent;
+        if (connectionIndex < Waypoints_Value.Count - 1)
+        {
+            double tx = Waypoints_Value[connectionIndex + 1].X - connectionPoint.X;
+            double ty = Waypoints_Value[connectionIndex + 1].Y - connectionPoint.Y;
+            pathTangent = Math.Atan2(ty, tx);
+        }
+        else if (connectionIndex > 0)
+        {
+            double tx = connectionPoint.X - Waypoints_Value[connectionIndex - 1].X;
+            double ty = connectionPoint.Y - Waypoints_Value[connectionIndex - 1].Y;
+            pathTangent = Math.Atan2(ty, tx);
+        }
+        else
+        {
+            return ApproachResult.Disabled;
+        }
+
+        // Bước 5: Tính 4 control points cho Cubic Bezier
+        // P0 = robot position, P3 = connection point
+        // P1 = dọc hướng P0→P3 (KHÔNG dùng robotTheta)
+        // P2 = tiếp cận theo pathTangent
+        double p0x = robotX, p0y = robotY;
+        double p3x = connectionPoint.X, p3y = connectionPoint.Y;
+
+        double dist = CalculateDistance(p0x, p0y, p3x, p3y);
+        if (dist < config.ResolutionSplit)
+            return ApproachResult.AlreadyOnPath;
+
+        // Direction P0→P3
+        double dirX = (p3x - p0x) / dist;
+        double dirY = (p3y - p0y) / dist;
+
+        double d1 = config.ControlArmRatioStart * dist;
+        double d2 = config.ControlArmRatioEnd * dist;
+
+        double p1x = p0x + d1 * dirX;
+        double p1y = p0y + d1 * dirY;
+        double p2x = p3x - d2 * Math.Cos(pathTangent);
+        double p2y = p3y - d2 * Math.Sin(pathTangent);
+
+        // Bước 6: Sample waypoints trên curve
+        var approachEdge = new SpaceEdge()
+        {
+            StartX = p0x, StartY = p0y,
+            EndX = p3x, EndY = p3y,
+            ControlPoint1X = p1x, ControlPoint1Y = p1y,
+            ControlPoint2X = p2x, ControlPoint2Y = p2y,
+            Degree = 3
+        };
+
+        double length = SpaceCompute.GetEdgeLength(approachEdge, config.ResolutionSplit);
+        if (length < config.ResolutionSplit)
+            return ApproachResult.AlreadyOnPath;
+
+        double step = config.ResolutionSplit / length;
+        var approachWaypoints = new List<NavigationNode>();
+
+        for (double t = 0; t < 1 - step; t += step)
+        {
+            (double x, double y) = SpaceCompute.BezierPoint(t, approachEdge);
+            approachWaypoints.Add(new NavigationNode
+            {
+                Id = Guid.NewGuid(),
+                NodeId = string.Empty,
+                X = x,
+                Y = y,
+                Direction = connectionPoint.Direction,
+                Speed = connectionPoint.Speed
+            });
+        }
+
+        if (approachWaypoints.Count == 0)
+            return ApproachResult.AlreadyOnPath;
+
+        // Bước 7a: Xác định edge chứa connectionIndex (trước khi modify Waypoints_Value)
+        int connectionEdgeIdx = 0;
+        if (_segmentCache is not null)
+        {
+            for (int i = 0; i < OrderEdges.Length; i++)
+            {
+                if (_segmentCache.TryGetValue(OrderEdges[i].EdgeId, out var range)
+                    && connectionIndex >= range.start && connectionIndex <= range.end)
+                {
+                    connectionEdgeIdx = i;
+                    break;
+                }
+            }
+        }
+
+        // Bước 7b: Lưu approach curve như 1 virtual edge (cho projection + segment cache)
+        _approachSpaceEdge = approachEdge;
+        _approachOrderEdge = new OrderEdge
+        {
+            EdgeId = "__approach__",
+            Degree = 3,
+            ControlPoint1X = p1x, ControlPoint1Y = p1y,
+            ControlPoint2X = p2x, ControlPoint2Y = p2y,
+            Direction = connectionPoint.Direction,
+            Speed = connectionPoint.Speed,
+        };
+
+        // Bước 7c: Insert approach waypoints ngay trước connectionIndex + xóa phần trước
+        _approachWaypointCount = approachWaypoints.Count;
+        Waypoints_Value.InsertRange(connectionIndex, approachWaypoints);
+        if (connectionIndex > 0)
+        {
+            Waypoints_Value.RemoveRange(0, connectionIndex);
+        }
+        // Kết quả: [approach_0, ..., approach_n, connectionPoint, ..., goal]
+        //           0                            _approachWaypointCount
+
+        _connectionEdgeIndex = connectionEdgeIdx;
+        closesEdgeIndex = connectionEdgeIdx;
+
+        // Bước 7d: Rebuild segment cache (approach edge + real edges từ connectionEdge trở đi)
+        BuildSegmentCache(_connectionEdgeIndex);
+
+        return ApproachResult.ApproachGenerated;
+    }
+
+    /// <summary>
+    /// Rebuild toàn bộ Waypoints_Value từ OrderNodes/OrderEdges đã lưu.
+    /// Dùng khi cần sinh lại approach từ vị trí mới (thay vì ClearApproachWaypoints).
+    /// Flow: RebuildPath() → check local planner → GenerateAndPrependApproachPath() nếu cần.
+    /// </summary>
+    public void RebuildPath()
+    {
+        string? goalId = Goal?.NodeId;
+
+        Waypoints_Value = [.. PathSplit(OrderNodes, OrderEdges)];
+        _approachWaypointCount = 0;
+        _connectionEdgeIndex = 0;
+        _approachOrderEdge = null;
+        _approachSpaceEdge = null;
+        BuildSegmentCache();
+
+        if (!string.IsNullOrEmpty(goalId))
+            UpdateGoal(goalId);
+    }
+
+    public void UpdateGoal(string goalId)
+    {
+        var goal = Waypoints_Value.FirstOrDefault(n => n.NodeId == goalId);
+        if (goal is not null) Goal = goal;
+    }
+
+    private NavigationNode[] PathSplit(OrderNode[] nodes, OrderEdge[] edges)
+    {
+        List<NavigationNode> navigationNode = [new()
+            {
+                Id = Guid.NewGuid(),
+                NodeId = nodes[0].NodeId,
+                X = nodes[0].X,
+                Y = nodes[0].Y,
+                Theta = nodes[0].Theta,
+                Direction = edges[0].Direction,
+                Speed = edges[0].Speed,
+            }];
+        foreach (var edge in edges)
+        {
+            var startNode = nodes.FirstOrDefault(n => n.NodeId == edge.StartNodeId);
+            var endNode = nodes.FirstOrDefault(n => n.NodeId == edge.EndNodeId);
+            if (startNode is null) throw new PathPlannerException(RobotErrors.Error1008(edge.EdgeId, edge.StartNodeId));
+            if (endNode is null) throw new PathPlannerException(RobotErrors.Error1009(edge.EdgeId, edge.EndNodeId));
+
+            var spaceEdge = new SpaceEdge()
+            {
+                StartX = startNode.X,
+                StartY = startNode.Y,
+                EndX = endNode.X,
+                EndY = endNode.Y,
+                ControlPoint1X = edge.ControlPoint1X ?? 0,
+                ControlPoint1Y = edge.ControlPoint1Y ?? 0,
+                ControlPoint2X = edge.ControlPoint2X ?? 0,
+                ControlPoint2Y = edge.ControlPoint2Y ?? 0,
+                Degree = edge.Degree,
+            };
+
+            double length = SpaceCompute.GetEdgeLength(spaceEdge, PurePursuitConfig.ResolutionSplit);
+            if (length <= 0) continue;
+            double step = PurePursuitConfig.ResolutionSplit / length;
+
+            for (double t = step; t <= 1 - step; t += step)
+            {
+                (double x, double y) = SpaceCompute.BezierPoint(t, spaceEdge);
+                navigationNode.Add(new()
+                {
+                    Id = Guid.NewGuid(),
+                    NodeId = string.Empty,
+                    X = x,
+                    Y = y,
+                    Theta = null,
+                    Direction = edge.Direction,
+                    Speed = edge.Speed,
+                });
+            }
+            navigationNode.Add(new()
+            {
+                Id = Guid.NewGuid(),
+                NodeId = endNode.NodeId,
+                X = endNode.X,
+                Y = endNode.Y,
+                Theta = endNode.Theta,
+                Direction = edge.Direction,
+                Speed = edge.Speed,
+            });
+        }
+        return [.. navigationNode];
+    }
+
+    /// <summary>
+    /// Build segment cache mapping EdgeId → (startWaypointIdx, endWaypointIdx).
+    /// Approach edge (nếu có) được thêm vào cache trước, sau đó build các real edges từ fromEdgeIndex.
+    /// </summary>
+    private void BuildSegmentCache(int fromEdgeIndex = 0)
+    {
+        _segmentCache = [];
+
+        // Thêm approach edge vào cache nếu có
+        if (_approachOrderEdge is not null && _approachWaypointCount > 0)
+        {
+            _segmentCache[_approachOrderEdge.EdgeId] = (0, _approachWaypointCount);
+        }
+
+        for (int i = fromEdgeIndex; i < OrderEdges.Length; i++)
+        {
+            var edge = OrderEdges[i];
+            int waypointStartIdx = Waypoints_Value.FindIndex(n => n.NodeId == edge.StartNodeId);
+            int waypointEndIdx = Waypoints_Value.FindIndex(n => n.NodeId == edge.EndNodeId);
+
+            // Edge đầu tiên: start node có thể đã bị xóa
+            // → lấy end của approach edge nếu có, hoặc 0
+            if (waypointStartIdx == -1)
+            {
+                if (i == fromEdgeIndex)
+                    waypointStartIdx = _approachOrderEdge is not null
+                        ? _segmentCache[_approachOrderEdge.EdgeId].end
+                        : 0;
+                else
+                    waypointStartIdx = _segmentCache[OrderEdges[i - 1].EdgeId].end;
+            }
+            if (waypointEndIdx == -1) waypointEndIdx = Waypoints_Value.Count - 1;
+
+            if (waypointStartIdx > waypointEndIdx) throw new NavigationException($"Waypoint has invalid range for edge {edge.EdgeId}: start={waypointStartIdx}, end={waypointEndIdx}");
+
+            _segmentCache[edge.EdgeId] = (waypointStartIdx, waypointEndIdx);
+        }
+    }
+
+    private (OrderEdge edge, double time) GetClosesEdges(double x, double y)
+    {
+        double minDistance = double.MaxValue;
+        OrderEdge? edgesResult = null;
+        double prjTime = 0;
+
+        // Project lên approach edge nếu robot chưa vượt qua connection edge
+        if (_approachSpaceEdge is not null && _approachOrderEdge is not null
+            && closesEdgeIndex <= _connectionEdgeIndex)
+        {
+            (_, _, var approachDist, double approachTime) = SpaceCompute.GetProjectionOnEdge(x, y, _approachSpaceEdge);
+            if (approachDist < minDistance)
+            {
+                minDistance = approachDist;
+                edgesResult = _approachOrderEdge;
+                prjTime = approachTime;
+            }
+        }
+
+        // Project lên các real edges (từ closesEdgeIndex)
+        for (int i = closesEdgeIndex; i < OrderEdges.Length; i++)
+        {
+            var startNode = OrderNodes.FirstOrDefault(node => node.NodeId == OrderEdges[i].StartNodeId);
+            var endNode = OrderNodes.FirstOrDefault(node => node.NodeId == OrderEdges[i].EndNodeId);
+            if (startNode is null || endNode is null) continue;
+
+            (_, _, var distance, double time) = SpaceCompute.GetProjectionOnEdge(x, y, new()
+            {
+                StartX = startNode.X,
+                StartY = startNode.Y,
+                EndX = endNode.X,
+                EndY = endNode.Y,
+                Degree = OrderEdges[i].Degree,
+                ControlPoint1X = OrderEdges[i].ControlPoint1X ?? 0,
+                ControlPoint1Y = OrderEdges[i].ControlPoint1Y ?? 0,
+                ControlPoint2X = OrderEdges[i].ControlPoint2X ?? 0,
+                ControlPoint2Y = OrderEdges[i].ControlPoint2Y ?? 0,
+            });
+            if (distance < minDistance)
+            {
+                minDistance = distance;
+                edgesResult = OrderEdges[i];
+                prjTime = time;
+                closesEdgeIndex = i;
+            }
+        }
+        return (edgesResult ?? OrderEdges[closesEdgeIndex], prjTime);
+    }
+
+    public (NavigationNode node, int index) OnNode(double x, double y)
+    {
+        // Edge-based projection thống nhất cho cả approach edge và real edges
+        (var closeEdge, double prjTime) = GetClosesEdges(x, y);
+        (var startNodeIdx, var endNodeIdx) = _segmentCache is not null && _segmentCache.TryGetValue(closeEdge.EdgeId, out var cached)
+            ? cached
+            : (0, Waypoints_Value.Count - 1);
+        int onNodeIndex = (int)(Math.Abs(endNodeIdx - startNodeIdx) * prjTime) + startNodeIdx;
+
+        return (Waypoints_Value[onNodeIndex], onNodeIndex);
+    }
+
+    /// <summary>
+    /// Calculate adaptive lookahead distance based on velocity, confidence, distance to goal, and path curvature
+    /// Lookahead adapts to:
+    /// 1. Velocity (faster = look further ahead)
+    /// 2. Distance to goal (near goal = shorter lookahead for precision)
+    /// 3. Path curvature (sharp curves = shorter lookahead for tighter tracking)
+    /// 4. Confidence (low confidence = shorter lookahead for safety)
+    /// 5. Velocity-based dynamic time limits
+    /// </summary>
+    private double GetLookaheadDistance(double vHybrid, double robotX, double robotY, int closestIndex)
+    {
+        // 1. Base lookahead from velocity
+        double baseLookahead = PurePursuitConfig.LookaheadMin + PurePursuitConfig.Kdd * Math.Abs(vHybrid);
+
+        // 2. Distance-to-goal adaptation
+        double distanceToGoal = CalculateDistanceToGoal(robotX, robotY);
+        double goalFactor = 1.0;
+        if (distanceToGoal < PurePursuitConfig.GoalRegionDistance)
+        {
+            // Gradually reduce lookahead as we approach goal
+            // At goal: factor = 0.5, At GoalRegionDistance: factor = 1.0
+            goalFactor = 0.5 + 0.5 * (distanceToGoal / PurePursuitConfig.GoalRegionDistance);
+        }
+
+        // 3. Curvature adaptation
+        double curvature = CalculateCurvature(closestIndex);
+        // curvatureFactor ranges from 1.0 (straight) to ~0.33 (very sharp curve with KCurvature=2.0)
+        double curvatureFactor = 1.0 / (1.0 + PurePursuitConfig.KCurvature * curvature);
+
+        // 5. Combine all factors
+        double adaptiveLookahead = baseLookahead * goalFactor * curvatureFactor;
+
+        double minLookahead = PurePursuitConfig.LookaheadMin;
+        double maxLookahead = PurePursuitConfig.LookaheadMax;
+
+        if (distanceToGoal < PurePursuitConfig.GoalRegionDistance && Math.Abs(vHybrid) > 0.0)
+        {
+            // 6. Apply velocity-based dynamic limits
+            // Minimum: look at least 0.3 seconds ahead or LookaheadMin (whichever is larger)
+            minLookahead = Math.Max(PurePursuitConfig.LookaheadMin, Math.Abs(vHybrid) * PurePursuitConfig.MinLookaheadTimeRatio);
+
+            // Maximum: look at most 2 seconds ahead or LookaheadMax (whichever is smaller)
+            maxLookahead = Math.Min(PurePursuitConfig.LookaheadMax, Math.Abs(vHybrid) * PurePursuitConfig.MaxLookaheadTimeRatio);
+
+            // Ensure min < max
+            if (minLookahead > maxLookahead)
+                minLookahead = maxLookahead;
+        }
+
+        adaptiveLookahead = Math.Clamp(adaptiveLookahead, minLookahead, maxLookahead);
+
+        if (double.IsNaN(adaptiveLookahead) || adaptiveLookahead <= 0)
+        {
+            adaptiveLookahead = PurePursuitConfig.LookaheadMin;
+        }
+
+        return adaptiveLookahead;
+    }
+
+    public (double linearVel, double angularVel) PurePursuit_step(double X_Ref,
+                                                                                            double Y_Ref,
+                                                                                            double Angle_Ref,
+                                                                                            double actualLinearVelocity,
+                                                                                            double maxLinearVelocity)
+    {
+        if (Waypoints_Value is null || Waypoints_Value.Count < 2)
+            throw new NavigationException("NAV PP Waypoint not yet set");
+
+        // 1. Get closest waypoint (KEEP ORIGINAL LOGIC)
+        var (onNode, index) = OnNode(X_Ref, Y_Ref);
+        if (onNode is null || Goal is null)
+            throw new NavigationException("NAV PP cannot get projection node");
+
+        // 2. Calculate adaptive lookahead distance (UPGRADED)
+        double lookaheadDistance = GetLookaheadDistance(actualLinearVelocity, X_Ref, Y_Ref, index);
+
+        // 3. Find target point with INTERPOLATION (UPGRADED)
+        NavigationNode? targetPoint = FindTargetPoint(index, lookaheadDistance);
+        targetPoint ??= Goal;
+
+        // 4. Apply speed limit from target point if available
+        double linearVel = maxLinearVelocity;
+        double? targetSpeed = targetPoint.Speed;
+
+        if (targetSpeed.HasValue && targetSpeed.Value > 0)
+        {
+            linearVel = Math.Min(maxLinearVelocity, targetSpeed.Value);
+        }
+
+        // 5. Check for final approach (PREPARATION FOR STANLEY)
+        double distanceToGoal = CalculateDistanceToGoal(X_Ref, Y_Ref);
+        if (targetPoint.Id == Goal.Id || distanceToGoal <= PurePursuitConfig.FinalApproachThreshold)
+        {
+            _isApproachGoal = true;
+        }
+
+        // 6. Normalize theta for backward (SIMPLIFIED)
+        bool isBackward = onNode.Direction == RobotDirection.BACKWARD;
+        if (isBackward) Angle_Ref += Math.PI;
+        Angle_Ref = NormalizeAngle(Angle_Ref);
+
+        // 7. Switch to Stanley when approaching goal (STANLEY INTEGRATION)
+        if (_isApproachGoal)
+        {
+            var (linear, angular) = FinalApproachController(X_Ref, Y_Ref, Angle_Ref, Goal, actualLinearVelocity, linearVel, isBackward);
+            return (linear, angular);
+        }
+
+        // 8. Calculate angle to target
+        var dx = targetPoint.X - X_Ref;
+        var dy = targetPoint.Y - Y_Ref;
+        var alpha = Math.Atan2(dy, dx) - Angle_Ref;
+
+        // Normalize alpha to [-π, π] (SIMPLIFIED)
+        alpha = NormalizeAngle(alpha);
+
+        // 9. Pure Pursuit formula: ω = 2 * v * sin(α) / L
+        var angularVelocity = 2.0 * Math.Abs(actualLinearVelocity) * Math.Sin(alpha) / lookaheadDistance;
+
+        // 10. Clamp to max angular velocity
+        if (Math.Abs(angularVelocity) > PurePursuitConfig.MaxAngularVelocity)
+        {
+            angularVelocity = Math.Sign(angularVelocity) * PurePursuitConfig.MaxAngularVelocity;
+        }
+
+        // 11. Apply direction sign to velocities
+        if (isBackward)
+        {
+            linearVel = -linearVel;
+        }
+
+        Console.WriteLine($"PP: Target=({targetPoint.X:F3},{targetPoint.Y:F3}), " +
+                         $"Pose=({X_Ref:F3},{Y_Ref:F3},{Angle_Ref * 180 / Math.PI:F2}°), " +
+                         $"Look={lookaheadDistance:F3}, Alpha={alpha * 180 / Math.PI:F2}°, " +
+                         $"LVel={linearVel:F3}, AngVel={angularVelocity:F3}");
+
+        return (linearVel, angularVelocity);
+    }
+
+    /// <summary>
+    /// Stanley-based final approach controller
+    /// When robot enters goal region (IsApproachGoal), uses Stanley algorithm for precise CTE-based tracking
+    /// </summary>
+    private (double linearVel, double angularVel) FinalApproachController(
+        double robotX,
+        double robotY,
+        double robotTheta,
+        NavigationNode goal,
+        double actualLinearVelocity,
+        double maxLinearVelocity,
+        bool isBackward)
+    {
+        // Calculate front axle position
+        double frontX = robotX + StanleyConfig.WheelBase * Math.Cos(robotTheta);
+        double frontY = robotY + StanleyConfig.WheelBase * Math.Sin(robotTheta);
+
+        // Find closest point on path to front axle
+        var (closestPoint, closestIndex) = GetClosestAheadWaypoint(frontX, frontY);
+
+        // Calculate heading at closest point (path direction)
+        double pathHeading = CalculatePathHeading(closestIndex);
+
+        // Calculate cross-track error (signed distance from front axle to path)
+        double crossTrackError = CalculateStanleyCrossTrackError(frontX, frontY, closestPoint, pathHeading);
+        if (isBackward) crossTrackError = -crossTrackError;
+
+        // Calculate heading error (path heading - robot heading)
+        double headingError = NormalizeAngle(pathHeading - robotTheta);
+
+        // Calculate curvature at closest point (for feedforward)
+        double curvature = 0;
+        if (StanleyConfig.EnableCurvatureFeedforward)
+        {
+            curvature = CalculateCurvature(closestIndex);
+        }
+
+        // Adaptive K gain: increase when close to goal for tighter tracking
+        double distanceToGoal = CalculateDistance(robotX, robotY, goal.X, goal.Y);
+        double adaptiveK = StanleyConfig.K;
+        if (distanceToGoal < StanleyConfig.GoalApproachDistance)
+        {
+            // Linearly increase K from K to K*GoalGainMultiplier as distance decreases
+            double approachRatio = 1.0 - (distanceToGoal / StanleyConfig.GoalApproachDistance);
+            adaptiveK = StanleyConfig.K * (1.0 + approachRatio * (StanleyConfig.GoalGainMultiplier - 1.0));
+        }
+
+        // Stanley formula: δ = ψ + arctan(K × e / (v + Ks))
+        double crossTrackTerm = Math.Atan2(adaptiveK * crossTrackError, Math.Abs(actualLinearVelocity) + StanleyConfig.Ks);
+
+        // Add curvature feedforward if enabled
+        double curvatureTerm = 0;
+        if (StanleyConfig.EnableCurvatureFeedforward && curvature != 0)
+        {
+            curvatureTerm = StanleyConfig.KCurvatureFF * Math.Atan(curvature * StanleyConfig.WheelBase);
+        }
+
+        // Total steering angle
+        double steeringAngle = headingError + crossTrackTerm + curvatureTerm;
+
+        // Clamp to maximum steering angle
+        steeringAngle = Math.Clamp(steeringAngle, -StanleyConfig.MaxSteeringAngle, StanleyConfig.MaxSteeringAngle);
+
+        // Convert steering angle to angular velocity using bicycle model
+        double angularVelocity = (actualLinearVelocity * Math.Tan(steeringAngle)) / StanleyConfig.WheelBase;
+
+        // Clamp angular velocity for final approach
+        angularVelocity = Math.Clamp(angularVelocity, -StanleyConfig.MaxAngularVelocity, StanleyConfig.MaxAngularVelocity);
+
+        // Apply direction to velocities
+        double linearVel = maxLinearVelocity;
+        if (isBackward)
+        {
+            linearVel = -linearVel;
+        }
+
+        // Debug output
+        Console.WriteLine($"FA-Stanley: Front=({frontX:F3},{frontY:F3}), Closest=({closestPoint.X:F3},{closestPoint.Y:F3}), " +
+                         $"Pose=({robotX:F3},{robotY:F3},{robotTheta * 180 / Math.PI:F1}°), " +
+                         $"CTE={crossTrackError:F3}m, HeadErr={headingError * 180 / Math.PI:F1}°, " +
+                         $"Curv={curvature:F3}, SteerAng={steeringAngle * 180 / Math.PI:F1}°, " +
+                         $"K={adaptiveK:F2}, DTG={distanceToGoal:F3}m, " +
+                         $"LVel={linearVel:F3}, AnVel={angularVelocity:F3}");
+
+        return (linearVel, angularVelocity);
+    }
+
+    /// <summary>
+    /// Find target point at lookahead distance from current index with interpolation
+    /// This provides smooth target point selection instead of discrete waypoints
+    /// Speed information is interpolated to provide accurate future speed limit
+    /// </summary>
+    private NavigationNode? FindTargetPoint(int startIndex, double lookaheadDistance)
+    {
+        if (startIndex >= Waypoints_Value.Count - 1)
+            return Goal;
+
+        double accumulatedDistance = 0;
+
+        for (int i = startIndex; i < Waypoints_Value.Count - 1; i++)
+        {
+            double dx = Waypoints_Value[i + 1].X - Waypoints_Value[i].X;
+            double dy = Waypoints_Value[i + 1].Y - Waypoints_Value[i].Y;
+            double segmentLength = Math.Sqrt(dx * dx + dy * dy);
+
+            if (accumulatedDistance + segmentLength >= lookaheadDistance)
+            {
+                // Interpolate within this segment
+                double t = (lookaheadDistance - accumulatedDistance) / segmentLength;
+
+                // Interpolate speed if both waypoints have speed info
+                double? interpolatedSpeed;
+                if (Waypoints_Value[i].Speed is { } speed1 && Waypoints_Value[i + 1].Speed is { } speed2)
+                {
+                    // Linear interpolation of speed limit
+                    interpolatedSpeed = speed1 + t * (speed2 - speed1);
+                }
+                else
+                {
+                    // Use next waypoint's speed if available (upcoming constraint)
+                    interpolatedSpeed = Waypoints_Value[i + 1].Speed ?? Waypoints_Value[i].Speed;
+                }
+
+                return new NavigationNode
+                {
+                    Id = Guid.NewGuid(),
+                    NodeId = string.Empty,
+                    X = Waypoints_Value[i].X + t * dx,
+                    Y = Waypoints_Value[i].Y + t * dy,
+                    Theta = null,
+                    Direction = Waypoints_Value[i].Direction,
+                    Speed = interpolatedSpeed  // Preserve speed limit for lookahead
+                };
+            }
+
+            accumulatedDistance += segmentLength;
+        }
+
+        return Goal;
+    }
+
+    #region Helper Methods
+
+    /// <summary>
+    /// Calculate distance between two points
+    /// </summary>
+    private static double CalculateDistance(double x1, double y1, double x2, double y2)
+    {
+        double dx = x2 - x1;
+        double dy = y2 - y1;
+        return Math.Sqrt(dx * dx + dy * dy);
+    }
+
+    /// <summary>
+    /// Normalize angle to [-π, π]
+    /// </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;
+    }
+
+    /// <summary>
+    /// Calculate distance from robot to goal point
+    /// </summary>
+    private double CalculateDistanceToGoal(double robotX, double robotY)
+    {
+        if (Goal == null)
+            return double.MaxValue;
+
+        double dx = Goal.X - robotX;
+        double dy = Goal.Y - robotY;
+        return Math.Sqrt(dx * dx + dy * dy);
+    }
+
+    /// <summary>
+    /// Calculate path curvature at given waypoint index using 3-point circle fitting (Menger curvature)
+    /// Returns curvature in 1/meters (larger value = sharper curve)
+    /// </summary>
+    private double CalculateCurvature(int index)
+    {
+        // Need at least 3 points for curvature calculation
+        if (Waypoints_Value.Count < 3 || index <= 0 || index >= Waypoints_Value.Count - 1)
+            return 0.0;
+
+        var p1 = Waypoints_Value[index - 1];
+        var p2 = Waypoints_Value[index];
+        var p3 = Waypoints_Value[index + 1];
+
+        // Calculate vectors
+        double dx1 = p2.X - p1.X;
+        double dy1 = p2.Y - p1.Y;
+        double dx2 = p3.X - p2.X;
+        double dy2 = p3.Y - p2.Y;
+
+        // Cross product magnitude (2 * triangle area)
+        double cross = Math.Abs(dx1 * dy2 - dy1 * dx2);
+
+        // Side lengths of triangle
+        double a = Math.Sqrt(dx1 * dx1 + dy1 * dy1);
+        double b = Math.Sqrt(dx2 * dx2 + dy2 * dy2);
+        double c = Math.Sqrt((p3.X - p1.X) * (p3.X - p1.X) + (p3.Y - p1.Y) * (p3.Y - p1.Y));
+
+        // Menger curvature formula: k = 4 * Area / (a * b * c)
+        // Area of triangle = cross / 2, so k = 2 * cross / (a * b * c)
+        double curvature = 2.0 * cross / (a * b * c + 1e-9);  // Add small epsilon to avoid division by zero
+
+        return curvature;
+    }
+
+    #endregion
+
+    #region Stanley Helper Methods (for FinalApproachController)
+
+    /// <summary>
+    /// Get closest waypoint ahead to given position (for Stanley front axle tracking)
+    /// </summary>
+    private (NavigationNode point, int index) GetClosestAheadWaypoint(double x, double y)
+    {
+        if (Waypoints_Value.Count == 0)
+            throw new InvalidOperationException("Path not set");
+
+        double minDistance = double.MaxValue;
+        int closestIndex = 0;
+
+        // Start search from current index for efficiency
+        for (int i = _currentWaypointAheadIndex; i < Waypoints_Value.Count; i++)
+        {
+            double distance = CalculateDistance(x, y, Waypoints_Value[i].X, Waypoints_Value[i].Y);
+
+            if (distance < minDistance)
+            {
+                minDistance = distance;
+                closestIndex = i;
+            }
+        }
+
+        // Also check previous waypoints in case robot moved backwards
+        for (int i = 0; i < _currentWaypointAheadIndex; i++)
+        {
+            double distance = CalculateDistance(x, y, Waypoints_Value[i].X, Waypoints_Value[i].Y);
+
+            if (distance < minDistance)
+            {
+                minDistance = distance;
+                closestIndex = i;
+            }
+        }
+
+        _currentWaypointAheadIndex = closestIndex;
+        return (Waypoints_Value[closestIndex], closestIndex);
+    }
+
+    /// <summary>
+    /// Calculate path heading at given waypoint index (for Stanley)
+    /// Uses current point and next point to determine direction
+    /// </summary>
+    private double CalculatePathHeading(int index)
+    {
+        if (index >= Waypoints_Value.Count - 1)
+        {
+            // Last point - use previous segment direction
+            if (index > 0)
+            {
+                double dx = Waypoints_Value[index].X - Waypoints_Value[index - 1].X;
+                double dy = Waypoints_Value[index].Y - Waypoints_Value[index - 1].Y;
+                return Math.Atan2(dy, dx);
+            }
+            return 0;
+        }
+
+        // Use current to next point
+        double dxNext = Waypoints_Value[index + 1].X - Waypoints_Value[index].X;
+        double dyNext = Waypoints_Value[index + 1].Y - Waypoints_Value[index].Y;
+        return Math.Atan2(dyNext, dxNext);
+    }
+
+    /// <summary>
+    /// Calculate signed cross-track error (for Stanley)
+    /// Positive: front axle is to the left of path
+    /// Negative: front axle is to the right of path
+    /// </summary>
+    private static double CalculateStanleyCrossTrackError(double frontX, double frontY, NavigationNode closestPoint, double pathHeading)
+    {
+        // Vector from closest point to front axle
+        double dx = frontX - closestPoint.X;
+        double dy = frontY - closestPoint.Y;
+
+        // Path direction vector
+        double pathDx = Math.Cos(pathHeading);
+        double pathDy = Math.Sin(pathHeading);
+
+        // Cross product to get signed perpendicular distance
+        // positive = left, negative = right
+        double crossTrackError = dx * pathDy - dy * pathDx;
+
+        return crossTrackError;
+    }
+
+    #endregion
+
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Navigation/CSharp/VelocityController.cs

@@ -0,0 +1,457 @@
+using RobotNet10.CANOpen.CiA402.Enums;
+using RobotNet10.RobotApp.Motion;
+using RobotNet10.RobotApp.Services.ConfigManager;
+
+namespace RobotNet10.RobotApp.Services.Navigation.CSharp;
+
+
+
+/// <summary>
+/// Configuration cho signal processing
+/// </summary>
+public class VelocitySignalProcessingConfig
+{
+    /// <summary>
+    /// Hệ số lọc cho encoder velocity
+    /// Giá trị nhỏ (0.1-0.2): Smooth nhưng lag
+    /// Giá trị lớn (0.3-0.4): Responsive nhưng nhiễu
+    /// </summary>
+    public double AlphaFilter { get; set; } = 0.3;
+
+    /// <summary>
+    /// Ngưỡng phát hiện encoder nhiễu (m/s)
+    /// Nếu thay đổi vận tốc > threshold trong 1 cycle → có thể nhiễu
+    /// </summary>
+    public double NoiseThreshold { get; set; } = 0.5;
+}
+
+/// <summary>
+/// Configuration cho velocity estimator
+/// </summary>
+public class VelocityEstimatorConfig
+{
+    // Blend ratio limits
+    public double MinBlendRatio { get; set; } = 0.15f;
+    public double MaxBlendRatio { get; set; } = 0.8f;
+    public double DefaultBlendRatio { get; set; } = 0.6;
+
+    // Adaptive blending thresholds
+    public double GoodTrackingThreshold { get; set; } = 0.12f;   // < 10% error
+    public double ModerateTrackingThreshold { get; set; } = 0.3; // < 30% error
+
+    // Blend ratios for different tracking qualities
+    public double GoodTrackingBlend { get; set; } = 0.7;
+    public double ModerateTrackingBlend { get; set; } = 0.5;
+    public double PoorTrackingBlend { get; set; } = 0.25f;
+
+    // Model confidence decay
+    public double ConfidenceDecayRate { get; set; } = 0.95f;
+    public double MinConfidence { get; set; } = 0.3;
+}
+
+public class VelocityController(IInverseKinematics InverseKinematic,
+                                OdometryService odometryService,
+                                IRobotConfiguration RobtoConfiguration,
+                                INavigationConfig NavigationConfig,
+                                ILogger<VelocityController> _logger) : IVelocityController
+{
+    public (double Linear, double Angular) ActualVelocity => GetCurrentVel();
+    public (double Linear, double Angular) RawVelocity => GetRawCurrentVel();
+
+    // vận tốc tính toán m/s và rad/s đối với vận tốc góc
+    private double _rightVelCmd = 0;
+    private double _leftVelCmd = 0;
+    private double _oldRightVel = 0;
+    private double _oldLeftVel = 0;
+
+    private MotorDynamicsConfig _motorDynamicsConifg = new();
+    private MotorDynamicsModel _motorDynamicsModel = new();
+    private VelocityEstimatorConfig _estimatorConfig = new();
+    public PurePursuitConfig _purePursuitConfig = new();
+    private VelocitySignalProcessingConfig _signalConfig = new();
+    private readonly CircularBuffer<double> _predictionErrors = new(20);
+
+    private double _currentConfidence = 1.0;
+    private readonly double wheelBase = RobtoConfiguration.GetRobotPhysicalConfig().WheelBase;
+    private int _ensureIKReadyCounter = 0; // Counter for logging throttling
+
+    public void SetVelocity(double linearVel, double angularVel)
+    {
+        // InverseKinematic.SetVelocity not available - commented out
+        // InverseKinematic.SetVelocity(new()
+        // {
+        //     Linear = new(){
+        //         X = linearVel,
+        //         Y = 0,
+        //     },
+        //     Angular = new(){
+        //         Z = angularVel,
+        //     },
+        // });
+        _leftVelCmd = linearVel - (wheelBase / 2) * angularVel;
+        _rightVelCmd = linearVel + (wheelBase / 2) * angularVel;
+    }
+    public (double linearVel, double angularVel) GetRawCurrentVel()
+    {
+        try
+        {
+            var odom = odometryService.CurrentOdometry;
+            double vActual = odom.Twist.Twist.Linear.X;
+            double omegaActual = odom.Twist.Twist.Angular.Z;
+            return (vActual, omegaActual);
+        }
+        catch { return (0, 0); }
+    }
+
+    public (double linearVel, double angularVel) GetCurrentVel()
+    {
+        try
+        {
+            var odom = odometryService.CurrentOdometry;
+            var vActual = odom.Twist.Twist.Linear.X;
+            var omegaActual = odom.Twist.Twist.Angular.Z;
+
+            // Convert linear/angular back to left/right wheel velocities for the estimator
+            _oldLeftVel = vActual - (wheelBase / 2) * omegaActual;
+            _oldRightVel = vActual + (wheelBase / 2) * omegaActual;
+
+            return Estimate(_oldLeftVel, _oldRightVel, _leftVelCmd, _rightVelCmd, wheelBase);
+        }
+        catch { return (0, 0); }
+    }
+
+    /// <summary>
+    /// Exponential Moving Average (EMA) Low-Pass Filter
+    /// </summary>
+    /// <param name="newValue">Giá trị mới từ sensor</param>
+    /// <param name="oldValue">Giá trị đã lọc trước đó</param>
+    /// <param name="alpha">Hệ số lọc (0-1). Càng nhỏ càng smooth, càng lớn càng responsive</param>
+    /// <returns>Giá trị sau khi lọc</returns>
+    private static double LowPassFilter(double newValue, double oldValue, double alpha)
+    {
+        // Validate alpha
+        if (alpha < 0 || alpha > 1)
+        {
+            throw new ArgumentException("Alpha must be between 0 and 1", nameof(alpha));
+        }
+
+        return alpha * newValue + (1.0 - alpha) * oldValue;
+    }
+
+    /// <summary>
+    /// MAIN FUNCTION: Estimate velocity
+    /// </summary>
+    private (double linearVel, double angularVel) Estimate(
+        double vLeftActual,      // Từ encoder (filtered)
+        double vRightActual,     // Từ encoder (filtered)
+        double vLeftCmdPrev,     // Command từ cycle trước
+        double vRightCmdPrev,    // Command từ cycle trước
+        double wheelbase)
+    {
+        // 1. Tính vận tốc actual (linear & angular)
+        double vActual = (vLeftActual + vRightActual) / 2.0;
+        double omegaActual = (vRightActual - vLeftActual) / wheelbase;
+
+        // 2. Tính vận tốc command từ cycle trước
+        double vCmdPrev = (vLeftCmdPrev + vRightCmdPrev) / 2.0;
+        double omegaCmdPrev = (vRightCmdPrev - vLeftCmdPrev) / wheelbase;
+
+        // 3. Tính prediction horizon
+        double predictionHorizon = CalculatePredictionHorizon(vActual);
+
+        // 4. Predict velocity cho từng bánh
+        double vLeftPredicted = _motorDynamicsModel.PredictVelocity(
+            vLeftCmdPrev,
+            vLeftActual,
+            predictionHorizon
+        );
+
+        double vRightPredicted = _motorDynamicsModel.PredictVelocity(
+            vRightCmdPrev,
+            vRightActual,
+            predictionHorizon
+        );
+
+        // 5. Tính linear & angular predicted
+        double vPredicted = (vLeftPredicted + vRightPredicted) / 2.0;
+        double omegaPredicted = (vRightPredicted - vLeftPredicted) / wheelbase;
+
+        // 6. Update model confidence
+        UpdateModelConfidence(vPredicted, vActual);
+
+        // 7. Tính tracking error
+        double linearErr = CalculateLinearTrackingError(vCmdPrev, vActual);
+        double angularErr = CalculateAngularTrackingError(omegaCmdPrev, omegaActual);
+        double combinedTrackingError = 0.65f * linearErr + 0.35f * angularErr;
+
+        // 8. Calculate adaptive blend ratio
+        double blendRatio = CalculateAdaptiveBlendRatio(
+            combinedTrackingError,
+            _currentConfidence
+        );
+
+        // 9. Blend predicted và actual
+        var vHybrid = (blendRatio * vPredicted) + ((1.0 - blendRatio) * vActual);
+        double omegaHybrid = blendRatio * omegaPredicted + (1.0 - blendRatio) * omegaActual;
+
+        // 10. Return result
+        return (vHybrid, omegaHybrid);
+    }
+
+    /// <summary>
+    /// Tính prediction horizon dựa vào lookahead distance
+    /// </summary>
+    private double CalculatePredictionHorizon(double vActual)
+    {
+        // Lookahead distance
+        double lookahead = _purePursuitConfig.LookaheadMin + _purePursuitConfig.Kdd * Math.Abs(vActual);
+
+        lookahead = Math.Clamp(lookahead, _purePursuitConfig.LookaheadMin, _purePursuitConfig.LookaheadMax);
+
+        // Prediction time = lookahead / velocity
+        // Nếu vận tốc quá nhỏ, dùng một giá trị minimum
+        double predictionTime = lookahead / Math.Max(Math.Abs(vActual), 0.1);
+
+        // Giới hạn prediction time (không nên quá xa)
+        predictionTime = Math.Clamp(predictionTime, 0.1, 2.0);
+
+        return predictionTime;
+    }
+
+    /// <summary>
+    /// Tính linear velocity tracking error (normalized)
+    /// </summary>
+    private static double CalculateLinearTrackingError(double vCmd, double vActual)
+    {
+        double error = Math.Abs(vCmd - vActual);
+        double normalizedError = error / Math.Max(Math.Abs(vCmd), 0.1);
+        return normalizedError;
+    }
+
+    /// <summary>
+    /// Tính angular velocity tracking error (normalized)
+    /// </summary>
+    private static double CalculateAngularTrackingError(double oCmd, double oActual)
+    {
+        double error = Math.Abs(oCmd - oActual);
+        double normalizedError = error / Math.Max(Math.Abs(oCmd), 0.05f);
+        return normalizedError;
+    }
+
+    /// <summary>
+    /// Update model confidence dựa trên prediction accuracy
+    /// </summary>
+    private void UpdateModelConfidence(double vPredictedPrev, double vActualNow)
+    {
+        // Prediction error từ cycle trước
+        double predError = Math.Abs(vPredictedPrev - vActualNow) / Math.Max(Math.Abs(vActualNow), 0.1);
+
+        _predictionErrors.Add(predError);
+
+        // Tính confidence dựa trên average error
+        if (_predictionErrors.Count > 0)
+        {
+            double avgError = _predictionErrors.Average();
+
+            // Confidence = 1 - avgError (capped)
+            double newConfidence = Math.Clamp(1.0 - avgError, 0.0, 1.0);
+
+            // Smooth update với decay
+            _currentConfidence = _estimatorConfig.ConfidenceDecayRate * _currentConfidence + (1.0 - _estimatorConfig.ConfidenceDecayRate) * newConfidence;
+
+            _currentConfidence = Math.Max(_currentConfidence, _estimatorConfig.MinConfidence);
+        }
+    }
+
+    /// <summary>
+    /// Calculate adaptive blend ratio
+    /// </summary>
+    private double CalculateAdaptiveBlendRatio(
+        double trackingError,
+        double modelConfidence)
+    {
+        double alpha;
+
+        // Factor 1: Tracking error
+        if (trackingError < _estimatorConfig.GoodTrackingThreshold)
+        {
+            // Motor tracking tốt → tin prediction nhiều
+            alpha = _estimatorConfig.GoodTrackingBlend;
+        }
+        else if (trackingError < _estimatorConfig.ModerateTrackingThreshold)
+        {
+            // Moderate error → balanced
+            alpha = _estimatorConfig.ModerateTrackingBlend;
+        }
+        else
+        {
+            // Poor tracking (slip/overload) → tin actual nhiều
+            alpha = _estimatorConfig.PoorTrackingBlend;
+        }
+
+        // Factor 2: Model confidence
+        // Nếu model không chính xác, giảm blend ratio
+        alpha *= modelConfidence;
+
+        // Clamp trong khoảng cho phép
+        alpha = Math.Clamp(alpha, _estimatorConfig.MinBlendRatio, _estimatorConfig.MaxBlendRatio);
+
+        return alpha;
+    }
+
+    /// <summary>
+    /// Reset estimator state
+    /// </summary>
+    public void Reset()
+    {
+        _predictionErrors.Clear();
+        _currentConfidence = 1.0;
+    }
+
+    /// <summary>
+    /// Get current model confidence
+    /// </summary>
+    public double GetModelConfidence()
+    {
+        return _currentConfidence;
+    }
+
+    public void LoadConfig()
+    {
+        _motorDynamicsConifg = NavigationConfig.GetMotorDynamicsConfig();
+        _motorDynamicsModel = new(_motorDynamicsConifg);
+        _estimatorConfig = NavigationConfig.GetVelocityEstimatorConfig();
+        _purePursuitConfig = NavigationConfig.GetPurepursuitConfig();
+        _signalConfig = NavigationConfig.GetVelocitySignalProcessingConfig();
+    }
+
+    public void SetAcceleration(double acc)
+    {
+        // SetAcceleration not available - commented out
+        // InverseKinematic.SetAcceleration(acc);
+    }
+
+    public void SetDeceleration(double dec)
+    {
+        // SetDeceleration not available - commented out
+        // InverseKinematic.SetDeceleration(dec);
+    }
+
+    public bool EnsureInverseKinematicsReady(CancellationToken cancellationToken)
+    {
+        try
+        {
+            // Increment counter for logging throttling
+            _ensureIKReadyCounter++;
+
+            // Check if need to reset fault first
+            // Note: DifferentialDrive doesn't expose IsFaulted, so we try FaultReset if not enabled
+            if (!InverseKinematic.IsOperationEnabled)
+            {
+                // Try fault reset first (in case it's in fault state)
+                InverseKinematic.FaultReset();
+                PreciseDelay(200, cancellationToken);
+            }
+
+            // Check if IInverseKinematics is in OperationEnabled state
+            if (!InverseKinematic.IsOperationEnabled)
+            {
+                // Auto-enable IInverseKinematics through state transitions
+                // Enable() is a convenience method that automatically transitions through all states
+                int maxAttempts = 3;
+                int attemptDelay = 300; // ms
+
+                for (int i = 0; i < maxAttempts && !InverseKinematic.IsOperationEnabled; i++)
+                {
+                    InverseKinematic.Enable();
+                    PreciseDelay(attemptDelay, cancellationToken);
+                }
+
+                // Check if enabled successfully
+                if (!InverseKinematic.IsOperationEnabled)
+                {
+                    // Log only once every 10 times to avoid spam
+                    if (_ensureIKReadyCounter % 10 == 0)
+                    {
+                        _logger.LogWarning("IInverseKinematics is not in OperationEnabled state. Cannot send velocity.");
+                    }
+                    return false;
+                }
+            }
+
+            // Check and set operation mode to ProfileVelocity (synchronous)
+            try
+            {
+                // GetOperationMode/SetOperationMode not available - commented out
+                // OperationMode currentMode = InverseKinematic.GetOperationMode();
+                // if (currentMode != OperationMode.ProfileVelocity)
+                // {
+                //     InverseKinematic.SetOperationMode(OperationMode.ProfileVelocity);
+                // }
+            }
+            catch (Exception ex)
+            {
+                // Log only once every 10 times to avoid spam
+                if (_ensureIKReadyCounter % 10 == 0)
+                {
+                    _logger.LogWarning(ex, "Error checking/setting operation mode");
+                }
+                // Continue anyway to prevent blocking
+            }
+
+            return true;
+        }
+        catch (OperationCanceledException)
+        {
+            throw;
+        }
+        catch (Exception ex)
+        {
+            // Log only once every 10 times to avoid spam
+            if (_ensureIKReadyCounter % 10 == 0)
+            {
+                _logger.LogError(ex, "Error ensuring IInverseKinematics ready");
+            }
+            return false;
+        }
+    }
+
+    /// <summary>
+    /// Precise synchronous delay using Thread.Sleep for longer delays and SpinWait for short delays
+    /// This ensures accurate timing for the update loop without async overhead
+    /// </summary>
+    private static void PreciseDelay(int milliseconds, CancellationToken cancellationToken)
+    {
+        if (milliseconds <= 0)
+            return;
+
+        if (milliseconds > 1)
+        {
+            // Use Thread.Sleep for longer delays (synchronous, more precise in dedicated thread)
+            // Check cancellation periodically during sleep
+            var sleepStart = DateTime.UtcNow;
+            while ((DateTime.UtcNow - sleepStart).TotalMilliseconds < milliseconds)
+            {
+                if (cancellationToken.IsCancellationRequested)
+                    return;
+
+                var remaining = milliseconds - (int)(DateTime.UtcNow - sleepStart).TotalMilliseconds;
+                if (remaining > 0)
+                {
+                    Thread.Sleep(Math.Min(remaining, 10)); // Sleep in 10ms chunks to check cancellation
+                }
+            }
+        }
+        else
+        {
+            // Use SpinWait for very short delays to maintain precise timing
+            var spinWait = new SpinWait();
+            for (int i = 0; i < 10; i++)
+            {
+                if (cancellationToken.IsCancellationRequested)
+                    break;
+                spinWait.SpinOnce();
+            }
+        }
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Navigation/CSharpNavigation.cs

@@ -0,0 +1,874 @@
+using RobotNet.VDA5050.Order;
+using RobotNet10.Common;
+using RobotNet10.RobotApp.Detection;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Services.ConfigManager;
+using RobotNet10.RobotApp.Services.Exceptions;
+using RobotNet10.RobotApp.Services.Navigation.CSharp;
+using RobotNet10.RobotApp.Services.Robot.Models;
+using RobotNet10.RobotApp.Services.Simulation;
+using RobotNet10.RobotApp.Shared.Enums;
+using RobotNet10.Shared.Geometry;
+
+namespace RobotNet10.RobotApp.Services.Navigation;
+
+public class NavigationConfig
+{
+    public double MaxLinearVelocity { get; set; }
+    public double MaxAngularVelocity { get; set; }
+    public double MinLinearVelocity { get; set; }
+    public double RotateAngularVelocity { get; set; }
+    public double Acceleration { get; set; } = 0.5;
+    public double Deceleration { get; set; } = 0.5;
+    public double ReachedRadius { get; set; } = 0.03;
+    public double HeadingTolerance { get; set; } = 3.0;
+    public double InitialRotationThreshold { get; set; } = 5.0;
+    public double DockToMaxSpeed { get; set; } = 0.3;
+    public double DockToRetrySpeed { get; set; } = 0.05;
+    public double DockToRotateSpeed { get; set; } = 0.05;
+    public Dictionary<SafetySpeed, double> SafetySpeedMap { get; set; } = [];
+    /// <summary>
+    /// Maximum distance (meters) from goal at which a Moving overshoot is still accepted as Completed.
+    /// Default: 0.15m
+    ///
+    /// Meaning: When overshoot is detected during Moving, if robot is within this radius of the goal,
+    /// navigation proceeds to final rotation → Completed. Otherwise → Error.
+    ///
+    /// ↑ Increase (0.2-0.3):
+    ///   ✓ More tolerant of overshoot — fewer Error states
+    ///   ✗ Robot may report Completed at a position far from goal
+    ///
+    /// ↓ Decrease (0.05-0.1):
+    ///   ✓ Higher positional accuracy requirement
+    ///   ✗ More likely to trigger Error on minor overshoot
+    ///
+    /// Tuning Tips:
+    /// - Should be ≥ ReachedRadius
+    /// - Must be ≤ MovingOvershootDetectionRadius to be meaningful
+    /// - For high-precision tasks: 0.05-0.1
+    /// - For general navigation: 0.15-0.2
+    /// </summary>
+    public double OvershootAcceptanceRadius { get; set; } = 0.15;
+
+    /// <summary>
+    /// Distance (meters) from goal at which overshoot detection begins during Moving.
+    /// Default: 0.5m
+    ///
+    /// Meaning: Overshoot detection only activates when robot is within this radius of the final goal.
+    /// Outside this radius, distance fluctuations are ignored.
+    ///
+    /// ↑ Increase (0.8-1.0):
+    ///   ✓ Earlier overshoot detection
+    ///   ✗ May false-trigger on path curvature near goal
+    ///
+    /// ↓ Decrease (0.2-0.3):
+    ///   ✓ Fewer false triggers
+    ///   ✗ Late detection — robot may travel further past goal before stopping
+    ///
+    /// Tuning Tips:
+    /// - Should be > OvershootAcceptanceRadius
+    /// - Typical: 2-5x the ReachedRadius
+    /// - If robot has high inertia/speed: increase to 0.8-1.0
+    /// </summary>
+    public double MovingOvershootDetectionRadius { get; set; } = 0.5;
+
+    /// <summary>
+    /// Distance (meters) from checkpoint at which PID deceleration begins during Moving.
+    /// Default: 5.0m
+    ///
+    /// Meaning: When distance to checkpoint > this value, robot runs at MaxLinearVelocity.
+    /// Below this distance, PID ramps velocity down proportionally.
+    ///
+    /// ↑ Increase (7-10):
+    ///   ✓ Earlier, smoother deceleration
+    ///   ✗ Slower average speed on long paths
+    ///
+    /// ↓ Decrease (2-3):
+    ///   ✓ Faster average speed — stays at max longer
+    ///   ✗ Sharper deceleration, may overshoot on heavy robots
+    ///
+    /// Tuning Tips:
+    /// - Depends on MaxLinearVelocity and robot mass/inertia
+    /// - Rule of thumb: stopping distance ≈ v² / (2 × deceleration)
+    /// - Heavy/fast robot: 7-10m; Light/slow robot: 2-3m
+    /// </summary>
+    public double DecelerationDistance { get; set; } = 5.0;
+
+    /// <summary>
+    /// Maximum linear velocity (m/s) when robot is carrying a load during Moving.
+    /// Default: 0.3 m/s
+    ///
+    /// When hasLoad=true, MaxLinearVelocity is capped at min(MaxLinearVelocity, LoadedMaxLinearVelocity).
+    /// </summary>
+    public double LoadedMaxLinearVelocity { get; set; } = 0.3;
+
+    /// <summary>
+    /// Maximum heading error (degrees) allowed when starting MoveStraight or Docking with a load.
+    /// Default: 10.0 degrees
+    ///
+    /// When hasLoad=true, the robot cannot rotate to correct heading before MoveStraight/Docking.
+    /// If the heading error exceeds this threshold at start, navigation transitions to Error.
+    /// </summary>
+    public double LoadedHeadingErrorThresholdDegrees { get; set; } = 10.0;
+}
+
+public partial class CSharpNavigation : INavigation, IDisposable
+{
+    public bool IsReady { get; private set; }
+    public bool Driving => NavState is NavigationState.Rotating or NavigationState.Moving or NavigationState.Docking or NavigationState.FinePositioning or NavigationState.MovingStraight or NavigationState.SafetyStop;
+    public double VelocityX => VelController.ActualVelocity.Linear;
+    public double VelocityY { get; private set; }
+    public double Omega => VelController.ActualVelocity.Angular;
+    public NavigationState State => NavState;
+    public IReadOnlyList<NavigationNode>? CurrentWaypoints => MovePurePursuit?.Waypoints_Value ?? MoveStraightController?.Waypoints_Value ?? DockToController?.Waypoints_Value;
+
+    // DockTo monitoring properties (read-only, for NavigationMonitor)
+    public bool IsDockingActive => NavState is NavigationState.Docking or NavigationState.FinePositioning;
+    public NavigationNode? DockGoal => DockToController?.Goal;
+    public string DockPhase => NavState switch
+    {
+        NavigationState.Docking => "Approaching",
+        NavigationState.FinePositioning when _finePositioningIsAligning => "Aligning",
+        NavigationState.FinePositioning => "Advancing",
+        _ => ""
+    };
+    public string DockDirection => DockToController?.DockConfig?.DockToDirection.ToString() ?? "";
+    public int DockRetryCount => _finePositioningRetryCount;
+    public int DockMaxRetries => _finePositioningMaxRetries;
+    public int DockWaypointCount => DockToController?.Waypoints_Value?.Count ?? 0;
+    public NavigationNode? DockStartNode => DockToController?.StartNode;
+    public IReadOnlyList<NavigationNode>? DockWaypoints => DockToController?.Waypoints_Value;
+
+    public event Action<NavigationState>? OnNavigationFinished;
+
+    private readonly ILocalization Localization;
+    public readonly IVelocityController VelController;
+    private readonly INavigationConfig NavigationConfig;
+    private readonly ILogger<CSharpNavigation> Logger;
+
+    private NavigationState NavState = NavigationState.Idle;
+    private NavigationState ResumeState = NavigationState.Idle;
+
+    // Safety stop: saves previous state for Refresh() to resume from
+    private NavigationState _safetyStopPreviousState = NavigationState.Idle;
+
+    private WatchThread<CSharpNavigation>? NavThread = null;
+    private const int CycleHandlerMilliseconds = 30;
+
+    private PID? MovePID;
+    private PurePursuit? MovePurePursuit;
+    private DockToController? DockToController;
+    private DockToController? MoveStraightController;
+    private IDetectSession? _dockSession;
+
+    private OrderNode? GoalRotate;
+    private OrderNode? CurrentBaseNode;
+    private HashSet<string> ProcessedRotations = [];
+
+    private double TargetAngle = 0;
+    private PID? RotatePID;
+
+    private readonly NavigationConfig NavCog;
+    private double MaxLinearVelocity = 0;
+
+    // Overshoot detection
+    private double _oldDistanceToGoal = double.MaxValue;
+    private bool _wasApproaching = false;
+
+    // Initial rotation skip
+    private bool _isInitialRotation = false;
+
+    // Fine Positioning state
+    private int _finePositioningRetryCount = 0;
+    private int _finePositioningMaxRetries = 3;
+    private int _finePositioningCycleCount = 0;
+    private int _finePositioningTimeoutMs = 6000;
+    private bool _finePositioningIsAligning = true;
+    private PID? _finePositionRotatePID = null;
+    private RobotDirection _finePositioningDirection = RobotDirection.FORWARD;
+    private bool _fpWasApproaching = false;
+    private int _fpOvershootCounter = 0;
+
+    private int _disposed = 0;
+
+    // HasLoad: robot is carrying a load — affects speed limits, rotation, and FinePositioning
+    private bool _hasLoad = false;
+
+    // Tracks whether velocity (0,0) has already been sent when entering Paused/SafetyStop.
+    // Prevents Navigation from continuously overwriting ManualControl velocity commands.
+    private bool _idleVelocityZeroSent = false;
+
+    private void ResetOvershootState()
+    {
+        _oldDistanceToGoal = double.MaxValue;
+        _wasApproaching = false;
+        _overshootCounter = 0;
+    }
+
+    public CSharpNavigation(IServiceProvider ServiceProvider)
+    {
+        Localization = ServiceProvider.GetRequiredService<ILocalization>();
+        VelController = ServiceProvider.GetRequiredService<IVelocityController>();
+        NavigationConfig = ServiceProvider.GetRequiredService<INavigationConfig>();
+        Logger = ServiceProvider.GetRequiredService<ILogger<CSharpNavigation>>();
+        NavCog = NavigationConfig.GetNavigationConfig();
+        MaxLinearVelocity = NavCog.MaxLinearVelocity;
+    }
+
+    public void Dispose()
+    {
+        if (Interlocked.Exchange(ref _disposed, 1) != 0) return;
+        HandleNavigationStop();
+        Clear();
+        OnNavigationFinished?.Invoke(NavState);
+        GC.SuppressFinalize(this);
+    }
+
+    public void Start()
+    {
+        IsReady = true;
+    }
+
+    public void Stop()
+    {
+        Dispose();
+    }
+
+    protected void HandleNavigationStart()
+    {
+        NavThread = new(CycleHandlerMilliseconds, NavigationHandler, Logger);
+        NavThread.Start();
+    }
+
+    protected void HandleNavigationStop()
+    {
+        NavThread?.Dispose();
+        NavThread = null;
+    }
+
+    public void CancelMovement()
+    {
+        NavState = NavigationState.Canceled;
+        Dispose();
+    }
+
+    public void Move(RobotNet.VDA5050.Order.OrderMsg order, bool hasLoad = false)
+    {
+        var nodes = order.Nodes;
+        var edges = order.Edges;
+        if (NavThread is not null) throw new NavigationException("The Navigation module is called during operation.");
+
+        _hasLoad = hasLoad;
+        NavState = NavigationState.Initializing;
+        if (!VelController.EnsureInverseKinematicsReady(CancellationToken.None))
+        {
+            NavState = NavigationState.Idle;
+            throw new NavigationException("The Velocity Controller is not ready for inverse kinematics.");
+        }
+        VelController.LoadConfig();
+        VelController.SetAcceleration(NavCog.Acceleration);
+        VelController.SetDeceleration(NavCog.Deceleration);
+        MovePID = new PID(NavigationConfig.GetMovePidConfig());
+        var ppConfig = NavigationConfig.GetPurepursuitConfig();
+        MovePurePursuit = new PurePursuit(NavigationConfig.GetPurepursuitConfig(), NavigationConfig.GetStanleyConig()).WithPath(nodes, edges, Localization.Theta);
+
+        // Reset overshoot detection state
+        ResetOvershootState();
+        _isInitialRotation = true;
+
+        (_, int index) = MovePurePursuit.OnNode(Localization.X, Localization.Y);
+        if (index >= MovePurePursuit.Waypoints_Value.Count - 1)
+        {
+            NavState = NavigationState.Completed;
+            Dispose();
+            return;
+        }
+
+        // === Local Planner: sinh approach path ===
+        var lpConfig = NavigationConfig.GetLocalPlannerConfig();
+        double heading;
+
+        var approachResult = lpConfig.Enabled
+            ? MovePurePursuit.GenerateAndPrependApproachPath(
+                  Localization.X, Localization.Y, index, lpConfig)
+            : ApproachResult.Disabled;
+
+        if (approachResult == ApproachResult.ApproachGenerated)
+        {
+            // Heading = hướng tiếp tuyến đầu approach curve (P0 → P1)
+            var wp0 = MovePurePursuit.Waypoints_Value[0];
+            var wp1 = MovePurePursuit.Waypoints_Value[1];
+            heading = Math.Atan2(wp1.Y - wp0.Y, wp1.X - wp0.X);
+
+            if (wp0.Direction == RobotDirection.BACKWARD)
+                heading += Math.PI;
+        }
+        else if (approachResult == ApproachResult.AlreadyOnPath)
+        {
+            // Robot đã trên path → dùng path tangent tại closest waypoint
+            var wp = MovePurePursuit.Waypoints_Value[index];
+            var wpNext = MovePurePursuit.Waypoints_Value[index + 1];
+            heading = Math.Atan2(wpNext.Y - wp.Y, wpNext.X - wp.X);
+
+            if (wp.Direction == RobotDirection.BACKWARD)
+                heading += Math.PI;
+        }
+        else
+        {
+            // TooFarFromPath / Disabled: heading hướng về lookahead point
+            double lookahead = (ppConfig.LookaheadMin + ppConfig.LookaheadMax) / 2;
+            var targetPoint = FindLookaheadTarget(index, lookahead);
+            targetPoint ??= MovePurePursuit.Waypoints_Value[^1];
+
+            heading = Math.Atan2(targetPoint.Y - Localization.Y, targetPoint.X - Localization.X);
+            if (targetPoint.Direction == RobotDirection.BACKWARD)
+                heading += Math.PI;
+        }
+
+        heading = SpaceCompute.NormalizeRadianAngle(heading);
+
+        // Cap speed when loaded
+        if (_hasLoad)
+        {
+            MaxLinearVelocity = Math.Min(MaxLinearVelocity, NavCog.LoadedMaxLinearVelocity);
+        }
+
+        Rotate(heading);
+    }
+
+    public void MoveStraight(double x, double y, bool hasLoad = false, RobotDirection? direction = null)
+    {
+        if (NavThread is not null) throw new NavigationException("The Navigation module is called during operation.");
+
+        _hasLoad = hasLoad;
+        NavState = NavigationState.Initializing;
+        if (!VelController.EnsureInverseKinematicsReady(CancellationToken.None))
+        {
+            NavState = NavigationState.Idle;
+            throw new NavigationException("The Velocity Controller is not ready for inverse kinematics.");
+        }
+        VelController.LoadConfig();
+        VelController.SetAcceleration(NavCog.Acceleration);
+        VelController.SetDeceleration(NavCog.Deceleration);
+
+        MovePID = new PID(NavigationConfig.GetMovePidConfig());
+        var straightCfg = NavigationConfig.GetMoveStraightConfig().Clone();
+        if (direction.HasValue) straightCfg.DockToDirection = direction.Value;
+
+        var startNode = new NavigationNode
+        {
+            Id = Guid.NewGuid(),
+            X = Localization.X,
+            Y = Localization.Y,
+        };
+
+        var goalNode = new NavigationNode
+        {
+            Id = Guid.NewGuid(),
+            X = x,
+            Y = y,
+        };
+
+        MoveStraightController = new DockToController(straightCfg).WithPath(startNode, goalNode);
+
+        (_, int index) = MoveStraightController.GetClosestAheadWaypoint(Localization.X, Localization.Y);
+        if (index >= MoveStraightController.Waypoints_Value.Count - 1)
+        {
+            NavState = NavigationState.Completed;
+            Dispose();
+            return;
+        }
+
+        double heading = Math.Atan2(y - Localization.Y, x - Localization.X);
+        if (straightCfg.DockToDirection == RobotDirection.BACKWARD)
+            heading += Math.PI;
+        heading = SpaceCompute.NormalizeRadianAngle(heading);
+
+        ResetOvershootState();
+        MaxLinearVelocity = NavCog.MaxLinearVelocity;
+
+        if (_hasLoad)
+        {
+            // When loaded: no rotation allowed. Check heading error.
+            double headingError = heading - Localization.Theta;
+            if (headingError > Math.PI) headingError -= 2 * Math.PI;
+            else if (headingError < -Math.PI) headingError += 2 * Math.PI;
+
+            double thresholdRad = NavCog.LoadedHeadingErrorThresholdDegrees * Math.PI / 180.0;
+            if (Math.Abs(headingError) > thresholdRad)
+            {
+                Logger.LogError($"MoveStraight hasLoad: heading error {Math.Abs(headingError) * 180 / Math.PI:F1}° exceeds threshold {NavCog.LoadedHeadingErrorThresholdDegrees:F1}°");
+                NavState = NavigationState.Error;
+                Dispose();
+                return;
+            }
+
+            // Skip rotation, go directly to MovingStraight
+            NavState = NavigationState.MovingStraight;
+            HandleNavigationStart();
+        }
+        else
+        {
+            _isInitialRotation = true;
+            Rotate(heading);
+        }
+    }
+
+    public void DockTo(IDetectSession session, bool hasLoad = false, RobotDirection? direction = null)
+    {
+        if (NavThread is not null) throw new NavigationException("The Navigation module is called during operation.");
+        var goal = session.Goal ?? throw new NavigationException("Dock to Goal is not existed");
+        _hasLoad = hasLoad;
+        _dockSession = session;
+        NavState = NavigationState.Initializing;
+
+        if (!VelController.EnsureInverseKinematicsReady(CancellationToken.None))
+        {
+            NavState = NavigationState.Idle;
+            throw new NavigationException("The Velocity Controller is not ready for inverse kinematics.");
+        }
+        VelController.LoadConfig();
+        VelController.SetAcceleration(NavCog.Acceleration);
+        VelController.SetDeceleration(NavCog.Deceleration);
+
+
+        MovePID = new PID(NavigationConfig.GetMovePidConfig());
+        var docktoConfig = NavigationConfig.GetDockToConfig().Clone();
+        if (direction.HasValue) docktoConfig.DockToDirection = direction.Value;
+        var currentGoal = new NavigationNode()
+        {
+            Id = Guid.NewGuid(),
+            X = goal.Pose.Position.X,
+            Y = goal.Pose.Position.Y,
+            Speed = NavCog.DockToMaxSpeed,
+            Theta = goal.Pose.Orientation.ToYawRadian(),
+        };
+
+        var startNode = GetDockToStartNode(Localization.X, Localization.Y, currentGoal.X, currentGoal.Y, currentGoal.Theta ?? 0, docktoConfig.DockToLength);
+        DockToController = new DockToController(docktoConfig).WithPath(startNode, currentGoal);
+
+        (_, int index) = DockToController.GetClosestAheadWaypoint(Localization.X, Localization.Y);
+        if (index >= DockToController.Waypoints_Value.Count - 1)
+        {
+            NavState = NavigationState.Completed;
+
+            Dispose();
+            return;
+        }
+
+        var pathAngle = Math.Atan2(currentGoal.Y - startNode.Y, currentGoal.X - startNode.X);
+
+        double frontX = DockToController.Waypoints_Value[index].X + docktoConfig.WheelBase * Math.Cos(pathAngle);
+        double frontY = DockToController.Waypoints_Value[index].Y + docktoConfig.WheelBase * Math.Sin(pathAngle);
+
+        double dx = frontX - Localization.X;
+        double dy = frontY - Localization.Y;
+        double heading = Math.Atan2(dy, dx);
+
+        if (docktoConfig.DockToDirection == RobotDirection.BACKWARD)
+            heading += Math.PI;
+
+        heading = SpaceCompute.NormalizeRadianAngle(heading);
+
+        ResetOvershootState();
+        MaxLinearVelocity = NavCog.DockToMaxSpeed;
+
+        // Load Fine Positioning config from DockToConfig
+        _finePositioningTimeoutMs = docktoConfig.FinePositioningTimeoutMs;
+        _finePositioningMaxRetries = docktoConfig.FinePositioningMaxRetries;
+
+        // Reset Fine Positioning state
+        _finePositioningRetryCount = 0;
+        _finePositioningCycleCount = 0;
+        _finePositioningIsAligning = true;
+        _finePositionRotatePID = null;
+        _finePositioningDirection = RobotDirection.FORWARD;
+        _fpWasApproaching = false;
+        _fpOvershootCounter = 0;
+
+        if (_hasLoad)
+        {
+            // When loaded: no rotation allowed. Check heading error.
+            double headingError = heading - Localization.Theta;
+            if (headingError > Math.PI) headingError -= 2 * Math.PI;
+            else if (headingError < -Math.PI) headingError += 2 * Math.PI;
+
+            double thresholdRad = NavCog.LoadedHeadingErrorThresholdDegrees * Math.PI / 180.0;
+            if (Math.Abs(headingError) > thresholdRad)
+            {
+                Logger.LogError($"DockTo hasLoad: heading error {Math.Abs(headingError) * 180 / Math.PI:F1}° exceeds threshold {NavCog.LoadedHeadingErrorThresholdDegrees:F1}°");
+                NavState = NavigationState.Error;
+                Dispose();
+                return;
+            }
+
+            // Skip rotation, go directly to Docking
+            NavState = NavigationState.Docking;
+            HandleNavigationStart();
+        }
+        else
+        {
+            _isInitialRotation = true;
+            Rotate(heading);
+        }
+    }
+
+    public void Pause()
+    {
+        ResumeState = NavState;
+        _idleVelocityZeroSent = false;
+        NavState = NavigationState.Paused;
+    }
+
+    public void SafetyStop()
+    {
+        _safetyStopPreviousState = NavState;
+        _idleVelocityZeroSent = false;
+        NavState = NavigationState.SafetyStop;
+    }
+
+    public void Refresh()
+    {
+        _idleVelocityZeroSent = false;
+        if (NavState != NavigationState.SafetyStop)
+        {
+            NavState = NavigationState.Idle;
+            return;
+        }
+
+        var prevState = _safetyStopPreviousState;
+        var x = Localization.X;
+        var y = Localization.Y;
+
+        if (prevState is NavigationState.Moving
+            && MovePurePursuit?.Waypoints_Value is { Count: > 2 })
+        {
+            MovePurePursuit.ResetTracking();
+            MovePurePursuit.RebuildPath();
+
+            (_, int index) = MovePurePursuit.OnNode(x, y);
+            if (index >= MovePurePursuit.Waypoints_Value.Count - 1)
+            { NavState = NavigationState.Completed; Dispose(); return; }
+
+            MovePID = new PID(NavigationConfig.GetMovePidConfig());
+            ResetOvershootState();
+
+            // === Local Planner (same logic as Move) ===
+            var lpConfig = NavigationConfig.GetLocalPlannerConfig();
+            var ppConfig = NavigationConfig.GetPurepursuitConfig();
+            double heading;
+
+            var approachResult = lpConfig.Enabled
+                ? MovePurePursuit.GenerateAndPrependApproachPath(
+                      x, y, index, lpConfig)
+                : ApproachResult.Disabled;
+
+            if (approachResult == ApproachResult.ApproachGenerated)
+            {
+                var wp0 = MovePurePursuit.Waypoints_Value[0];
+                var wp1 = MovePurePursuit.Waypoints_Value[1];
+                heading = Math.Atan2(wp1.Y - wp0.Y, wp1.X - wp0.X);
+                if (wp0.Direction == RobotDirection.BACKWARD) heading += Math.PI;
+            }
+            else if (approachResult == ApproachResult.AlreadyOnPath)
+            {
+                var wp = MovePurePursuit.Waypoints_Value[index];
+                var wpNext = MovePurePursuit.Waypoints_Value[index + 1];
+                heading = Math.Atan2(wpNext.Y - wp.Y, wpNext.X - wp.X);
+                if (wp.Direction == RobotDirection.BACKWARD) heading += Math.PI;
+            }
+            else
+            {
+                double lookahead = (ppConfig.LookaheadMin + ppConfig.LookaheadMax) / 2;
+                var target = FindLookaheadTarget(index, lookahead)
+                             ?? MovePurePursuit.Waypoints_Value[^1];
+                heading = Math.Atan2(target.Y - y, target.X - x);
+                if (target.Direction == RobotDirection.BACKWARD) heading += Math.PI;
+            }
+
+            heading = SpaceCompute.NormalizeRadianAngle(heading);
+
+            _isInitialRotation = true;
+            RotatePID = new PID(NavigationConfig.GetRotatePidConfig());
+            TargetAngle = heading;
+            NavState = NavigationState.Rotating;
+        }
+        else if (prevState is NavigationState.Docking
+                 && DockToController?.Waypoints_Value is { Count: > 2 })
+        {
+            DockToController.ResetTracking();
+            (_, int index) = DockToController.GetClosestAheadWaypoint(x, y);
+            if (index >= DockToController.Waypoints_Value.Count - 1)
+            { NavState = NavigationState.Completed; Dispose(); return; }
+
+            MovePID = new PID(NavigationConfig.GetMovePidConfig());
+            ResetOvershootState();
+
+            var pathAngle = Math.Atan2(
+                DockToController.Goal.Y - DockToController.Waypoints_Value[0].Y,
+                DockToController.Goal.X - DockToController.Waypoints_Value[0].X);
+            double frontX = DockToController.Waypoints_Value[index].X
+                + DockToController.DockConfig.WheelBase * Math.Cos(pathAngle);
+            double frontY = DockToController.Waypoints_Value[index].Y
+                + DockToController.DockConfig.WheelBase * Math.Sin(pathAngle);
+            double heading = Math.Atan2(frontY - y, frontX - x);
+            if (DockToController.DockConfig.DockToDirection == RobotDirection.BACKWARD)
+                heading += Math.PI;
+            heading = SpaceCompute.NormalizeRadianAngle(heading);
+
+            if (_hasLoad)
+            {
+                // When loaded: check heading error, resume Docking directly without rotation
+                double headingError = heading - Localization.Theta;
+                if (headingError > Math.PI) headingError -= 2 * Math.PI;
+                else if (headingError < -Math.PI) headingError += 2 * Math.PI;
+
+                double thresholdRad = NavCog.LoadedHeadingErrorThresholdDegrees * Math.PI / 180.0;
+                if (Math.Abs(headingError) > thresholdRad)
+                {
+                    Logger.LogError($"Refresh Docking hasLoad: heading error {Math.Abs(headingError) * 180 / Math.PI:F1}° exceeds threshold");
+                    NavState = NavigationState.Error;
+                    Dispose();
+                    return;
+                }
+                NavState = NavigationState.Docking;
+            }
+            else
+            {
+                _isInitialRotation = true;
+                RotatePID = new PID(NavigationConfig.GetRotatePidConfig());
+                TargetAngle = heading;
+                NavState = NavigationState.Rotating;
+            }
+        }
+        else if (prevState is NavigationState.MovingStraight
+                 && MoveStraightController?.Waypoints_Value is { Count: > 2 })
+        {
+            MoveStraightController.ResetTracking();
+            (_, int index) = MoveStraightController.GetClosestAheadWaypoint(x, y);
+            if (index >= MoveStraightController.Waypoints_Value.Count - 1)
+            { NavState = NavigationState.Completed; Dispose(); return; }
+
+            MovePID = new PID(NavigationConfig.GetMovePidConfig());
+            ResetOvershootState();
+
+            double heading = Math.Atan2(
+                MoveStraightController.Goal.Y - y,
+                MoveStraightController.Goal.X - x);
+            if (MoveStraightController.DockConfig.DockToDirection == RobotDirection.BACKWARD)
+                heading += Math.PI;
+            heading = SpaceCompute.NormalizeRadianAngle(heading);
+
+            if (_hasLoad)
+            {
+                // When loaded: check heading error, resume MovingStraight directly without rotation
+                double headingError = heading - Localization.Theta;
+                if (headingError > Math.PI) headingError -= 2 * Math.PI;
+                else if (headingError < -Math.PI) headingError += 2 * Math.PI;
+
+                double thresholdRad = NavCog.LoadedHeadingErrorThresholdDegrees * Math.PI / 180.0;
+                if (Math.Abs(headingError) > thresholdRad)
+                {
+                    Logger.LogError($"Refresh MovingStraight hasLoad: heading error {Math.Abs(headingError) * 180 / Math.PI:F1}° exceeds threshold");
+                    NavState = NavigationState.Error;
+                    Dispose();
+                    return;
+                }
+                NavState = NavigationState.MovingStraight;
+            }
+            else
+            {
+                _isInitialRotation = true;
+                RotatePID = new PID(NavigationConfig.GetRotatePidConfig());
+                TargetAngle = heading;
+                NavState = NavigationState.Rotating;
+            }
+        }
+        else if (prevState is NavigationState.Rotating)
+        {
+            RotatePID = new PID(NavigationConfig.GetRotatePidConfig());
+            NavState = NavigationState.Rotating;
+        }
+        else if (prevState is NavigationState.FinePositioning)
+        {
+            _finePositioningCycleCount = 0;
+            _finePositioningIsAligning = true;
+            _finePositionRotatePID = null;
+            NavState = NavigationState.Docking;
+        }
+        else
+        {
+            NavState = NavigationState.Idle;
+        }
+    }
+
+    public void RefreshOrder(Node[] nodes, Edge[] edges)
+    {
+    }
+
+    public void Resume()
+    {
+        if (ResumeState == NavigationState.FinePositioning)
+        {
+            _finePositioningCycleCount = 0;
+        }
+        _idleVelocityZeroSent = false;
+        NavState = ResumeState;
+    }
+
+    public void Rotate(double angle)
+    {
+        if (NavThread is not null) throw new NavigationException("The Navigation module is called during operation.");
+        
+        if (!VelController.EnsureInverseKinematicsReady(CancellationToken.None))
+        {
+            NavState = NavigationState.Idle;
+            throw new NavigationException("The Velocity Controller is not ready for inverse kinematics.");
+        }
+        
+        RotatePID = new PID(NavigationConfig.GetRotatePidConfig());
+        TargetAngle = SpaceCompute.NormalizeRadianAngle(angle);
+        NavState = NavigationState.Rotating;
+        HandleNavigationStart();
+    }
+
+    public void SetSpeed(double speed)
+    {
+        MaxLinearVelocity = _hasLoad ? Math.Min(speed, NavCog.LoadedMaxLinearVelocity) : speed;
+    }
+
+    protected void UpdateGoal(string goalId)
+    {
+        MovePurePursuit?.UpdateGoal(goalId);
+    }
+
+    public void UpdateOrder(string newBaseNodeId)
+    {
+        var newBaseNode = MovePurePursuit?.OrderNodes.FirstOrDefault(n => n.NodeId == newBaseNodeId);
+        if (newBaseNode is not null && newBaseNode.NodeId != CurrentBaseNode?.NodeId)
+        {
+            CurrentBaseNode = newBaseNode;
+            var newGoalRotate = FindNextRotateGoal();
+            if (newGoalRotate is not null && newGoalRotate.NodeId != GoalRotate?.NodeId)
+            {
+                GoalRotate = newGoalRotate;
+                UpdateGoal(newGoalRotate.NodeId);
+            }
+        }
+    }
+
+    private void Clear()
+    {
+        VelController.SetVelocity(0, 0);
+        CurrentBaseNode = null;
+        MovePurePursuit = null;
+        MovePID = null;
+        RotatePID = null;
+        GoalRotate = null;
+        DockToController = null;
+        MoveStraightController = null;
+        _dockSession = null;
+        ProcessedRotations = [];
+        ResetOvershootState();
+        _isInitialRotation = false;
+        _finePositioningRetryCount = 0;
+        _finePositioningCycleCount = 0;
+        _finePositioningIsAligning = true;
+        _finePositionRotatePID = null;
+        _finePositioningDirection = RobotDirection.FORWARD;
+        _fpWasApproaching = false;
+        _fpOvershootCounter = 0;
+        _hasLoad = false;
+        _idleVelocityZeroSent = false;
+    }
+
+    /// <summary>
+    /// Tìm target point tại lookahead distance từ vị trí hiện tại trên path.
+    /// Nội suy giữa các waypoint để tạo điểm mượt.
+    /// </summary>
+    private NavigationNode? FindLookaheadTarget(int startIndex, double lookaheadDistance)
+    {
+        if (MovePurePursuit is null || startIndex >= MovePurePursuit.Waypoints_Value.Count - 1)
+            return null;
+
+        double accumulatedDistance = 0;
+
+        for (int i = startIndex; i < MovePurePursuit.Waypoints_Value.Count - 1; i++)
+        {
+            double dx = MovePurePursuit.Waypoints_Value[i + 1].X - MovePurePursuit.Waypoints_Value[i].X;
+            double dy = MovePurePursuit.Waypoints_Value[i + 1].Y - MovePurePursuit.Waypoints_Value[i].Y;
+            double segmentLength = Math.Sqrt(dx * dx + dy * dy);
+
+            if (accumulatedDistance + segmentLength >= lookaheadDistance)
+            {
+                double t = (lookaheadDistance - accumulatedDistance) / segmentLength;
+                return new NavigationNode
+                {
+                    Id = Guid.NewGuid(),
+                    NodeId = string.Empty,
+                    X = MovePurePursuit.Waypoints_Value[i].X + t * dx,
+                    Y = MovePurePursuit.Waypoints_Value[i].Y + t * dy,
+                    Direction = MovePurePursuit.Waypoints_Value[i].Direction
+                };
+            }
+
+            accumulatedDistance += segmentLength;
+        }
+
+        return null;
+    }
+
+    /// <summary>
+    /// Tìm node có IsWaitingRotate đầu tiên trong path từ currentNode đến currentGoal
+    /// </summary>
+    protected OrderNode? FindNextRotateGoal()
+    {
+        if (CurrentBaseNode == null || MovePurePursuit is null || MovePurePursuit.OrderNodes.Length == 0) return null;
+
+        int goalIndex = Array.FindIndex(MovePurePursuit.OrderNodes, n => n.NodeId == CurrentBaseNode.NodeId);
+        if (goalIndex == -1) return null;
+
+        int lastNodeIdx = Array.FindIndex(MovePurePursuit.OrderNodes, n => n.NodeId == GoalRotate?.NodeId);
+        lastNodeIdx = lastNodeIdx == -1 ? 0 : lastNodeIdx + 1;
+
+        // Tìm từ node hiện tại đến goal
+        for (int i = lastNodeIdx; i <= goalIndex; i++)
+        {
+            var node = MovePurePursuit.OrderNodes[i];
+
+            // Tìm node có IsWaitingRotate và chưa xử lý
+            if (node.IsWaitRotating && !ProcessedRotations.Contains(node.NodeId))
+            {
+                return node;
+            }
+        }
+        return CurrentBaseNode;
+    }
+
+    private NavigationNode GetDockToStartNode(double x, double y, double goalX, double goalY, double goalTheta, double length)
+    {
+        // Hướng vuông góc với theta
+        double perpAngle = SpaceCompute.NormalizeRadianAngle(goalTheta + Math.PI / 2);
+
+        // Hai endpoint của đoạn thẳng, cách goal ±length theo hướng vuông góc
+        double ep1X = goalX + length * Math.Cos(perpAngle);
+        double ep1Y = goalY + length * Math.Sin(perpAngle);
+
+        double ep2X = goalX - length * Math.Cos(perpAngle);
+        double ep2Y = goalY - length * Math.Sin(perpAngle);
+
+        // Chọn endpoint gần (x, y) nhất
+        double dist1Sq = (ep1X - x) * (ep1X - x) + (ep1Y - y) * (ep1Y - y);
+        double dist2Sq = (ep2X - x) * (ep2X - x) + (ep2Y - y) * (ep2Y - y);
+
+        double startX = dist1Sq <= dist2Sq ? ep1X : ep2X;
+        double startY = dist1Sq <= dist2Sq ? ep1Y : ep2Y;
+
+        return new NavigationNode
+        {
+            Id = Guid.NewGuid(),
+            X = startX,
+            Y = startY,
+        };
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Navigation/CSharpNavigationDifferental.cs

@@ -0,0 +1,693 @@
+using RobotNet10.Common;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Services.ConfigManager;
+using RobotNet10.RobotApp.Services.Navigation.CSharp;
+using RobotNet10.RobotApp.Services.Simulation;
+using RobotNet10.RobotApp.Shared.Enums;
+using RobotNet10.Shared.Geometry;
+
+namespace RobotNet10.RobotApp.Services.Navigation;
+
+public partial class CSharpNavigation
+{
+    private bool IsBackToPath = false;
+    private double? BackToAngle;
+    private int _overshootCounter = 0;
+    private readonly int _overshootOut = 5;
+
+    private void Rotating()
+    {
+        if (RotatePID is not null)
+        {
+            double Error = SpaceCompute.NormalizeRadianAngle(TargetAngle - Localization.Theta);
+
+            // Skip initial rotation nếu heading error đủ nhỏ
+            if (_isInitialRotation)
+            {
+                double initialThresholdRad = (NavCog?.InitialRotationThreshold ?? 5.0) * Math.PI / 180.0;
+                if (Math.Abs(Error) < initialThresholdRad)
+                {
+                    _isInitialRotation = false;
+                    VelController.SetVelocity(0, 0);
+                    if (MovePurePursuit is not null && MovePurePursuit.Waypoints_Value is not null && MovePurePursuit.Waypoints_Value.Count > 2)
+                    {
+                        ResetOvershootState();
+                        NavState = NavigationState.Moving;
+                    }
+                    else if (MoveStraightController is not null && MoveStraightController.Waypoints_Value is not null && MoveStraightController.Waypoints_Value.Count > 2)
+                    {
+                        ResetOvershootState();
+                        NavState = NavigationState.MovingStraight;
+                    }
+                    else if (DockToController is not null && DockToController.Waypoints_Value is not null && DockToController.Waypoints_Value.Count > 2)
+                    {
+                        ResetOvershootState();
+                        NavState = NavigationState.Docking;
+                    }
+                    else
+                    {
+                        NavState = NavigationState.Completed;
+                        Logger.LogInformation($"Navigation Reached initial heading: Pose({Localization.X} - {Localization.Y} - {Localization.Theta})");
+                        Dispose();
+                    }
+                    return;
+                }
+            }
+
+            double headingToleranceRad = (NavCog?.HeadingTolerance ?? 3.0) * Math.PI / 180.0;
+            if (Math.Abs(Error) < headingToleranceRad)
+            {
+                _isInitialRotation = false;
+                if (IsBackToPath && BackToAngle.HasValue)
+                {
+                    TargetAngle = BackToAngle.Value;
+                    BackToAngle = null;
+                    IsBackToPath = false;
+                }
+                else
+                {
+                    ResetOvershootState();
+                    if (MovePurePursuit is not null && MovePurePursuit.Waypoints_Value is not null && MovePurePursuit.Waypoints_Value.Count > 2)
+                    {
+                        NavState = NavigationState.Moving;
+                    }
+                    else if (MoveStraightController is not null && MoveStraightController.Waypoints_Value is not null && MoveStraightController.Waypoints_Value.Count > 2)
+                    {
+                        NavState = NavigationState.MovingStraight;
+                    }
+                    else if (DockToController is not null && DockToController.Waypoints_Value is not null && DockToController.Waypoints_Value.Count > 2)
+                    {
+                        NavState = NavigationState.Docking;
+                    }
+                    else
+                    {
+                        NavState = NavigationState.Completed;
+                        Logger.LogInformation($"Navigation Reached heading: Pose({Localization.X} - {Localization.Y} - {Localization.Theta})");
+                        Dispose();
+                    }
+                }
+            }
+            else
+            {
+                var SpeedCal = RotatePID.PID_step(Error, NavCog?.RotateAngularVelocity ?? 0.1, -(NavCog?.RotateAngularVelocity ?? 0.1), CycleHandlerMilliseconds / 1000.0);
+                VelController.SetVelocity(0, SpeedCal);
+            }
+        }
+    }
+
+    private void Moving()
+    {
+        if (MovePID is not null && MovePurePursuit is not null && MovePurePursuit.Waypoints_Value is not null && MovePurePursuit?.OrderNodes is not null && MovePurePursuit.OrderNodes.Length > 1 && GoalRotate is not null)
+        {
+            var DistanceToGoal = Math.Sqrt(Math.Pow(Localization.X - MovePurePursuit.OrderNodes[^1].X, 2) + Math.Pow(Localization.Y - MovePurePursuit.OrderNodes[^1].Y, 2));
+            var DistanceToCheckingNode = Math.Sqrt(Math.Pow(Localization.X - GoalRotate.X, 2) + Math.Pow(Localization.Y - GoalRotate.Y, 2));
+            var reachedRadius = NavCog?.ReachedRadius ?? 0.05;
+            var deviation = GoalRotate.NodeId == MovePurePursuit.OrderNodes[^1].NodeId ? reachedRadius : GoalRotate.AllowedDeviationXY ?? 0.1;
+
+            // Overshoot detection: phát hiện robot đi qua goal
+            var overshootDetectionRadius = NavCog?.MovingOvershootDetectionRadius ?? 0.5;
+            if (DistanceToGoal < overshootDetectionRadius)
+            {
+                if (DistanceToGoal < _oldDistanceToGoal)
+                {
+                    _wasApproaching = true;
+                    _overshootCounter = 0;
+                }
+                else if (_wasApproaching && DistanceToGoal > _oldDistanceToGoal)
+                {
+                    _overshootCounter++;
+                    if (_wasApproaching && _overshootCounter >= _overshootOut)
+                    {
+                        _wasApproaching = false;
+                        VelController.SetVelocity(0, 0);
+
+                        // Kiểm tra vị trí overshoot có chấp nhận được không
+                        double acceptanceRadius = NavCog?.OvershootAcceptanceRadius ?? 0.15;
+                        if (DistanceToGoal > acceptanceRadius)
+                        {
+                            Logger.LogError($"Moving overshoot too far: distance={DistanceToGoal:F4}m > acceptance={acceptanceRadius}m");
+                            NavState = NavigationState.Error;
+                            Dispose();
+                            return;
+                        }
+
+                        Logger.LogWarning($"Overshoot detected at distance {DistanceToGoal:F4}m (within acceptance={acceptanceRadius}m), transitioning to final rotation");
+                        if (MovePurePursuit.OrderNodes[^1].Theta is { } overshootTheta)
+                        {
+                            TargetAngle = overshootTheta;
+                            NavState = NavigationState.Rotating;
+                            RotatePID?.Reset();
+                            MovePurePursuit = null;
+                        }
+                        else
+                        {
+                            NavState = NavigationState.Completed;
+                            Dispose();
+                        }
+                        return;
+                    }
+                }
+                else _overshootCounter = 0;
+            }
+            _oldDistanceToGoal = DistanceToGoal;
+
+            if (DistanceToCheckingNode > deviation)
+            {
+                double dt = CycleHandlerMilliseconds / 1000.0;
+                double decelerationDist = NavCog?.DecelerationDistance ?? 5.0;
+                double maxLinearVel = DistanceToCheckingNode > decelerationDist
+                    ? MaxLinearVelocity
+                    : MovePID.PID_step(DistanceToCheckingNode, MaxLinearVelocity, NavCog?.MinLinearVelocity ?? 0.01, dt);
+                maxLinearVel = Math.Clamp(maxLinearVel, NavCog?.MinLinearVelocity ?? 0.01, MaxLinearVelocity);
+
+                (double linearVelActual, _) = VelController.ActualVelocity;
+                (double LinearVel, double AngularVel) = MovePurePursuit.PurePursuit_step(Localization.X, Localization.Y, Localization.Theta, linearVelActual, maxLinearVel);
+
+                // Clamp angular velocity
+                AngularVel = Math.Clamp(AngularVel, -(NavCog?.MaxAngularVelocity ?? 1.5), NavCog?.MaxAngularVelocity ?? 1.5);
+
+                // Clamp linear velocity (giữ dấu cho backward)
+                var linearSign = Math.Sign(LinearVel);
+                LinearVel = linearSign * Math.Clamp(Math.Abs(LinearVel), NavCog?.MinLinearVelocity ?? 0.01, MaxLinearVelocity);
+
+                VelController.SetVelocity(LinearVel, AngularVel);
+            }
+            else if (DistanceToGoal < reachedRadius)
+            {
+                VelController.SetVelocity(0, 0);
+                if (MovePurePursuit.OrderNodes[^1].Theta is { } theta)
+                {
+                    TargetAngle = theta;
+                    NavState = NavigationState.Rotating;
+                    RotatePID?.Reset();
+                    MovePurePursuit = null;
+                }
+                else
+                {
+                    NavState = NavigationState.Completed;
+                    Logger.LogInformation($"Navigation Reached: Pose({Localization.X} - {Localization.Y} - {Localization.Theta})");
+                    Dispose();
+                }
+            }
+            else
+            {
+                double? targetAngle = null;
+                if (GoalRotate.Theta is { } theta)
+                {
+                    targetAngle = theta;
+                    ProcessedRotations.Add(GoalRotate.NodeId);
+                    BackToAngle = GoalRotate?.ContinueTheta;
+                    IsBackToPath = true;
+                }
+
+                var newGoalRotate = FindNextRotateGoal();
+                if (newGoalRotate is not null && newGoalRotate.NodeId != GoalRotate?.NodeId)
+                {
+                    GoalRotate = newGoalRotate;
+                    UpdateGoal(newGoalRotate.NodeId);
+                }
+
+                if (targetAngle.HasValue)
+                {
+                    TargetAngle = targetAngle.Value;
+                    NavState = NavigationState.Rotating;
+                    RotatePID?.Reset();
+                }
+            }
+        }
+    }
+
+    private void Docking()
+    {
+        if (MovePID is not null && DockToController is not null && DockToController.Waypoints_Value is not null && DockToController.Goal != null)
+        {
+            var dockCfg = DockToController.DockConfig;
+
+            // --- Continuous goal update from detection session ---
+            TryUpdateDockGoalFromSession(NavCog.DockToMaxSpeed);
+
+            var DistanceToGoal = Math.Sqrt(Math.Pow(Localization.X - DockToController.Goal.X, 2) + Math.Pow(Localization.Y - DockToController.Goal.Y, 2));
+            var deviation = dockCfg.ReachedRadius;
+
+            // Overshoot detection: phát hiện robot đi qua goal
+            var dockOvershootRadius = dockCfg.DockingOvershootDetectionRadius;
+            if (DistanceToGoal < dockOvershootRadius)
+            {
+                if (DistanceToGoal < _oldDistanceToGoal)
+                {
+                    _wasApproaching = true;
+                    _overshootCounter = 0;
+                }
+                else if (_wasApproaching && DistanceToGoal > _oldDistanceToGoal)
+                {
+                    _overshootCounter++;
+                    if (_wasApproaching && _overshootCounter >= _overshootOut)
+                    {
+                        _wasApproaching = false;
+                        _overshootCounter = 0;
+                        VelController.SetVelocity(0, 0);
+
+                        if (_hasLoad)
+                        {
+                            // When loaded: no FinePositioning allowed, go to Error
+                            Logger.LogError($"Docking overshoot with load at distance {DistanceToGoal:F4}m. No FinePositioning allowed.");
+                            NavState = NavigationState.Error;
+                            Dispose();
+                            return;
+                        }
+
+                        Logger.LogWarning(
+                            $"Overshoot detected at distance {DistanceToGoal:F4}m. " +
+                            $"Entering FinePositioning (attempt {_finePositioningRetryCount + 1}/{_finePositioningMaxRetries}).");
+
+                        // Transition to FinePositioning instead of giving up
+                        _finePositioningCycleCount = 0;
+                        _finePositioningIsAligning = true;
+                        _finePositionRotatePID = new PID(NavigationConfig.GetRotatePidConfig());
+                        _oldDistanceToGoal = double.MaxValue;
+                        _fpWasApproaching = false;
+                        _fpOvershootCounter = 0;
+                        NavState = NavigationState.FinePositioning;
+                        return;
+                    }
+                }
+                else _overshootCounter = 0;
+            }
+            _oldDistanceToGoal = DistanceToGoal;
+
+            if (DistanceToGoal > deviation)
+            {
+                double dt = CycleHandlerMilliseconds / 1000.0;
+                double dockDecelerationDist = dockCfg.DecelerationDistance;
+                double maxLinearVel = DistanceToGoal > dockDecelerationDist
+                    ? MaxLinearVelocity
+                    : MovePID.PID_step(DistanceToGoal, MaxLinearVelocity, NavCog?.MinLinearVelocity ?? 0.01, dt);
+                maxLinearVel = Math.Clamp(maxLinearVel, NavCog?.MinLinearVelocity ?? 0.01, MaxLinearVelocity);
+
+                (double linearVelActual, _) = VelController.ActualVelocity;
+                (double LinearVel, double AngularVel) = DockToController.FinalApproachController(Localization.X, Localization.Y, Localization.Theta, linearVelActual, maxLinearVel);
+
+                // Clamp angular velocity
+                AngularVel = Math.Clamp(AngularVel, -(NavCog?.MaxAngularVelocity ?? 1.5), NavCog?.MaxAngularVelocity ?? 1.5);
+
+                // Clamp linear velocity (giữ dấu cho backward)
+                var linearSign = Math.Sign(LinearVel);
+                LinearVel = linearSign * Math.Clamp(Math.Abs(LinearVel), NavCog?.MinLinearVelocity ?? 0.01, MaxLinearVelocity);
+
+                VelController.SetVelocity(LinearVel, AngularVel);
+            }
+            else
+            {
+                Console.WriteLine($"Dock To reached. Pose= ({Localization.X} - {Localization.Y} - {Localization.Theta}), Distance to goal: {DistanceToGoal}");
+                VelController.SetVelocity(0, 0);
+                if (!_hasLoad && DockToController.Goal.Theta is { } theta)
+                {
+                    TargetAngle = theta;
+                    NavState = NavigationState.Rotating;
+                    RotatePID?.Reset();
+                    DockToController = null;
+                }
+                else
+                {
+                    NavState = NavigationState.Completed;
+                    Logger.LogInformation($"DockTo Reached: Pose({Localization.X} - {Localization.Y} - {Localization.Theta})");
+                    Dispose();
+                }
+            }
+        }
+    }
+
+    private void MovingStraight()
+    {
+        if (MovePID is not null && MoveStraightController is not null
+            && MoveStraightController.Waypoints_Value is not null && MoveStraightController.Goal != null)
+        {
+            var cfg = MoveStraightController.DockConfig;
+            var DistanceToGoal = Math.Sqrt(
+                Math.Pow(Localization.X - MoveStraightController.Goal.X, 2) +
+                Math.Pow(Localization.Y - MoveStraightController.Goal.Y, 2));
+            var deviation = cfg.ReachedRadius;
+
+            // Overshoot detection (same pattern as Moving)
+            var overshootRadius = cfg.DockingOvershootDetectionRadius;
+            if (DistanceToGoal < overshootRadius)
+            {
+                if (DistanceToGoal < _oldDistanceToGoal)
+                {
+                    _wasApproaching = true;
+                    _overshootCounter = 0;
+                }
+                else if (_wasApproaching && DistanceToGoal > _oldDistanceToGoal)
+                {
+                    _overshootCounter++;
+                    if (_wasApproaching && _overshootCounter >= _overshootOut)
+                    {
+                        _wasApproaching = false;
+                        VelController.SetVelocity(0, 0);
+
+                        double acceptanceRadius = NavCog?.OvershootAcceptanceRadius ?? 0.15;
+                        if (DistanceToGoal > acceptanceRadius)
+                        {
+                            Logger.LogError($"MovingStraight overshoot too far: distance={DistanceToGoal:F4}m > acceptance={acceptanceRadius}m");
+                            NavState = NavigationState.Error;
+                            Dispose();
+                            return;
+                        }
+
+                        Logger.LogWarning($"MovingStraight overshoot at {DistanceToGoal:F4}m (within acceptance). Completing.");
+                        NavState = NavigationState.Completed;
+                        Dispose();
+                        return;
+                    }
+                }
+                else _overshootCounter = 0;
+            }
+            _oldDistanceToGoal = DistanceToGoal;
+
+            if (DistanceToGoal > deviation)
+            {
+                double dt = CycleHandlerMilliseconds / 1000.0;
+                double decelerationDist = cfg.DecelerationDistance;
+                double maxLinearVel = DistanceToGoal > decelerationDist
+                    ? MaxLinearVelocity
+                    : MovePID.PID_step(DistanceToGoal, MaxLinearVelocity, NavCog?.MinLinearVelocity ?? 0.01, dt);
+                maxLinearVel = Math.Clamp(maxLinearVel, NavCog?.MinLinearVelocity ?? 0.01, MaxLinearVelocity);
+
+                (double linearVelActual, _) = VelController.ActualVelocity;
+                (double LinearVel, double AngularVel) = MoveStraightController.FinalApproachController(
+                    Localization.X, Localization.Y, Localization.Theta, linearVelActual, maxLinearVel);
+
+                AngularVel = Math.Clamp(AngularVel, -(NavCog?.MaxAngularVelocity ?? 1.5), NavCog?.MaxAngularVelocity ?? 1.5);
+
+                // Clamp linear velocity (giữ dấu cho backward)
+                var linearSign = Math.Sign(LinearVel);
+                LinearVel = linearSign * Math.Clamp(Math.Abs(LinearVel), NavCog?.MinLinearVelocity ?? 0.01, MaxLinearVelocity);
+
+                VelController.SetVelocity(LinearVel, AngularVel);
+            }
+            else
+            {
+                Logger.LogInformation($"MoveStraight reached. Pose=({Localization.X} - {Localization.Y} - {Localization.Theta}), Distance: {DistanceToGoal}");
+                VelController.SetVelocity(0, 0);
+                NavState = NavigationState.Completed;
+                Dispose();
+            }
+        }
+    }
+
+    /// <summary>
+    /// FinePositioning: Cơ chế retry chính xác khi Docking overshoot.
+    /// Phase 1 (Align): Xoay tại chỗ hướng về goal theo góc nhỏ nhất (forward hoặc backward).
+    ///   Hướng được lock 1 lần khi vào align, không thay đổi trong suốt quá trình xoay.
+    /// Phase 2 (Advance): Tiến thẳng về goal ở DockToRetrySpeed với P-correction trên angular velocity.
+    /// Success: distance ≤ ReachedRadius → final rotation hoặc Completed.
+    /// Failure: hết retry hoặc timeout → Error.
+    /// </summary>
+    private void FinePositioning()
+    {
+        if (DockToController?.Goal is not { } goal)
+        {
+            Logger.LogError("FinePositioning: DockToController or Goal is null. Aborting.");
+            NavState = NavigationState.Error;
+            Dispose();
+            return;
+        }
+
+        // Continuously update goal from dock session (same as Docking phase)
+        var updatedGoal = TryUpdateDockGoalFromSession(NavCog.DockToRetrySpeed);
+        if (updatedGoal is not null) goal = updatedGoal;
+
+        // Per-attempt time-based timeout
+        _finePositioningCycleCount++;
+        int elapsedMs = _finePositioningCycleCount * CycleHandlerMilliseconds;
+        if (elapsedMs > _finePositioningTimeoutMs)
+        {
+            _finePositioningRetryCount++;
+            Logger.LogWarning(
+                $"FinePositioning attempt {_finePositioningRetryCount} timed out " +
+                $"(elapsed {elapsedMs}ms > {_finePositioningTimeoutMs}ms).");
+
+            if (_finePositioningRetryCount >= _finePositioningMaxRetries)
+            {
+                Logger.LogError(
+                    $"FinePositioning exhausted all {_finePositioningMaxRetries} retries. Transitioning to Error.");
+                VelController.SetVelocity(0, 0);
+                NavState = NavigationState.Error;
+                Dispose();
+            }
+            else
+            {
+                VelController.SetVelocity(0, 0);
+                _finePositioningCycleCount = 0;
+                _finePositioningIsAligning = true;
+                _finePositionRotatePID?.Reset();
+                _oldDistanceToGoal = double.MaxValue;
+                _fpWasApproaching = false;
+                _fpOvershootCounter = 0;
+            }
+            return;
+        }
+
+        double robotX = Localization.X;
+        double robotY = Localization.Y;
+        double robotTheta = Localization.Theta;
+
+        double dx = goal.X - robotX;
+        double dy = goal.Y - robotY;
+        double distanceToGoal = Math.Sqrt(dx * dx + dy * dy);
+
+        // Success check
+        double reachedRadius = DockToController.DockConfig.ReachedRadius;
+        if (distanceToGoal <= reachedRadius)
+        {
+            Logger.LogInformation(
+                $"FinePositioning SUCCESS: distance={distanceToGoal:F4}m, " +
+                $"Pose({robotX:F3}, {robotY:F3}, {robotTheta:F3}) → Goal({goal.X:F3}, {goal.Y:F3}, {goal.Theta:F3}, {_dockSession?.Goal?.Header.FrameId}, {_dockSession?.Goal?.Header.Stamp}). ");
+            VelController.SetVelocity(0, 0);
+
+            if (goal.Theta is { } finalTheta)
+            {
+                TargetAngle = finalTheta;
+                NavState = NavigationState.Rotating;
+                RotatePID = new PID(NavigationConfig.GetRotatePidConfig());
+                DockToController = null;
+            }
+            else
+            {
+                NavState = NavigationState.Completed;
+                DockToController = null;
+                Dispose();
+            }
+            return;
+        }
+
+        // Chọn hướng tiếp cận có góc xoay nhỏ nhất (forward hoặc backward)
+        // Direction chỉ được chọn 1 lần khi vào align phase (cycle đầu tiên),
+        // sau đó giữ nguyên suốt align + advance để tránh flip do sensor noise.
+        double rawHeading = Math.Atan2(dy, dx);
+
+        if (_finePositioningIsAligning && _finePositioningCycleCount == 1)
+        {
+            double fwdErr = SpaceCompute.NormalizeRadianAngle(rawHeading) - robotTheta;
+            if (fwdErr > Math.PI) fwdErr -= 2 * Math.PI;
+            else if (fwdErr < -Math.PI) fwdErr += 2 * Math.PI;
+
+            double bwdErr = SpaceCompute.NormalizeRadianAngle(rawHeading + Math.PI) - robotTheta;
+            if (bwdErr > Math.PI) bwdErr -= 2 * Math.PI;
+            else if (bwdErr < -Math.PI) bwdErr += 2 * Math.PI;
+
+            _finePositioningDirection = Math.Abs(fwdErr) <= Math.Abs(bwdErr)
+                ? RobotDirection.FORWARD
+                : RobotDirection.BACKWARD;
+        }
+
+        // Tính heading error theo hướng đã lock
+        double targetHeading = _finePositioningDirection == RobotDirection.BACKWARD
+            ? SpaceCompute.NormalizeRadianAngle(rawHeading + Math.PI)
+            : SpaceCompute.NormalizeRadianAngle(rawHeading);
+
+        double headingError = targetHeading - robotTheta;
+        if (headingError > Math.PI) headingError -= 2 * Math.PI;
+        else if (headingError < -Math.PI) headingError += 2 * Math.PI;
+
+        double FineAlignThresholdRad = (DockToController.DockConfig.FineAlignThresholdDegrees) * Math.PI / 180.0;
+        double ReAlignThresholdRad = (DockToController.DockConfig.ReAlignThresholdDegrees) * Math.PI / 180.0;
+
+        // Overshoot detection during advance phase — consecutive increase pattern
+        if (!_finePositioningIsAligning)
+        {
+            if (distanceToGoal < _oldDistanceToGoal)
+            {
+                _fpWasApproaching = true;
+                _fpOvershootCounter = 0;
+            }
+            else if (_fpWasApproaching && distanceToGoal > _oldDistanceToGoal)
+            {
+                _fpOvershootCounter++;
+                if (_fpOvershootCounter >= DockToController.DockConfig.FinePositioningOvershootCount)
+                {
+                    _finePositioningRetryCount++;
+                    _fpWasApproaching = false;
+                    _fpOvershootCounter = 0;
+                    VelController.SetVelocity(0, 0);
+
+                    if (_finePositioningRetryCount >= _finePositioningMaxRetries)
+                    {
+                        Logger.LogError("FinePositioning: repeated overshoot during advance. Giving up.");
+                        NavState = NavigationState.Error;
+                        Dispose();
+                        return;
+                    }
+
+                    Logger.LogWarning(
+                        $"FinePositioning: overshoot during advance (dist={distanceToGoal:F4}m). " +
+                        $"Retry {_finePositioningRetryCount}/{_finePositioningMaxRetries}.");
+                    _finePositioningIsAligning = true;
+                    _finePositionRotatePID?.Reset();
+                    _finePositioningCycleCount = 0;
+                    _oldDistanceToGoal = double.MaxValue;
+                    return;
+                }
+            }
+            else
+            {
+                _fpOvershootCounter = 0;
+            }
+        }
+        _oldDistanceToGoal = distanceToGoal;
+
+        // Phase 1: ALIGN — xoay tại chỗ theo góc nhỏ nhất
+        if (_finePositioningIsAligning)
+        {
+            if (Math.Abs(headingError) < FineAlignThresholdRad)
+            {
+                _finePositioningIsAligning = false;
+                VelController.SetVelocity(0, 0);
+                Logger.LogInformation(
+                    $"FinePositioning aligned ({_finePositioningDirection}): " +
+                    $"headingError={headingError * 180 / Math.PI:F1}°. Advancing.");
+            }
+            else
+            {
+                _finePositionRotatePID ??= new PID(NavigationConfig.GetRotatePidConfig());
+                double rotateAngularVel = NavCog?.DockToRotateSpeed ?? 0.05;
+                double angularCmd = _finePositionRotatePID.PID_step(
+                    headingError, rotateAngularVel, -rotateAngularVel, CycleHandlerMilliseconds / 1000.0);
+                VelController.SetVelocity(0, angularCmd);
+                Console.WriteLine(
+                    $"FinePos-Align({_finePositioningDirection}): " +
+                    $"HeadErr={headingError * 180 / Math.PI:F1}°, AngVel={angularCmd:F4}, Dist={distanceToGoal:F4}m");
+            }
+            return;
+        }
+
+        // Phase 2: ADVANCE — tiến thẳng về goal theo hướng đã chọn ở Phase 1
+        if (Math.Abs(headingError) > ReAlignThresholdRad)
+        {
+            _finePositioningIsAligning = true;
+            _finePositionRotatePID?.Reset();
+            VelController.SetVelocity(0, 0);
+            Logger.LogWarning($"FinePositioning heading drift: {headingError * 180 / Math.PI:F1}°. Re-aligning.");
+            return;
+        }
+
+        double minLinVel = NavCog?.DockToRetrySpeed ?? 0.05;
+        double linearCmd = _finePositioningDirection == RobotDirection.BACKWARD ? -minLinVel : minLinVel;
+
+        // Corrective angular velocity proportional to heading error
+        // Dùng Max(|actualVel|, minLinVel) để đảm bảo correction không bằng 0 khi vừa bắt đầu advance
+        (double linearVelActual, _) = VelController.ActualVelocity;
+        double effectiveVel = Math.Max(Math.Abs(linearVelActual), minLinVel);
+        double headingGain = DockToController.DockConfig.AdvanceHeadingCorrectionGain;
+        double dockAdvanceMaxAngVel = DockToController.DockConfig.DockToAdvanceMaxAngularVelocity;
+        double advanceAngularCorrection = Math.Clamp(headingError * headingGain * effectiveVel, -dockAdvanceMaxAngVel, dockAdvanceMaxAngVel);
+
+        VelController.SetVelocity(linearCmd, advanceAngularCorrection);
+        Console.WriteLine(
+            $"FinePos-Advance({_finePositioningDirection}): LinVel={linearCmd:F4}, AngCorr={advanceAngularCorrection:F4}, " +
+            $"Dist={distanceToGoal:F4}m, HeadErr={headingError * 180 / Math.PI:F1}°");
+    }
+
+    private NavigationNode? TryUpdateDockGoalFromSession(double speed)
+    {
+        if (_dockSession is null || DockToController?.DockConfig is not { } dockCfg)
+            return null;
+
+        var snapshot = _dockSession.Goal;
+        if (!snapshot.HasValue) return null;
+
+        var pose = snapshot.Value.Pose;
+
+        var oldGoal = DockToController!.Goal;
+        double dxGoal = pose.Position.X - oldGoal.X;
+        double dyGoal = pose.Position.Y - oldGoal.Y;
+        double distShift = Math.Sqrt(dxGoal * dxGoal + dyGoal * dyGoal);
+
+        double newTheta = pose.Orientation.ToYawRadian();
+        double oldTheta = oldGoal.Theta ?? 0;
+        double angleShift = Math.Abs(SpaceCompute.NormalizeRadianAngle(newTheta - oldTheta));
+        double maxAngleShiftRad = dockCfg.MaxGoalAngleShiftDegrees * Math.PI / 180.0;
+
+        if (distShift > dockCfg.MaxGoalPositionShift || angleShift > maxAngleShiftRad)
+            return null;
+
+        var updatedGoal = new NavigationNode()
+        {
+            Id = Guid.NewGuid(),
+            X = pose.Position.X,
+            Y = pose.Position.Y,
+            Speed = speed,
+            Theta = newTheta,
+        };
+
+        var newStartNode = GetDockToStartNode(
+            Localization.X, Localization.Y,
+            updatedGoal.X, updatedGoal.Y,
+            updatedGoal.Theta ?? 0,
+            dockCfg.DockToLength);
+
+        DockToController.WithPath(newStartNode, updatedGoal);
+        return updatedGoal;
+    }
+
+    private void NavigationHandler()
+    {
+        try
+        {
+            switch (NavState)
+            {
+                case NavigationState.Rotating:
+                    Rotating();
+                    break;
+                case NavigationState.Moving:
+                    Moving();
+                    break;
+                case NavigationState.Docking:
+                    Docking();
+                    break;
+                case NavigationState.MovingStraight:
+                    MovingStraight();
+                    break;
+                case NavigationState.FinePositioning:
+                    FinePositioning();
+                    break;
+                case NavigationState.Paused:
+                case NavigationState.SafetyStop:
+                    if (!_idleVelocityZeroSent)
+                    {
+                        VelController.SetVelocity(0, 0);
+                        _idleVelocityZeroSent = true;
+                    }
+                    break;
+                default:
+                    break;
+            }
+        }
+        catch (Exception ex)
+        {
+            NavState = NavigationState.Error;
+            Dispose();
+            Logger.LogError($"Error in DifferentialNavigation: {ex.Message}");
+        }
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Navigation/IVelocityController.cs

@@ -0,0 +1,13 @@
+namespace RobotNet10.RobotApp.Services.Navigation;
+
+public interface IVelocityController
+{
+    (double Linear, double Angular) ActualVelocity { get; }
+    (double Linear, double Angular) RawVelocity { get; }
+    void SetVelocity(double linearVel, double angularVel);
+    void LoadConfig();
+    double GetModelConfidence();
+    void SetAcceleration(double acc);
+    void SetDeceleration(double dec);
+    bool EnsureInverseKinematicsReady(CancellationToken cancellationToken);
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/NavigationMonitor/NavigationMonitorService.cs

@@ -0,0 +1,428 @@
+using Microsoft.AspNetCore.SignalR;
+using Microsoft.Extensions.Logging;
+using RobotNet10.Common;
+using RobotNet10.RobotApp.Hubs;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Services.Navigation;
+using RobotNet10.RobotApp.Services.Robot.Modules;
+using RobotNet10.RobotApp.Services.Simulation;
+using RobotNet10.RobotApp.Shared.Enums;
+using RobotNet10.RobotApp.Shared.NavigationMonitor;
+
+namespace RobotNet10.RobotApp.Services.NavigationMonitor;
+
+/// <summary>
+/// Independent monitoring service for CSharpNavigation.
+/// Periodically reads robot state and broadcasts telemetry via SignalR.
+/// Can be enabled/disabled at runtime without affecting navigation.
+/// </summary>
+public class NavigationMonitorService : IHostedService, IDisposable
+{
+    private const int DefaultIntervalMs = 100; // 10Hz internal tick (safety checks)
+    private const int BroadcastEveryNTicks = 5; // Broadcast telemetry every 5 ticks = 2Hz
+
+    private readonly ILocalization _localization;
+    private readonly IVelocityController _velocityController;
+    private readonly RobotNavigation _navigation;
+    private readonly IHubContext<NavigationMonitorHub> _hubContext;
+    private readonly ILogger<NavigationMonitorService> _logger;
+
+    private WatchTimer<NavigationMonitorService>? _timer;
+    private volatile bool _telemetryEnabled;
+    private volatile bool _safetyStopEnabled;
+    private NavigationSafetyConfigDto _safetyConfig = new();
+    private readonly Lock _configLock = new();
+
+    // Safety stop latch state
+    private volatile bool _safetyStopLatched;
+    private string _safetyStopReason = "";
+    private int _safetyStopGraceTicks;
+    private const int SafetyStopGraceCount = 20; // 2s at 10Hz
+
+    // State for acceleration calculation
+    private double _lastLinearVel;
+    private double _lastAngularVel;
+    private long _lastTimestampMs;
+
+    // Broadcast throttle counter
+    private int _broadcastTickCounter;
+
+    public NavigationMonitorService(
+        ILocalization localization,
+        IVelocityController velocityController,
+        RobotNavigation navigation,
+        IHubContext<NavigationMonitorHub> hubContext,
+        ILogger<NavigationMonitorService> logger)
+    {
+        _localization = localization;
+        _velocityController = velocityController;
+        _navigation = navigation;
+        _hubContext = hubContext;
+        _logger = logger;
+    }
+
+    public Task StartAsync(CancellationToken cancellationToken)
+    {
+        _logger.LogInformation("NavigationMonitorService started (telemetry disabled by default)");
+        return Task.CompletedTask;
+    }
+
+    public Task StopAsync(CancellationToken cancellationToken)
+    {
+        StopTimer();
+        _logger.LogInformation("NavigationMonitorService stopped");
+        return Task.CompletedTask;
+    }
+
+    public void SetTelemetryEnabled(bool enabled)
+    {
+        _telemetryEnabled = enabled;
+        if (enabled)
+            StartTimer();
+        else
+            StopTimer();
+
+        _logger.LogInformation("Navigation telemetry {State}", enabled ? "enabled" : "disabled");
+        BroadcastStateAsync();
+    }
+
+    public void SetSafetyStopEnabled(bool enabled)
+    {
+        _safetyStopEnabled = enabled;
+        _logger.LogInformation("Navigation safety stop {State}", enabled ? "enabled" : "disabled");
+        BroadcastStateAsync();
+    }
+
+    public void UpdateSafetyConfig(NavigationSafetyConfigDto config)
+    {
+        lock (_configLock)
+        {
+            _safetyConfig = config;
+        }
+        _logger.LogInformation("Navigation safety config updated");
+        BroadcastStateAsync();
+    }
+
+    public NavigationMonitorStateDto GetState()
+    {
+        NavigationSafetyConfigDto configCopy;
+        lock (_configLock)
+        {
+            configCopy = new NavigationSafetyConfigDto
+            {
+                MaxLinearVelocity = _safetyConfig.MaxLinearVelocity,
+                MaxAngularVelocity = _safetyConfig.MaxAngularVelocity,
+                MaxLinearAcceleration = _safetyConfig.MaxLinearAcceleration,
+                MaxCrossTrackError = _safetyConfig.MaxCrossTrackError,
+                MaxHeadingError = _safetyConfig.MaxHeadingError
+            };
+        }
+
+        return new NavigationMonitorStateDto
+        {
+            TelemetryEnabled = _telemetryEnabled,
+            SafetyStopEnabled = _safetyStopEnabled,
+            SafetyStopLatched = _safetyStopLatched,
+            SafetyStopReason = _safetyStopReason,
+            SafetyConfig = configCopy,
+            UpdateFrequencyHz = 1000.0 / (DefaultIntervalMs * BroadcastEveryNTicks)
+        };
+    }
+
+    public void ReleaseSafetyStop()
+    {
+        if (!_safetyStopLatched) return;
+        _safetyStopLatched = false;
+        _safetyStopReason = "";
+        _safetyStopGraceTicks = SafetyStopGraceCount;
+        _navigation.Refresh();
+        _logger.LogInformation("Safety stop released by user");
+        BroadcastStateAsync();
+    }
+
+    private void StartTimer()
+    {
+        StopTimer();
+        _lastTimestampMs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
+        _lastLinearVel = 0;
+        _lastAngularVel = 0;
+        _broadcastTickCounter = 0;
+        _timer = new WatchTimer<NavigationMonitorService>(DefaultIntervalMs, OnTick, _logger);
+        _timer.Start();
+    }
+
+    private void StopTimer()
+    {
+        _timer?.Dispose();
+        _timer = null;
+    }
+
+    private void OnTick()
+    {
+        try
+        {
+            var now = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
+            var dtSeconds = (now - _lastTimestampMs) / 1000.0;
+            if (dtSeconds <= 0) dtSeconds = DefaultIntervalMs / 1000.0;
+
+            // Read robot state
+            var x = _localization.X;
+            var y = _localization.Y;
+            var theta = _localization.Theta;
+            var (linearVel, angularVel) = _velocityController.ActualVelocity;
+            var modelConfidence = _velocityController.GetModelConfidence();
+            var navState = _navigation.State;
+            var driving = _navigation.Driving;
+
+            // Calculate acceleration
+            var linearAccel = (linearVel - _lastLinearVel) / dtSeconds;
+            var angularAccel = (angularVel - _lastAngularVel) / dtSeconds;
+            _lastLinearVel = linearVel;
+            _lastAngularVel = angularVel;
+            _lastTimestampMs = now;
+
+            // Calculate CTE and heading error if navigating with path
+            double cte = 0;
+            double headingError = 0;
+            double distanceToGoal = 0;
+
+            var path = _navigation.CurrentPath; // capture once (volatile)
+            if (path is { Count: >= 2 } && driving)
+            {
+                // Skip heading error check during Rotating/FinePositioning:
+                // - Rotating: robot intentionally turning in place, heading diverges from path tangent
+                // - FinePositioning: robot re-aligning to dock goal, heading changes rapidly
+                bool skipHeadingCheck = navState is NavigationState.Rotating or NavigationState.FinePositioning;
+                (cte, headingError) = CalculatePathErrors(x, y, theta, path, skipHeadingCheck);
+                var goal = path[^1];
+                distanceToGoal = Math.Sqrt((x - goal.X) * (x - goal.X) + (y - goal.Y) * (y - goal.Y));
+            }
+
+            // DockTo-specific telemetry
+            DockToTelemetryDto? dockToTelemetry = null;
+            if (_navigation.IsDockingActive)
+            {
+                var dockGoal = _navigation.DockGoal;
+                var dockStart = _navigation.DockStartNode;
+                dockToTelemetry = new DockToTelemetryDto
+                {
+                    Phase = _navigation.DockPhase,
+                    Direction = _navigation.DockDirection,
+                    RetryCount = _navigation.DockRetryCount,
+                    MaxRetries = _navigation.DockMaxRetries,
+                    GoalX = dockGoal?.X ?? 0,
+                    GoalY = dockGoal?.Y ?? 0,
+                    GoalTheta = dockGoal?.Theta ?? 0,
+                    TotalWaypoints = _navigation.DockWaypointCount,
+                    StartX = dockStart?.X ?? 0,
+                    StartY = dockStart?.Y ?? 0,
+                    StartTheta = dockStart?.Theta ?? 0,
+                    Waypoints = DownsampleWaypoints(_navigation.DockWaypoints, 30)
+                };
+            }
+
+            // Downsample current path waypoints for client visualization
+            var waypoints = path is { Count: >= 2 } && driving
+                ? DownsampleNavigationWaypoints(path, 50)
+                : [];
+
+            var telemetry = new NavigationTelemetryDto
+            {
+                TimestampMs = now,
+                X = x,
+                Y = y,
+                Theta = theta,
+                LinearVelocity = linearVel,
+                AngularVelocity = angularVel,
+                LinearAcceleration = double.IsFinite(linearAccel) ? linearAccel : 0,
+                AngularAcceleration = double.IsFinite(angularAccel) ? angularAccel : 0,
+                NavigationState = navState.ToString(),
+                Driving = driving,
+                CrossTrackError = cte,
+                HeadingError = headingError,
+                DistanceToGoal = distanceToGoal,
+                ModelConfidence = modelConfidence,
+                DockTo = dockToTelemetry,
+                Waypoints = waypoints
+            };
+
+            // Broadcast telemetry at reduced rate (2Hz) to save client resources
+            _broadcastTickCounter++;
+            if (_broadcastTickCounter >= BroadcastEveryNTicks)
+            {
+                _broadcastTickCounter = 0;
+                _ = _hubContext.Clients.Group("monitor").SendAsync("ReceiveTelemetry", telemetry);
+            }
+
+            // Auto-release latch if navigation ended while latched
+            if (_safetyStopLatched && !driving)
+            {
+                _safetyStopLatched = false;
+                _safetyStopReason = "";
+                _navigation.Refresh();
+                _logger.LogInformation("Safety stop auto-released: navigation ended");
+                BroadcastStateAsync();
+            }
+
+            // Grace period countdown after user release
+            if (_safetyStopGraceTicks > 0) _safetyStopGraceTicks--;
+
+            // Safety checks
+            NavigationSafetyConfigDto configSnapshot;
+            lock (_configLock)
+            {
+                configSnapshot = new NavigationSafetyConfigDto
+                {
+                    MaxLinearVelocity = _safetyConfig.MaxLinearVelocity,
+                    MaxAngularVelocity = _safetyConfig.MaxAngularVelocity,
+                    MaxLinearAcceleration = _safetyConfig.MaxLinearAcceleration,
+                    MaxCrossTrackError = _safetyConfig.MaxCrossTrackError,
+                    MaxHeadingError = _safetyConfig.MaxHeadingError
+                };
+            }
+
+            var violations = NavigationSafetyChecker.Check(telemetry, configSnapshot);
+            if (violations.Count > 0)
+            {
+                foreach (var v in violations)
+                {
+                    _ = _hubContext.Clients.Group("monitor").SendAsync("ReceiveSafetyViolation", v);
+                }
+
+                // Latch safety stop on critical violations: Pause navigation + hold
+                if (_safetyStopEnabled && !_safetyStopLatched
+                    && _safetyStopGraceTicks == 0
+                    && violations.Exists(v => v.Severity == SafetyViolationSeverity.Critical))
+                {
+                    _safetyStopLatched = true;
+                    _safetyStopReason = violations.First(v => v.Severity == SafetyViolationSeverity.Critical).Message;
+                    _navigation.SafetyStop();
+                    _logger.LogWarning("Safety stop latched: {Reason}", _safetyStopReason);
+                    BroadcastStateAsync();
+                }
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "Error in NavigationMonitor tick");
+        }
+    }
+
+    /// <summary>
+    /// Calculate cross-track error and heading error from current position to closest path point.
+    /// </summary>
+    private static (double cte, double headingError) CalculatePathErrors(
+        double x, double y, double theta,
+        IReadOnlyList<NavigationNode> path,
+        bool skipHeadingCheck = false)
+    {
+        // Find closest point on path
+        int closestIndex = 0;
+        double minDist = double.MaxValue;
+
+        for (int i = 0; i < path.Count; i++)
+        {
+            var dx = x - path[i].X;
+            var dy = y - path[i].Y;
+            var dist = dx * dx + dy * dy;
+            if (dist < minDist)
+            {
+                minDist = dist;
+                closestIndex = i;
+            }
+        }
+
+        double cte = Math.Sqrt(minDist);
+
+        if (skipHeadingCheck)
+            return (cte, 0);
+
+        // Calculate heading error using path tangent, accounting for Direction
+        double headingError = 0;
+        double refTheta;
+        if (closestIndex < path.Count - 1)
+        {
+            var dx = path[closestIndex + 1].X - path[closestIndex].X;
+            var dy = path[closestIndex + 1].Y - path[closestIndex].Y;
+            refTheta = Math.Atan2(dy, dx);
+        }
+        else if (closestIndex > 0)
+        {
+            var dx = path[closestIndex].X - path[closestIndex - 1].X;
+            var dy = path[closestIndex].Y - path[closestIndex - 1].Y;
+            refTheta = Math.Atan2(dy, dx);
+        }
+        else
+        {
+            return (cte, 0);
+        }
+
+        // When path segment is BACKWARD, robot faces opposite to path tangent
+        if (path[closestIndex].Direction == RobotDirection.BACKWARD)
+            refTheta = NormalizeAngle(refTheta + Math.PI);
+
+        headingError = NormalizeAngle(theta - refTheta);
+
+        return (cte, headingError);
+    }
+
+    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 static List<DockWaypointDto> DownsampleWaypoints(IReadOnlyList<NavigationNode>? waypoints, int maxPoints)
+    {
+        if (waypoints is null or { Count: 0 }) return [];
+        if (waypoints.Count <= maxPoints)
+            return waypoints.Select(w => new DockWaypointDto { X = w.X, Y = w.Y }).ToList();
+
+        var result = new List<DockWaypointDto>(maxPoints);
+        result.Add(new DockWaypointDto { X = waypoints[0].X, Y = waypoints[0].Y });
+
+        double step = (double)(waypoints.Count - 1) / (maxPoints - 1);
+        for (int i = 1; i < maxPoints - 1; i++)
+        {
+            int idx = (int)Math.Round(i * step);
+            result.Add(new DockWaypointDto { X = waypoints[idx].X, Y = waypoints[idx].Y });
+        }
+
+        var last = waypoints[^1];
+        result.Add(new DockWaypointDto { X = last.X, Y = last.Y });
+        return result;
+    }
+
+    private static List<WaypointDto> DownsampleNavigationWaypoints(IReadOnlyList<NavigationNode> waypoints, int maxPoints)
+    {
+        if (waypoints.Count <= maxPoints)
+            return waypoints.Select(w => new WaypointDto { X = w.X, Y = w.Y, Direction = w.Direction.ToString() }).ToList();
+
+        var result = new List<WaypointDto>(maxPoints);
+        result.Add(new WaypointDto { X = waypoints[0].X, Y = waypoints[0].Y, Direction = waypoints[0].Direction.ToString() });
+
+        double step = (double)(waypoints.Count - 1) / (maxPoints - 1);
+        for (int i = 1; i < maxPoints - 1; i++)
+        {
+            int idx = (int)Math.Round(i * step);
+            result.Add(new WaypointDto { X = waypoints[idx].X, Y = waypoints[idx].Y, Direction = waypoints[idx].Direction.ToString() });
+        }
+
+        var last = waypoints[^1];
+        result.Add(new WaypointDto { X = last.X, Y = last.Y, Direction = last.Direction.ToString() });
+        return result;
+    }
+
+    private void BroadcastStateAsync()
+    {
+        var state = GetState();
+        _ = _hubContext.Clients.Group("monitor").SendAsync("ReceiveMonitorState", state);
+    }
+
+    public void Dispose()
+    {
+        StopTimer();
+        GC.SuppressFinalize(this);
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/NavigationMonitor/NavigationSafetyChecker.cs

@@ -0,0 +1,91 @@
+using RobotNet10.RobotApp.Shared.NavigationMonitor;
+
+namespace RobotNet10.RobotApp.Services.NavigationMonitor;
+
+/// <summary>
+/// Stateless safety checker: evaluates telemetry against configurable thresholds.
+/// </summary>
+public static class NavigationSafetyChecker
+{
+    public static List<NavigationSafetyViolationDto> Check(
+        NavigationTelemetryDto telemetry,
+        NavigationSafetyConfigDto config)
+    {
+        var violations = new List<NavigationSafetyViolationDto>();
+        var now = telemetry.TimestampMs;
+
+        // 1. Linear velocity
+        if (Math.Abs(telemetry.LinearVelocity) > config.MaxLinearVelocity)
+        {
+            violations.Add(new NavigationSafetyViolationDto
+            {
+                TimestampMs = now,
+                Type = SafetyViolationType.LinearVelocity,
+                Severity = SafetyViolationSeverity.Warning,
+                Value = Math.Abs(telemetry.LinearVelocity),
+                Threshold = config.MaxLinearVelocity,
+                Message = $"Linear velocity {Math.Abs(telemetry.LinearVelocity):F2} m/s exceeds limit {config.MaxLinearVelocity:F2} m/s"
+            });
+        }
+
+        // 2. Angular velocity
+        if (Math.Abs(telemetry.AngularVelocity) > config.MaxAngularVelocity)
+        {
+            violations.Add(new NavigationSafetyViolationDto
+            {
+                TimestampMs = now,
+                Type = SafetyViolationType.AngularVelocity,
+                Severity = SafetyViolationSeverity.Warning,
+                Value = Math.Abs(telemetry.AngularVelocity),
+                Threshold = config.MaxAngularVelocity,
+                Message = $"Angular velocity {Math.Abs(telemetry.AngularVelocity):F2} rad/s exceeds limit {config.MaxAngularVelocity:F2} rad/s"
+            });
+        }
+
+        // 3. Linear acceleration
+        if (Math.Abs(telemetry.LinearAcceleration) > config.MaxLinearAcceleration)
+        {
+            violations.Add(new NavigationSafetyViolationDto
+            {
+                TimestampMs = now,
+                Type = SafetyViolationType.LinearAcceleration,
+                Severity = SafetyViolationSeverity.Warning,
+                Value = Math.Abs(telemetry.LinearAcceleration),
+                Threshold = config.MaxLinearAcceleration,
+                Message = $"Linear acceleration {Math.Abs(telemetry.LinearAcceleration):F2} m/s² exceeds limit {config.MaxLinearAcceleration:F2} m/s²"
+            });
+        }
+
+        // 4. Cross-track error (only when navigating and path is available)
+        double maxCteRadians = config.MaxCrossTrackError;
+        if (telemetry.Driving && telemetry.CrossTrackError > maxCteRadians)
+        {
+            violations.Add(new NavigationSafetyViolationDto
+            {
+                TimestampMs = now,
+                Type = SafetyViolationType.CrossTrackError,
+                Severity = SafetyViolationSeverity.Critical,
+                Value = telemetry.CrossTrackError,
+                Threshold = config.MaxCrossTrackError,
+                Message = $"CTE {telemetry.CrossTrackError:F3} m exceeds limit {config.MaxCrossTrackError:F3} m"
+            });
+        }
+
+        // 5. Heading error (only when navigating and path is available)
+        double maxHeadingRadians = config.MaxHeadingError * Math.PI / 180.0;
+        if (telemetry.Driving && Math.Abs(telemetry.HeadingError) > maxHeadingRadians)
+        {
+            violations.Add(new NavigationSafetyViolationDto
+            {
+                TimestampMs = now,
+                Type = SafetyViolationType.HeadingError,
+                Severity = SafetyViolationSeverity.Critical,
+                Value = Math.Abs(telemetry.HeadingError) * 180.0 / Math.PI,
+                Threshold = config.MaxHeadingError,
+                Message = $"Heading error {Math.Abs(telemetry.HeadingError) * 180.0 / Math.PI:F1}° exceeds limit {config.MaxHeadingError:F1}°"
+            });
+        }
+
+        return violations;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/OdometryBroadcastService.cs

@@ -0,0 +1,54 @@
+using RobotNet10.RobotApp.Hubs;
+
+namespace RobotNet10.RobotApp.Services;
+
+/// <summary>
+/// Broadcast odometry tới SignalR clients định kỳ (mặc định 10 Hz) để hiển thị realtime
+/// </summary>
+public class OdometryBroadcastService : IHostedService, IDisposable
+{
+    private readonly OdometryHubContext _odometryHubContext;
+    private readonly ILogger<OdometryBroadcastService> _logger;
+    private Timer? _timer;
+    private const int BroadcastIntervalMs = 100; // 10 Hz
+
+    public OdometryBroadcastService(OdometryHubContext odometryHubContext, ILogger<OdometryBroadcastService> logger)
+    {
+        _odometryHubContext = odometryHubContext;
+        _logger = logger;
+    }
+
+    public Task StartAsync(CancellationToken cancellationToken)
+    {
+        _timer = new Timer(
+            async _ =>
+            {
+                try
+                {
+                    await _odometryHubContext.BroadcastOdometryAsync();
+                }
+                catch (Exception ex)
+                {
+                    _logger.LogDebug(ex, "Odometry broadcast skipped");
+                }
+            },
+            null,
+            TimeSpan.FromMilliseconds(BroadcastIntervalMs),
+            TimeSpan.FromMilliseconds(BroadcastIntervalMs));
+        _logger.LogInformation("Odometry broadcast started at {Hz} Hz", 1000.0 / BroadcastIntervalMs);
+        return Task.CompletedTask;
+    }
+
+    public Task StopAsync(CancellationToken cancellationToken)
+    {
+        _timer?.Change(Timeout.Infinite, 0);
+        _logger.LogInformation("Odometry broadcast stopped");
+        return Task.CompletedTask;
+    }
+
+    public void Dispose()
+    {
+        _timer?.Dispose();
+        GC.SuppressFinalize(this);
+    }
+}

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;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/ScriptEngineResource.cs

@@ -0,0 +1,30 @@
+using RobotNet10.RobotApp.Models;
+using RobotNet10.RobotApp.Script.Shared;
+using RobotNet10.ScriptEngine.Helpers;
+using RobotNet10.ScriptEngine.Shared;
+using System.Collections.Immutable;
+
+namespace RobotNet10.RobotApp.Services;
+
+public class ScriptEngineResource : IScriptEngineResource
+{
+    public Type AppGlobalType => RobotAppScriptEngineResource.GlobalType;
+
+    public ImmutableArray<string> UsingNamespaces => RobotAppScriptEngineResource.UsingNamespaces;
+
+    public ImmutableArray<string> Modules => RobotAppScriptEngineResource.Modules;
+
+    public ImmutableArray<string> DocModules => RobotAppScriptEngineResource.DocModules;
+
+    public IDictionary<string, object?> GetMissionGlobals(Guid id, CancellationToken cancellationToken)
+    {
+        var globals = new RobotAppScriptGlobals();
+        return ScriptHelper.ConvertGlobalsToDictionary(globals, typeof(IRobotAppScriptGlobals));
+    }
+
+    public IDictionary<string, object?> GetTaskGlobals()
+    {
+        var globals = new RobotAppScriptGlobals();
+        return ScriptHelper.ConvertGlobalsToDictionary(globals, typeof(IRobotAppScriptGlobals));
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Simulation/Algorithm/FuzzyLogic.cs

@@ -0,0 +1,538 @@
+namespace RobotNet10.RobotApp.Services.Simulation.Algorithm;
+
+public class FuzzyLogic
+{
+    // ============================================================================
+    // FUZZY LOGIC CONTROLLER - PHIÊN BẢN GỐC ĐÃ TỔ CHỨC LẠI
+    // Dải đầu ra: 0.0 - 1.0 | Độ phân giải: 5 mức | Số luật: 25 (5x5)
+    // ============================================================================
+
+    // ============================================================================
+    // PHẦN 1: CÁC HÀM MEMBERSHIP (HÀM THUỘC)
+    // ============================================================================
+
+    /// <summary>
+    /// Hàm thuộc hình thang (Trapezoidal Membership Function)
+    /// Dạng hình thang với 4 điểm: [left_base, left_top, right_top, right_base]
+    ///
+    ///        left_top ______ right_top
+    ///              /          \
+    ///             /            \
+    ///   _________/              \_________
+    /// left_base                  right_base
+    /// 
+    /// </summary>
+    /// <param name="inputValue">Giá trị đầu vào cần tính độ thuộc</param>
+    /// <param name="left_base">Điểm bắt đầu hình thang (μ = 0)</param>
+    /// <param name="left_top">Điểm bắt đầu vùng phẳng trên (μ = 1)</param>
+    /// <param name="right_top">Điểm kết thúc vùng phẳng trên (μ = 1)</param>
+    /// <param name="right_base">Điểm kết thúc hình thang (μ = 0)</param>
+    /// <returns>Độ thuộc trong khoảng [0, 1]</returns>
+    private static double Fuzzy_trapmf(double inputValue, double left_base, double left_top, double right_top, double right_base)
+    {
+        double membership = 0.0;
+
+        // Trường hợp 1: Nằm ngoài phía trái hình thang
+        if (inputValue <= left_base)
+        {
+            membership = 0.0;
+        }
+        // Trường hợp 2: Nằm trên cạnh tăng dần (trái)
+        else if (inputValue > left_base && inputValue < left_top)
+        {
+            if (left_top != left_base)  // Tránh chia cho 0
+            {
+                membership = (inputValue - left_base) / (left_top - left_base);
+            }
+        }
+        // Trường hợp 3: Nằm trên vùng phẳng (đỉnh)
+        else if (inputValue >= left_top && inputValue <= right_top)
+        {
+            membership = 1.0;
+        }
+        // Trường hợp 4: Nằm trên cạnh giảm dần (phải)
+        else if (inputValue > right_top && inputValue < right_base)
+        {
+            if (right_base != right_top)  // Tránh chia cho 0
+            {
+                membership = (right_base - inputValue) / (right_base - right_top);
+            }
+        }
+        // Trường hợp 5: Nằm ngoài phía phải hình thang
+        else if (inputValue >= right_base)
+        {
+            membership = 0.0;
+        }
+
+        return membership;
+    }
+
+    /// <summary>
+    /// Hàm thuộc tam giác (Triangular Membership Function)
+    /// Dạng tam giác với 3 điểm: [left, peak, right]
+    ///
+    ///           peak
+    ///           /\
+    ///          /  \
+    ///         /    \
+    ///   _____/      \_____
+    ///      left      right
+    /// 
+    /// </summary>
+    /// <param name="inputValue">Giá trị đầu vào cần tính độ thuộc</param>
+    /// <param name="left">Điểm bắt đầu tam giác (μ = 0)</param>
+    /// <param name="peak">Điểm đỉnh tam giác (μ = 1)</param>
+    /// <param name="right">Điểm kết thúc tam giác (μ = 0)</param>
+    /// <returns>Độ thuộc trong khoảng [0, 1]</returns>
+    private static double Fuzzy_trimf(double inputValue, double left, double peak, double right)
+    {
+        double membership = 0.0;
+
+        // Trường hợp 1: Nằm ngoài phía trái tam giác
+        if (inputValue <= left)
+        {
+            membership = 0.0;
+        }
+        // Trường hợp 2: Nằm trên cạnh tăng dần (trái)
+        else if (inputValue > left && inputValue < peak)
+        {
+            if (peak != left)  // Tránh chia cho 0
+            {
+                membership = (inputValue - left) / (peak - left);
+            }
+        }
+        // Trường hợp 3: Đúng tại đỉnh tam giác
+        else if (inputValue == peak)
+        {
+            membership = 1.0;
+        }
+        // Trường hợp 4: Nằm trên cạnh giảm dần (phải)
+        else if (inputValue > peak && inputValue < right)
+        {
+            if (right != peak)  // Tránh chia cho 0
+            {
+                membership = (right - inputValue) / (right - peak);
+            }
+        }
+        // Trường hợp 5: Nằm ngoài phía phải tam giác
+        else if (inputValue >= right)
+        {
+            membership = 0.0;
+        }
+
+        return membership;
+    }
+
+    // ============================================================================
+    // PHẦN 2: FUZZIFICATION (MỜ HÓA ĐẦU VÀO)
+    // ============================================================================
+
+    /// <summary>
+    /// Mờ hóa tín hiệu PI thành 5 tập mờ
+    /// </summary>
+    /// <param name="piSignal">Tín hiệu đầu ra của bộ PI</param>
+    /// <param name="membershipValues">Mảng lưu giá trị độ thuộc (vị trí 0-4)</param>
+    private static void FuzzifyPISignal(double piSignal, double[] membershipValues)
+    {
+        // 1. NB (Negative Big): [-∞, -∞, -1.0, -0.5]
+        membershipValues[0] = Fuzzy_trapmf(piSignal, -1.0E+10, -1.0E+10, -1.0, -0.5);
+
+        // 2. Z (Zero): [-0.5, 0.0, 0.5]
+        membershipValues[1] = Fuzzy_trimf(piSignal, -0.5, 0.0, 0.5);
+
+        // 3. PB (Positive Big): [0.5, 1.0, +∞, +∞]
+        membershipValues[2] = Fuzzy_trapmf(piSignal, 0.5, 1.0, 1.0E+10, 1.0E+10);
+
+        // 4. NM (Negative Medium): [-1.0, -0.5, 0.0]
+        membershipValues[3] = Fuzzy_trimf(piSignal, -1.0, -0.5, 0.0);
+
+        // 5. PM (Positive Medium): [0.0, 0.5, 1.0]
+        membershipValues[4] = Fuzzy_trimf(piSignal, 0.0, 0.5, 1.0);
+    }
+
+    /// <summary>
+    /// Mờ hóa vận tốc V thành 11 tập mờ (dải 0.0 - 3.0)
+    /// Độ phân giải: 0.3 m/s - Cân bằng tốt giữa độ chính xác và hiệu suất
+    /// </summary>
+    /// <param name="velocity">Vận tốc tuyến tính mong muốn (0.0 - 3.0 m/s)</param>
+    /// <param name="membershipValues">Mảng lưu giá trị độ thuộc (vị trí 5-15)</param>
+    private static void FuzzifyVelocity(double velocity, double[] membershipValues)
+    {
+        // 11 tập mờ phân bố đều từ 0.0 đến 3.0
+        // Khoảng cách giữa các đỉnh: 3.0 / 10 = 0.3 m/s
+
+        // 1. VVS (Very Very Slow): [-∞, -∞, 0.0, 0.3]
+        membershipValues[5] = Fuzzy_trapmf(velocity, -1.0E+9, -1.0E+9, 0.0, 0.3);
+
+        // 2. VS (Very Slow): [0.0, 0.3, 0.6]
+        membershipValues[6] = Fuzzy_trimf(velocity, 0.0, 0.3, 0.6);
+
+        // 3. S- (Slow Low): [0.3, 0.6, 0.9]
+        membershipValues[7] = Fuzzy_trimf(velocity, 0.3, 0.6, 0.9);
+
+        // 4. S (Slow): [0.6, 0.9, 1.2]
+        membershipValues[8] = Fuzzy_trimf(velocity, 0.6, 0.9, 1.2);
+
+        // 5. S+ (Slow High): [0.9, 1.2, 1.5]
+        membershipValues[9] = Fuzzy_trimf(velocity, 0.9, 1.2, 1.5);
+
+        // 6. M (Medium): [1.2, 1.5, 1.8]
+        membershipValues[10] = Fuzzy_trimf(velocity, 1.2, 1.5, 1.8);
+
+        // 7. F- (Fast Low): [1.5, 1.8, 2.1]
+        membershipValues[11] = Fuzzy_trimf(velocity, 1.5, 1.8, 2.1);
+
+        // 8. F (Fast): [1.8, 2.1, 2.4]
+        membershipValues[12] = Fuzzy_trimf(velocity, 1.8, 2.1, 2.4);
+
+        // 9. F+ (Fast High): [2.1, 2.4, 2.7]
+        membershipValues[13] = Fuzzy_trimf(velocity, 2.1, 2.4, 2.7);
+
+        // 10. VF (Very Fast): [2.4, 2.7, 3.0]
+        membershipValues[14] = Fuzzy_trimf(velocity, 2.4, 2.7, 3.0);
+
+        // 11. VVF (Very Very Fast): [2.7, 3.0, +∞, +∞]
+        membershipValues[15] = Fuzzy_trapmf(velocity, 2.7, 3.0, 1.0E+9, 1.0E+9);
+    }
+
+    // ============================================================================
+    // PHẦN 3: RULE EVALUATION (ĐÁNH GIÁ LUẬT MỜ)
+    // ============================================================================
+
+    /// <summary>
+    /// Đánh giá luật mờ cho một bộ điều khiển
+    /// </summary>
+    /// <param name="inputMembershipValues">Độ thuộc của các đầu vào (16 giá trị: 5 PI + 11 V)</param>
+    /// <param name="ruleAntecedentIndices">Ma trận chỉ số tiền đề (numRules * 2 phần tử)</param>
+    /// <param name="ruleConsequentIndices">Ma trận chỉ số hệ quả (numRules phần tử)</param>
+    /// <param name="outputSingletons">Các giá trị singleton đầu ra</param>
+    /// <param name="numRules">Số lượng luật</param>
+    /// <returns>(weightedSum: tổng có trọng số, totalWeight: tổng trọng số)</returns>
+    private static (double weightedSum, double totalWeight) EvaluateRules(
+        double[] inputMembershipValues,
+        byte[] ruleAntecedentIndices,
+        byte[] ruleConsequentIndices,
+        double[] outputSingletons,
+        int numRules)
+    {
+        const int VELOCITY_OFFSET = 5;  // Chỉ số bắt đầu của V trong inputMembershipValues
+
+        double weightedSum = 0.0;
+        double totalWeight = 0.0;
+
+        for (int ruleIndex = 0; ruleIndex < numRules; ruleIndex++)
+        {
+            // Lấy chỉ số tập mờ cho PI (từ 1-5, cần trừ 1 để thành 0-4)
+            int piMembershipIndex = ruleAntecedentIndices[ruleIndex] - 1;
+
+            // Lấy chỉ số tập mờ cho Velocity (từ 1-11, cộng offset)
+            int velocityMembershipIndex = ruleAntecedentIndices[ruleIndex + numRules] + VELOCITY_OFFSET - 1;
+
+            // Tính độ kích hoạt của luật (AND operator = phép nhân)
+            double ruleActivation = inputMembershipValues[piMembershipIndex]
+                                   * inputMembershipValues[velocityMembershipIndex];
+
+            // Lấy giá trị singleton đầu ra tương ứng (từ 1-11, cần trừ 1)
+            int outputIndex = ruleConsequentIndices[ruleIndex] - 1;
+            double outputValue = outputSingletons[outputIndex];
+
+            // Tích lũy tổng trọng số và tổng có trọng số
+            totalWeight += ruleActivation;
+            weightedSum += outputValue * ruleActivation;
+        }
+
+        return (weightedSum, totalWeight);
+    }
+
+    // ============================================================================
+    // PHẦN 4: DEFUZZIFICATION (GIẢI MỜ ĐẦU RA)
+    // ============================================================================
+
+    /// <summary>
+    /// Giải mờ bằng phương pháp trọng tâm (Weighted Average / Center of Gravity)
+    /// Công thức: output = Σ(singleton_i × weight_i) / Σ(weight_i)
+    /// </summary>
+    /// <param name="weightedSum">Tổng đầu ra có trọng số</param>
+    /// <param name="totalWeight">Tổng trọng số của tất cả các luật</param>
+    /// <param name="defaultValue">Giá trị mặc định nếu totalWeight = 0</param>
+    /// <returns>Giá trị đầu ra rõ (crisp output)</returns>
+    private static double Defuzzify(double weightedSum, double totalWeight, double defaultValue = 0.5)
+    {
+        // Nếu không có luật nào được kích hoạt, trả về giá trị mặc định
+        if (totalWeight == 0.0)
+        {
+            return defaultValue;
+        }
+        else
+        {
+            // Tính trọng tâm: output = weightedSum / totalWeight
+            return weightedSum / totalWeight;
+        }
+    }
+
+    // ============================================================================
+    // PHẦN 5: BẢNG LUẬT VÀ CẤU HÌNH
+    // ============================================================================
+
+    /// <summary>
+    /// Bảng chỉ số tiền đề luật cho bánh phải (Rule Antecedent Indices - Right Wheel)
+    /// 110 phần tử: 55 cho PI + 55 cho Velocity
+    /// Hệ thống: 5 tập mờ PI × 11 tập mờ V = 55 luật
+    /// Giá trị từ 1-5 cho PI, 1-11 cho V
+    /// </summary>
+    private static readonly byte[] RULE_ANTECEDENT_INDICES_RIGHT =
+    [
+        // 55 phần tử đầu: Chỉ số tập mờ của PI Signal (1-5)
+        // Mỗi tập PI lặp lại 11 lần (cho 11 mức vận tốc)
+        1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,  // Luật 1-11:  PI = NB (tập 1)
+        2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2,  // Luật 12-22: PI = Z  (tập 2)
+        3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3,  // Luật 23-33: PI = PB (tập 3)
+        4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4,  // Luật 34-44: PI = NM (tập 4)
+        5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,  // Luật 45-55: PI = PM (tập 5)
+    
+        // 55 phần tử sau: Chỉ số tập mờ của Velocity (1-11)
+        // Lặp lại theo pattern: VVS, VS, S-, S, S+, M, F-, F, F+, VF, VVF
+        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,  // Luật 1-11:  V = VVS đến VVF
+        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,  // Luật 12-22: V = VVS đến VVF
+        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,  // Luật 23-33: V = VVS đến VVF
+        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,  // Luật 34-44: V = VVS đến VVF
+        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11   // Luật 45-55: V = VVS đến VVF
+    ];
+
+    /// <summary>
+    /// Bảng chỉ số hệ quả cho bánh phải (Rule Consequent Indices - Right Wheel)
+    /// 55 phần tử tương ứng với 55 luật
+    /// Giá trị từ 1-11 tương ứng với 11 mức tốc độ đầu ra (0.0 - 3.0)
+    /// 
+    /// Logic bánh phải:
+    /// - PI âm (NB, NM): Bánh phải chậm hơn → Robot rẽ trái
+    /// - PI = 0 (Z): Bánh phải theo vận tốc V → Robot đi thẳng
+    /// - PI dương (PB, PM): Bánh phải nhanh hơn → Robot rẽ phải
+    /// </summary>
+    private static readonly byte[] RULE_CONSEQUENT_INDICES_RIGHT =
+    [   
+        // PI = NB (Negative Big) - Bánh phải RẤT CHẬM (rẽ trái mạnh)
+        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
+             1,  1,  1,  2,  2,  3,  3,  4,  5,  6,  7,    // Luật 1-11
+    
+        // PI = Z (Zero) - Bánh phải THEO VẬN TỐC (đi thẳng)
+        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
+             2,  2,  3,  4,  5,  6,  7,  8,  9,  10, 11,   // Luật 12-22
+    
+        // PI = PB (Positive Big) - Bánh phải RẤT NHANH (rẽ phải mạnh)
+        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
+             4,  5,  6,  7,  8,  9,  10, 10, 11, 11, 11,   // Luật 23-33
+    
+        // PI = NM (Negative Medium) - Bánh phải HƠI CHẬM (rẽ trái nhẹ)
+        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
+             1,  2,  2,  3,  4,  5,  6,  7,  8,  9,  10,   // Luật 34-44
+    
+        // PI = PM (Positive Medium) - Bánh phải HƠI NHANH (rẽ phải nhẹ)
+        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
+             2,  3,  4,  5,  6,  7,  8,  8,  9,  10, 11    // Luật 45-55
+    ];
+
+    /// <summary>
+    /// Bảng chỉ số tiền đề luật cho bánh trái (Rule Antecedent Indices - Left Wheel)
+    /// 110 phần tử: 55 cho PI + 55 cho Velocity
+    /// </summary>
+    private static readonly byte[] RULE_ANTECEDENT_INDICES_LEFT =
+    [
+        // 55 phần tử đầu: Chỉ số tập mờ của PI Signal (1-5)
+        1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,  // Luật 1-11:  PI = NB (tập 1)
+        2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2,  // Luật 12-22: PI = Z  (tập 2)
+        3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3,  // Luật 23-33: PI = PB (tập 3)
+        4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4,  // Luật 34-44: PI = NM (tập 4)
+        5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,  // Luật 45-55: PI = PM (tập 5)
+    
+        // 55 phần tử sau: Chỉ số tập mờ của Velocity (1-11)
+        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,  // Luật 1-11
+        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,  // Luật 12-22
+        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,  // Luật 23-33
+        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,  // Luật 34-44
+        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11   // Luật 45-55
+    ];
+
+    /// <summary>
+    /// Bảng chỉ số hệ quả cho bánh trái (Rule Consequent Indices - Left Wheel)
+    /// 55 phần tử tương ứng với 55 luật
+    /// 
+    /// Logic bánh trái: NGƯỢC LẠI với bánh phải
+    /// - PI âm (NB, NM): Bánh trái nhanh hơn → Robot rẽ trái
+    /// - PI = 0 (Z): Bánh trái theo vận tốc V → Robot đi thẳng
+    /// - PI dương (PB, PM): Bánh trái chậm hơn → Robot rẽ phải
+    /// </summary>
+    private static readonly byte[] RULE_CONSEQUENT_INDICES_LEFT =
+    [
+        // PI = NB (Negative Big) - Bánh trái RẤT NHANH (rẽ trái mạnh)
+        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
+             4,  5,  6,  7,  8,  9,  10, 10, 11, 11, 11,   // Luật 1-11
+    
+        // PI = Z (Zero) - Bánh trái THEO VẬN TỐC (đi thẳng)
+        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
+             2,  2,  3,  4,  5,  6,  7,  8,  9,  10, 11,   // Luật 12-22
+    
+        // PI = PB (Positive Big) - Bánh trái RẤT CHẬM (rẽ phải mạnh)
+        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
+             1,  1,  1,  2,  2,  3,  3,  4,  5,  6,  7,    // Luật 23-33
+    
+        // PI = NM (Negative Medium) - Bánh trái HƠI NHANH (rẽ trái nhẹ)
+        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
+             3,  4,  5,  6,  7,  8,  8,  9,  10, 11, 11,   // Luật 34-44
+    
+        // PI = PM (Positive Medium) - Bánh trái HƠI CHẬM (rẽ phải nhẹ)
+        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
+             1,  2,  2,  3,  4,  4,  5,  6,  7,  8,  9,    // Luật 45-55
+    ];
+
+    /// <summary>
+    /// Các mức đầu ra singleton (Output Singletons)
+    /// 11 mức tốc độ từ 0.0 đến 3.0 m/s
+    /// Độ phân giải: 3.0 / 10 = 0.3 m/s
+    /// </summary>
+    private static readonly double[] OUTPUT_SINGLETON_LEVELS =
+    [
+        0.0,   // Mức 1:  Dừng hoàn toàn (0%)
+        0.3,   // Mức 2:  Rất chậm (10%)
+        0.6,   // Mức 3:  Chậm (20%)
+        0.9,   // Mức 4:  Hơi chậm (30%)
+        1.2,   // Mức 5:  Chậm vừa (40%)
+        1.5,   // Mức 6:  Trung bình (50%) - Điểm chuẩn
+        1.8,   // Mức 7:  Hơi nhanh (60%)
+        2.1,   // Mức 8:  Nhanh (70%)
+        2.4,   // Mức 9:  Nhanh vừa (80%)
+        2.7,   // Mức 10: Rất nhanh (90%)
+        3.0    // Mức 11: Tối đa (100%)
+    ];
+
+    // ============================================================================
+    // PHẦN 6: HÀM CHÍNH - BỘ ĐIỀU KHIỂN PI + FUZZY
+    // ============================================================================
+
+    // Biến trạng thái bộ tích phân
+    private static double integratorState = 0.0;
+
+    // Hệ số bộ điều khiển PI
+    private static double proportionalGain = 1.0;  // Hệ số tỷ lệ (Kp)
+    private static double integralGain = 0.1;      // Hệ số tích phân (Ki)
+
+    /// <summary>
+    /// Hàm điều khiển chính - Tính toán tốc độ bánh trái và phải
+    /// Sử dụng bộ điều khiển PI kết hợp với logic mờ
+    /// 
+    /// CẤU HÌNH PHIÊN BẢN 3.0 - 11 TẬP MỜ:
+    /// - PI Signal: 5 tập mờ (NB, Z, PB, NM, PM)
+    /// - Velocity: 11 tập mờ (VVS, VS, S-, S, S+, M, F-, F, F+, VF, VVF)
+    /// - Tổng số luật: 5 × 11 = 55 luật cho mỗi bánh
+    /// - Dải đầu ra: 0.0 - 3.0 m/s
+    /// - Độ phân giải: 0.3 m/s (tối ưu - cân bằng giữa độ mịn và hiệu suất)
+    /// </summary>
+    /// <param name="desiredVelocity">Vận tốc tuyến tính mong muốn (0.0 - 3.0 m/s)</param>
+    /// <param name="desiredAngularVelocity">Vận tốc góc mong muốn (rad/s)</param>
+    /// <param name="samplingTime">Chu kỳ lấy mẫu (giây, khuyến nghị 0.01s)</param>
+    /// <returns>(leftWheelSpeed, rightWheelSpeed): Tốc độ bánh trái và phải trong [0.0, 3.0]</returns>
+    public (double leftWheelSpeed, double rightWheelSpeed) Fuzzy_step(
+        double desiredVelocity,
+        double desiredAngularVelocity,
+        double samplingTime)
+    {
+        const int NUM_INPUT_MEMBERSHIPS = 16;   // 5 cho PI + 11 cho Velocity
+        const int NUM_OUTPUT_LEVELS = 11;       // 11 mức đầu ra (0.0 - 3.0)
+        const int NUM_RULES = 55;               // 5 × 11 = 55 luật
+
+        // Khởi tạo mảng lưu độ thuộc đầu vào
+        double[] inputMembershipValues = new double[NUM_INPUT_MEMBERSHIPS];
+
+        // Khởi tạo mảng lưu các mức đầu ra
+        double[] outputLevels = new double[NUM_OUTPUT_LEVELS];
+        Array.Copy(OUTPUT_SINGLETON_LEVELS, outputLevels, NUM_OUTPUT_LEVELS);
+
+        // ========== BƯỚC 1: BỘ ĐIỀU KHIỂN PI ==========
+        // Cập nhật trạng thái tích phân: I(t) = I(t-1) + Ki * error * dt
+        integratorState += integralGain * desiredAngularVelocity * samplingTime;
+
+        // Chống bão hòa tích phân (Anti-windup)
+        integratorState = Math.Clamp(integratorState, -1.5, 1.5);
+
+        // Tính tín hiệu điều khiển PI: u(t) = Kp * error + I(t)
+        double piControlSignal = proportionalGain * desiredAngularVelocity + integratorState;
+
+        // ========== BƯỚC 2: MỜ HÓA ĐẦU VÀO ==========
+        FuzzifyPISignal(piControlSignal, inputMembershipValues);
+        FuzzifyVelocity(desiredVelocity, inputMembershipValues);
+
+        // ========== BƯỚC 3: TÍNH TOÁN BÁNH PHẢI ==========
+        var (weightedSum_Right, totalWeight_Right) = EvaluateRules(
+            inputMembershipValues,
+            RULE_ANTECEDENT_INDICES_RIGHT,
+            RULE_CONSEQUENT_INDICES_RIGHT,
+            outputLevels,
+            NUM_RULES
+        );
+
+        double rightWheelSpeed = Defuzzify(weightedSum_Right, totalWeight_Right, defaultValue: 1.5);
+
+        // ========== BƯỚC 4: TÍNH TOÁN BÁNH TRÁI ==========
+        var (weightedSum_Left, totalWeight_Left) = EvaluateRules(
+            inputMembershipValues,
+            RULE_ANTECEDENT_INDICES_LEFT,
+            RULE_CONSEQUENT_INDICES_LEFT,
+            outputLevels,
+            NUM_RULES
+        );
+
+        double leftWheelSpeed = Defuzzify(weightedSum_Left, totalWeight_Left, defaultValue: 1.5);
+
+        // ========== BƯỚC 5: GIỚI HẠN AN TOÀN ==========
+        // Đảm bảo tốc độ không vượt quá giới hạn
+        leftWheelSpeed = Math.Clamp(leftWheelSpeed, 0.0, 3.0);
+        rightWheelSpeed = Math.Clamp(rightWheelSpeed, 0.0, 3.0);
+
+        // ========== BƯỚC 6: TRẢ VỀ KẾT QUẢ ==========
+        return (leftWheelSpeed, rightWheelSpeed);
+    }
+
+    // ============================================================================
+    // PHẦN 7: HÀM HỖ TRỢ
+    // ============================================================================
+
+    /// <summary>
+    /// Reset trạng thái bộ tích phân về 0
+    /// Nên gọi khi bắt đầu chu kỳ điều khiển mới hoặc khi cần reset hệ thống
+    /// </summary>
+    public void ResetIntegrator()
+    {
+        integratorState = 0.0;
+    }
+
+    /// <summary>
+    /// Thiết lập hệ số cho bộ điều khiển PI
+    /// </summary>
+    /// <param name="kp">Hệ số tỷ lệ (Proportional Gain) - Phản ứng với sai số hiện tại</param>
+    /// <param name="ki">Hệ số tích phân (Integral Gain) - Loại bỏ sai số tích lũy</param>
+    public FuzzyLogic WithPIGains(double kp, double ki)
+    {
+        proportionalGain = kp;
+        integralGain = ki;
+        return this;
+    }
+
+    /// <summary>
+    /// Lấy trạng thái hiện tại của bộ tích phân
+    /// Hữu ích cho việc debug và giám sát hệ thống
+    /// </summary>
+    /// <returns>Giá trị tích phân hiện tại</returns>
+    public double GetIntegratorState()
+    {
+        return integratorState;
+    }
+
+    /// <summary>
+    /// Lấy hệ số PI hiện tại
+    /// </summary>
+    /// <returns>(Kp, Ki): Hệ số tỷ lệ và tích phân</returns>
+    public (double Kp, double Ki) GetPIGains()
+    {
+        return (proportionalGain, integralGain);
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Simulation/Algorithm/PID.cs

@@ -0,0 +1,55 @@
+namespace RobotNet10.RobotApp.Services.Simulation.Algorithm;
+
+public class PID
+{
+    private double Kp = 0.3;
+    private double Ki = 0.0001;
+    private double Kd = 0.01;
+
+    private double _prevError;
+    private double _integral;
+
+    public PID WithKp(double kp)
+    {
+        Kp = kp;
+        return this;
+    }
+
+    public PID WithKi(double ki)
+    {
+        Ki = ki;
+        return this;
+    }
+
+    public PID WithKd(double kd)
+    {
+        Kd = kd;
+        return this;
+    }
+
+    public double PID_step(double error, double max, double min, double timeSample)
+    {
+        double integralStep = 0.5 * (error + _prevError) * timeSample;
+        _integral += integralStep;
+
+        double derivative = (error - _prevError) / timeSample;
+        _prevError = error;
+
+        double Out = Kp * error
+                   + Ki * _integral
+                   + Kd * derivative;
+
+        // Anti-windup
+        double clamped = Math.Clamp(Out, min, max);
+        if (clamped != Out)
+            _integral -= integralStep;
+
+        return clamped;
+    }
+
+    public void Reset()
+    {
+        _prevError = 0;
+        _integral = 0;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Simulation/Algorithm/PurePursuit.cs

@@ -0,0 +1,380 @@
+using RobotNet.VDA5050.Order;
+using RobotNet10.Common;
+using RobotNet10.Common.Models;
+using RobotNet10.RobotApp.Services.Exceptions;
+using RobotNet10.RobotApp.Services.Robot;
+using RobotNet10.RobotApp.Services.Robot.Helper;
+using RobotNet10.RobotApp.Services.Robot.Models;
+using RobotNet10.RobotApp.Shared.Enums;
+
+namespace RobotNet10.RobotApp.Services.Simulation.Algorithm;
+
+public class PurePursuit
+{
+    private double MaxAngularVelocity = 1.5;
+    private double LookaheadDistance = 0.5;
+
+    private readonly double ResolutionSplit = 0.1;
+
+    private KDTree? KDTreeOrder;
+    private KDTree? KDTreeWay;
+    private OrderNode? LastNode;
+    private Dictionary<string, (int start, int end)>? _segmentCache;
+
+    public OrderNode? LastOrderNode = null;
+    private int LastNavNodeIndex = 0;
+    public int OnNodeIndex = 0;
+    public NavigationNode? Goal;
+    public List<NavigationNode> Waypoints_Value = [];
+
+    public OrderNode[] OrderNodes = [];
+    public OrderEdge[] OrderEdges = [];
+
+    public PurePursuit WithLookheadDistance(double distance)
+    {
+        LookaheadDistance = distance;
+        return this;
+    }
+
+    public PurePursuit WithMaxAngularVelocity(double vel)
+    {
+        MaxAngularVelocity = vel;
+        return this;
+    }
+
+    public PurePursuit WithPath(Node[] nodes, Edge[] edges, double currentTheta)
+    {
+        if (nodes.Length < 2) throw new SimulationException(RobotErrors.Error1002(nodes.Length));
+        if (edges.Length < 1) throw new SimulationException();
+        if (edges.Length != nodes.Length - 1) throw new SimulationException(RobotErrors.Error1004(nodes.Length, edges.Length));
+        (OrderNodes, OrderEdges) = OrderConverter.Validate(nodes, edges, currentTheta);
+        Waypoints_Value = [.. PathSplit(OrderNodes, OrderEdges)];
+        BuildSegmentCache();
+        KDTreeOrder = new KDTree(OrderNodes.Select(n => new KDTreeData(n.NodeId, n.X, n.Y)));
+        KDTreeWay = new KDTree(Waypoints_Value.Select(n => new KDTreeData(n.Id.ToString(), n.X, n.Y)));
+        return this;
+    }
+
+    public void UpdateGoal(string goalId)
+    {
+        var goal = Waypoints_Value.FirstOrDefault(n => n.NodeId == goalId);
+        if (goal is not null) Goal = goal;
+    }
+
+    private NavigationNode[] PathSplit(OrderNode[] nodes, OrderEdge[] edges)
+    {
+        List<NavigationNode> navigationNode = [new()
+            {
+                Id = Guid.NewGuid(),
+                NodeId = nodes[0].NodeId,
+                X = nodes[0].X,
+                Y = nodes[0].Y,
+                Theta = nodes[0].Theta,
+                Direction = edges[0].Direction,
+            }];
+        foreach (var edge in edges)
+        {
+            var startNode = nodes.FirstOrDefault(n => n.NodeId == edge.StartNodeId);
+            var endNode = nodes.FirstOrDefault(n => n.NodeId == edge.EndNodeId);
+            if (startNode is null) throw new PathPlannerException(RobotErrors.Error1008(edge.EdgeId, edge.StartNodeId));
+            if (endNode is null) throw new PathPlannerException(RobotErrors.Error1009(edge.EdgeId, edge.EndNodeId));
+
+            var spaceEdge = new SpaceEdge()
+            {
+                StartX = startNode.X,
+                StartY = startNode.Y,
+                EndX = endNode.X,
+                EndY = endNode.Y,
+                ControlPoint1X = edge.ControlPoint1X ?? 0,
+                ControlPoint1Y = edge.ControlPoint1Y ?? 0,
+                ControlPoint2X = edge.ControlPoint2X ?? 0,
+                ControlPoint2Y = edge.ControlPoint2Y ?? 0,
+                Degree = edge.Degree,
+            };
+
+            double length = SpaceCompute.GetEdgeLength(spaceEdge, ResolutionSplit);
+            if (length <= 0) continue;
+            double step = ResolutionSplit / length;
+
+            for (double t = step; t <= 1 - step; t += step)
+            {
+                (double x, double y) = SpaceCompute.BezierPoint(t, spaceEdge);
+                navigationNode.Add(new()
+                {
+                    Id = Guid.NewGuid(),
+                    NodeId = string.Empty,
+                    X = x,
+                    Y = y,
+                    Theta = null,
+                    Direction = edge.Direction,
+                });
+            }
+            navigationNode.Add(new()
+            {
+                Id = Guid.NewGuid(),
+                NodeId = endNode.NodeId,
+                X = endNode.X,
+                Y = endNode.Y,
+                Theta = endNode.Theta,
+                Direction = edge.Direction,
+            });
+        }
+        return [.. navigationNode];
+    }
+
+    private void BuildSegmentCache()
+    {
+        _segmentCache = [];
+
+        for (int i = 0; i < OrderNodes.Length - 1; i++)
+        {
+            var currentNodeId = OrderNodes[i].NodeId;
+
+            // Xác định phạm vi: từ (i-1) đến (i+1)
+            int rangeStart = Math.Max(0, i - 1);
+            int rangeEnd = Math.Min(OrderNodes.Length - 1, i + 1);
+
+            var startNodeId = OrderNodes[rangeStart].NodeId;
+            var endNodeId = OrderNodes[rangeEnd].NodeId;
+
+            // Tìm indices trong Waypoints_Value
+            int waypointStartIdx = Waypoints_Value.FindIndex(n => n.NodeId == startNodeId);
+            int waypointEndIdx = Waypoints_Value.FindIndex(n => n.NodeId == endNodeId);
+
+            // Xử lý trường hợp không tìm thấy
+            if (waypointStartIdx == -1) waypointStartIdx = 0;
+            if (waypointEndIdx == -1) waypointEndIdx = Waypoints_Value.Count - 1;
+
+            // Cache theo NodeId của OrderNode
+            _segmentCache[currentNodeId] = (waypointStartIdx, waypointEndIdx);
+        }
+    }
+
+    private (OrderNode node, int index)? GetOnNodeWithKDTree(double x, double y)
+    {
+        KDTreeOrder ??= new KDTree(OrderNodes.Select(n => new KDTreeData(n.NodeId, n.X, n.Y)));
+        LastNode ??= OrderNodes[0];
+        var dx = LastNode.X - x;
+        var dy = LastNode.Y - y;
+        var minDistance = Math.Sqrt(dx * dx + dy * dy);
+        var closesFindedNode = KDTreeOrder.FindNearest(x, y, minDistance);
+        if (closesFindedNode == null) return null;
+
+        var closesNodeIndex = Array.FindIndex(OrderNodes, n => n.NodeId == closesFindedNode?.Id);
+        if (closesNodeIndex == -1) return null;
+
+        if (OrderNodes[closesNodeIndex].NodeId != LastNode.NodeId)
+        {
+            var skipIndex = closesNodeIndex == 0 ? 0 : closesNodeIndex - 1;
+            var newNodes = OrderNodes.Skip(skipIndex);
+            KDTreeOrder = new KDTree(newNodes.Select(n => new KDTreeData(n.NodeId, n.X, n.Y)));
+
+            LastNode = OrderNodes[closesNodeIndex];
+        }
+        return (OrderNodes[closesNodeIndex], closesNodeIndex);
+    }
+
+    private (OrderNode node, int index) GetOnNode(double x, double y)
+    {
+        LastNode ??= OrderNodes[0];
+        var lastIndex = Array.FindIndex(OrderNodes, n => n.NodeId == LastNode?.NodeId);
+        lastIndex = Math.Max(0, lastIndex);
+
+        var dx = LastNode.X - x;
+        var dy = LastNode.Y - y;
+        var minDistance = Math.Sqrt(dx * dx + dy * dy);
+
+        OrderNode onNode = LastNode;
+        int index = 0;
+        for (int i = lastIndex; i < OrderNodes.Length; i++)
+        {
+            var node = OrderNodes[i]; // FIX: Dùng đúng array
+            dx = x - node.X;
+            dy = y - node.Y;
+            var distance = dx * dx + dy * dy;
+            if (distance < minDistance)
+            {
+                onNode = OrderNodes[i];
+                minDistance = distance;
+                index = i;
+            }
+        }
+        return (onNode, index);
+    }
+
+    public (NavigationNode node, int index) GetOnNavNode(double x, double y)
+    {
+        double minDistance = double.MaxValue;
+        NavigationNode onNode = Waypoints_Value[0];
+        int index = 0;
+        for (int i = 1; i < Waypoints_Value.Count; i++)
+        {
+            var distance = Math.Sqrt(Math.Pow(x - Waypoints_Value[i].X, 2) + Math.Pow(y - Waypoints_Value[i].Y, 2));
+            if (distance < minDistance)
+            {
+                onNode = Waypoints_Value[i];
+                minDistance = distance;
+                index = i;
+            }
+        }
+        return (onNode, index);
+    }
+
+    private (NavigationNode? node, int index) OnNodeLinear(double x, double y)
+    {
+        var (node, index) = GetOnNodeWithKDTree(x, y) ?? GetOnNode(x, y);
+
+        int waypointStartIdx = 0;
+        int waypointEndIdx = Waypoints_Value.Count - 1;
+        if (_segmentCache is null)
+        {
+            var startIndex = Math.Max(0, index - 1);
+            var endIndex = Math.Min(OrderNodes.Length - 1, index + 1);
+            var navStartNodeIndex = Waypoints_Value.FindIndex(n => n.NodeId == OrderNodes[startIndex].NodeId);
+            waypointStartIdx = navStartNodeIndex == -1 ? 0 : navStartNodeIndex;
+            var navEndNodeIndex = Waypoints_Value.FindIndex(n => n.NodeId == OrderNodes[endIndex].NodeId);
+            waypointEndIdx = navEndNodeIndex == -1 ? Waypoints_Value.Count - 1 : navEndNodeIndex;
+        }
+        else if (_segmentCache.TryGetValue(node.NodeId, out var range)) (waypointStartIdx, waypointEndIdx) = range;
+
+        waypointStartIdx = Math.Min(waypointStartIdx, OnNodeIndex);
+
+        var dx = OrderNodes[index].X - x;
+        var dy = OrderNodes[index].Y - y;
+        var minDistance = Math.Sqrt(dx * dx + dy * dy);
+
+        NavigationNode? bestNode = null;
+        int bestIndex = -1;
+
+        for (int i = waypointStartIdx; i <= waypointEndIdx; i++)
+        {
+            var orderNodeId = Waypoints_Value[i].NodeId;
+
+            // Tìm NavigationNode tương ứng
+            var navIndex = Waypoints_Value.FindIndex(n => n.NodeId == orderNodeId);
+            if (navIndex == -1) continue;
+
+            var navNode = Waypoints_Value[navIndex];
+            dx = x - navNode.X;
+            dy = y - navNode.Y;
+            var distanceSq = dx * dx + dy * dy;
+
+            if (distanceSq < minDistance)
+            {
+                minDistance = distanceSq;
+                bestNode = navNode;
+                bestIndex = navIndex;
+            }
+        }
+
+        return (bestNode, bestIndex);
+    }
+
+    private (NavigationNode? node, int index) OnNodeWithKDTree(double x, double y)
+    {
+        KDTreeWay ??= new KDTree(Waypoints_Value.Select(n => new KDTreeData(n.Id.ToString(), n.X, n.Y)));
+        var kdtreeNode = KDTreeWay.FindNearest(x, y, double.MaxValue);
+        if(kdtreeNode is not null && Guid.TryParse(kdtreeNode.Id, out Guid kdtreeNodeId))
+        {
+            var nodeIndex = Waypoints_Value.FindIndex(n => n.Id == kdtreeNodeId);
+            if(nodeIndex != -1)  return (Waypoints_Value[nodeIndex], nodeIndex);
+        }
+        return (null, -1);
+    }
+
+    private (NavigationNode? node, int index) OnNode(double x, double y)
+    {
+        var (node, index) = OnNodeWithKDTree(x, y);
+        if (node is null || index == -1) return OnNodeLinear(x, y);
+        return (node, index);
+    }
+
+    private void UpdateLastOrderNode()
+    {
+        var oldNavNodes = Waypoints_Value.Skip(LastNavNodeIndex).Take(OnNodeIndex);
+        var lastNavNode = oldNavNodes.LastOrDefault(n => string.IsNullOrEmpty(n.NodeId));
+        if (lastNavNode is null) return;
+        LastOrderNode = OrderNodes.FirstOrDefault(n => n.NodeId == lastNavNode.NodeId);
+        if (LastOrderNode is not null) LastNavNodeIndex = Waypoints_Value.IndexOf(lastNavNode);
+    }
+
+    public double PurePursuit_step(double X_Ref, double Y_Ref, double Angle_Ref)
+    {
+        if (Waypoints_Value is null || Waypoints_Value.Count < 2) return 0;
+        NavigationNode? lookaheadStartPt = null;
+        var (onNode, index) = OnNode(X_Ref, Y_Ref);
+        if (onNode is null || Goal is null) return 0;
+        OnNodeIndex = index;
+        UpdateLastOrderNode();
+        double lookDistance = 0;
+        for (int i = OnNodeIndex + 1; i < Waypoints_Value.IndexOf(Goal); i++)
+        {
+            lookDistance += Math.Sqrt(Math.Pow(Waypoints_Value[i - 1].X - Waypoints_Value[i].X, 2) + Math.Pow(Waypoints_Value[i - 1].Y - Waypoints_Value[i].Y, 2));
+            if (lookDistance >= LookaheadDistance || Waypoints_Value[i].Direction != onNode.Direction)
+            {
+                lookaheadStartPt = Waypoints_Value[i];
+                break;
+            }
+        }
+        lookaheadStartPt ??= Goal;
+        if (onNode.Direction == RobotDirection.BACKWARD)
+        {
+            if (Angle_Ref > Math.PI) Angle_Ref -= Math.PI * 2;
+            else if (Angle_Ref < -Math.PI) Angle_Ref += Math.PI * 2;
+            Angle_Ref += Math.PI;
+            if (Angle_Ref > Math.PI) Angle_Ref -= Math.PI * 2;
+        }
+        var distance = Math.Atan2(lookaheadStartPt.Y - Y_Ref, lookaheadStartPt.X - X_Ref) - Angle_Ref;
+
+        if (Math.Abs(distance) > Math.PI)
+        {
+            double minDistance;
+            if (distance + Math.PI == 0.0) minDistance = 0.0;
+            else
+            {
+                double data = (distance + Math.PI) / (2 * Math.PI);
+                if (data < 0) data = Math.Round(data + 0.5);
+                else data = Math.Round(data - 0.5);
+                minDistance = distance + Math.PI - data * (2 * Math.PI);
+                double checker = 0;
+                if (minDistance != 0.0)
+                {
+                    checker = Math.Abs((distance + Math.PI) / (2 * Math.PI));
+                }
+                if (!(Math.Abs(checker - Math.Floor(checker + 0.5)) > 2.2204460492503131E-16 * checker))
+                {
+                    minDistance = 0.0;
+                }
+                else if (distance + Math.PI < 0.0)
+                {
+                    minDistance += Math.PI * 2;
+                }
+            }
+            if (minDistance == 0.0 && distance + Math.PI > 0.0)
+            {
+                minDistance = Math.PI * 2;
+            }
+            distance = minDistance - Math.PI;
+        }
+
+        var AngularVelocity = 2.0 * 0.5 * Math.Sin(distance) / LookaheadDistance;
+        if (Math.Abs(AngularVelocity) > MaxAngularVelocity)
+        {
+            if (AngularVelocity < 0.0)
+            {
+                AngularVelocity = -1.0;
+            }
+            else if (AngularVelocity > 0.0)
+            {
+                AngularVelocity = 1.0;
+            }
+            else if (AngularVelocity == 0.0)
+            {
+                AngularVelocity = 0.0;
+            }
+            AngularVelocity *= MaxAngularVelocity;
+        }
+        return AngularVelocity;
+    }
+
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Simulation/Navigation/DifferentialNavigation.cs

@@ -0,0 +1,128 @@
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Shared.Enums;
+
+namespace RobotNet10.RobotApp.Services.Simulation.Navigation;
+
+public class DifferentialNavigation : SimulationNavigation
+{
+    private readonly Logger<DifferentialNavigation> Logger;
+
+    public DifferentialNavigation(IServiceProvider ServiceProvider) : base(ServiceProvider)
+    {
+        using var scope = ServiceProvider.CreateScope();
+        Logger = scope.ServiceProvider.GetRequiredService<Logger<DifferentialNavigation>>();
+    }
+
+    private bool IsBackToPath = false;
+    private double? BackToAngle;
+
+    protected override void NavigationHandler()
+    {
+        try
+        {
+            if (NavState == NavigationState.Rotating)
+            {
+                if (RotatePID is not null)
+                {
+                    double Error = Visualization.Theta - TargetAngle;
+                    if (Error > 180) Error -= 360;
+                    else if (Error < -180) Error += 360;
+                    if (Math.Abs(Error) < 1)
+                    {
+                        if(IsBackToPath && BackToAngle.HasValue)
+                        {
+                            TargetAngle = BackToAngle.Value;
+                            BackToAngle = null;
+                            IsBackToPath = false;
+                        }
+                        else if (MovePurePursuit is not null && MovePurePursuit.Waypoints_Value is not null && MovePurePursuit.Waypoints_Value.Count > 2) NavState = NavigationState.Moving;
+                        else
+                        {
+                            NavState = NavigationState.Completed;
+                            Dispose();
+                        }
+                    }
+                    else
+                    {
+                        var SpeedCal = RotatePID.PID_step(Error * Math.PI / 180, AngularVelocity, -AngularVelocity, CycleHandlerMilliseconds / 1000.0);
+                        VelocityController.SetSpeed(SpeedCal, SpeedCal, CycleHandlerMilliseconds / 1000.0);
+                    }
+                }
+            }
+            else if (NavState == NavigationState.Moving)
+            {
+                if (MovePurePursuit is not null && MovePurePursuit.Waypoints_Value is not null && MovePurePursuit?.OrderNodes is not null && MovePurePursuit.OrderNodes.Length > 1 && GoalRotate is not null)
+                {
+                    if (MovePID is not null && MoveFuzzy is not null && MovePurePursuit is not null)
+                    {
+                        var DistanceToGoal = Math.Sqrt(Math.Pow(Visualization.X - MovePurePursuit.OrderNodes[^1].X, 2) + Math.Pow(Visualization.Y - MovePurePursuit.OrderNodes[^1].Y, 2));
+                        var DistanceToCheckingNode = Math.Sqrt(Math.Pow(Visualization.X - GoalRotate.X, 2) + Math.Pow(Visualization.Y - GoalRotate.Y, 2));
+                        var deviation = GoalRotate.NodeId == MovePurePursuit.OrderNodes[^1].NodeId ? 0.02 : 0.05;
+                        if (DistanceToCheckingNode > deviation)
+                        {
+                            double SpeedTarget = MovePID.PID_step(DistanceToCheckingNode, MaxVelocity, 0, CycleHandlerMilliseconds / 1000.0);
+                            double AngularVel = MovePurePursuit.PurePursuit_step(Visualization.X, Visualization.Y, Visualization.Theta * Math.PI / 180);
+                            AngularVel *= MovePurePursuit.Waypoints_Value[MovePurePursuit.OnNodeIndex].Direction == RobotDirection.FORWARD ? 1 : -1;
+                            (double AngularVelocityLeft, double AngularVelocityRight) = MoveFuzzy.Fuzzy_step(SpeedTarget, AngularVel, CycleHandlerMilliseconds / 1000.0);
+
+                            if (MovePurePursuit.Waypoints_Value[MovePurePursuit.OnNodeIndex].Direction == RobotDirection.FORWARD)
+                            {
+                                AngularVelocityLeft /= PhysicalCog.WheelRadius;
+                                AngularVelocityRight = AngularVelocityRight / PhysicalCog.WheelRadius * -1;
+                            }
+                            else
+                            {
+                                AngularVelocityLeft = AngularVelocityLeft / PhysicalCog.WheelRadius * -1;
+                                AngularVelocityRight /= PhysicalCog.WheelRadius;
+                            }
+                            VelocityController.SetSpeed(AngularVelocityLeft, AngularVelocityRight, CycleHandlerMilliseconds / 1000.0);
+                        }
+                        else if (DistanceToGoal < 0.02)
+                        {
+                            if (MovePurePursuit.OrderNodes[^1].Theta is { } theta)
+                            {
+                                TargetAngle = theta * 180 / Math.PI;
+                                NavState = NavigationState.Rotating;
+                                MovePurePursuit = null;
+                            }
+                            else
+                            {
+                                NavState = NavigationState.Completed;
+                                Dispose();
+                            }
+                        }
+                        else
+                        {
+                            double? targetAngle = null;
+                            if (GoalRotate.Theta is { } theta)
+                            {
+                                targetAngle = theta;
+                                ProcessedRotations.Add(GoalRotate.NodeId);
+                                BackToAngle = GoalRotate?.ContinueTheta * 180 / Math.PI;
+                                IsBackToPath = true;
+                            }
+
+                            var newGoalRotate = FindNextRotateGoal();
+                            if (newGoalRotate is not null && newGoalRotate.NodeId != GoalRotate?.NodeId)
+                            {
+                                GoalRotate = newGoalRotate;
+                                UpdateGoal(newGoalRotate.NodeId);
+                            }
+
+                            if(targetAngle.HasValue)
+                            {
+                                TargetAngle = targetAngle.Value * 180 / Math.PI;
+                                NavState = NavigationState.Rotating;
+                            }
+                        }
+                    }
+                }
+            }
+            else if (NavState == NavigationState.Paused) VelocityController.Stop();
+        }
+        catch (Exception ex)
+        {
+            Logger.Error($"Error in DifferentialNavigation: {ex.Message}");
+        }
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Simulation/Navigation/ForkliftNavigation.cs

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

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Simulation/NavigationNode.cs

@@ -0,0 +1,16 @@
+using RobotNet10.RobotApp.Shared.Enums;
+
+namespace RobotNet10.RobotApp.Services.Simulation;
+
+public class NavigationNode
+{
+    public Guid Id { get; set; }
+    public string NodeId { get; set; } = string.Empty;
+    public double X { get; set; }
+    public double Y { get; set; }
+    public double? Theta { get; set; }
+    public RobotDirection Direction { get; set; }
+    public double? AllowedDeviationXY { get; set; }
+    public double? AllowedDeviationTheta { get; set; }
+    public double? Speed { get; set; }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Simulation/SimulationConfig.cs

@@ -0,0 +1,10 @@
+namespace RobotNet10.RobotApp.Services.Simulation;
+
+public class SimulationConfig
+{
+    public bool IsEnable { get; set; }
+    public double MaxVelocity { get; set; } 
+    public double MaxAngularVelocity { get; set; }
+    public double Acceleration { get; set; }
+    public double Deceleration { get; set; }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Simulation/SimulationExtensions.cs

@@ -0,0 +1,10 @@
+namespace RobotNet10.RobotApp.Services.Simulation;
+
+public static class SimulationExtensions
+{
+    public static IServiceCollection AddRobotSimulation(this IServiceCollection services)
+    {
+        services.AddSingleton<SimulationVisualization>();
+        return services;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Simulation/SimulationNavigation.cs

@@ -0,0 +1,268 @@
+using RobotNet.VDA5050.Order;
+using RobotNet10.Common;
+using RobotNet10.RobotApp.Detection;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Services.ConfigManager;
+using RobotNet10.RobotApp.Services.Robot;
+using RobotNet10.RobotApp.Services.Robot.Models;
+using RobotNet10.RobotApp.Services.Simulation.Algorithm;
+using RobotNet10.RobotApp.Shared.Enums;
+
+namespace RobotNet10.RobotApp.Services.Simulation;
+
+public class SimulationNavigation : INavigation, IDisposable
+{
+    public NavigationState State => NavState;
+    public bool Driving => NavDriving;
+    public bool IsReady => true;
+    public double VelocityX => Visualization.Vx;
+    public double VelocityY => Visualization.Vy;
+    public double Omega => Visualization.Omega;
+
+    public event Action<NavigationState>? OnNavigationFinished;
+
+    protected NavigationState NavState = NavigationState.Idle;
+    protected bool NavDriving = false;
+
+    protected const int CycleHandlerMilliseconds = 50;
+    private WatchThread<SimulationNavigation>? NavigationTimer;
+
+    protected double TargetAngle = 0;
+    protected PID? RotatePID;
+    protected readonly double AngularVelocity;
+
+    protected PID? MovePID;
+    protected FuzzyLogic? MoveFuzzy;
+    protected PurePursuit? MovePurePursuit;
+
+    protected readonly SimulationVisualization Visualization;
+    protected readonly SimulationVelocity VelocityController;
+    protected readonly IRobotConfiguration RobotConfiguration;
+
+    protected OrderNode? GoalRotate;
+    protected OrderNode? CurrentBaseNode;
+    protected HashSet<string> ProcessedRotations = [];
+    protected NavigationState ResumeState = NavigationState.Idle;
+
+    protected SimulationConfig SimCog;
+    protected RobotPhysicalConfig PhysicalCog;
+    protected double MaxVelocity;
+
+    private readonly ILogger<SimulationNavigation> Logger;
+
+    public SimulationNavigation(IServiceProvider ServiceProvider)
+    {
+        using var scope = ServiceProvider.CreateScope();
+        Logger = scope.ServiceProvider.GetRequiredService<ILogger<SimulationNavigation>>();
+        Visualization = scope.ServiceProvider.GetRequiredService<SimulationVisualization>();
+        RobotConfiguration = scope.ServiceProvider.GetRequiredService<IRobotConfiguration>();
+        SimCog = RobotConfiguration.GetSimulationConfig();
+        MaxVelocity = SimCog.MaxVelocity;
+        PhysicalCog = RobotConfiguration.GetRobotPhysicalConfig();
+        VelocityController = new(Visualization, SimCog);
+        Visualization.SetPhysical(PhysicalCog.WheelRadius, PhysicalCog.Width);
+        AngularVelocity = SimCog.MaxAngularVelocity * PhysicalCog.Width / 2 / 2 / PhysicalCog.WheelRadius;
+    }
+
+    protected void HandleNavigationStart()
+    {
+        SimCog = RobotConfiguration.GetSimulationConfig();
+        PhysicalCog = RobotConfiguration.GetRobotPhysicalConfig();
+        Visualization.SetPhysical(PhysicalCog.WheelRadius, PhysicalCog.Width);
+        NavigationTimer = new(CycleHandlerMilliseconds, NavigationHandler, Logger);
+        NavigationTimer.Start();
+    }
+
+    protected void HandleNavigationStop()
+    {
+        NavigationTimer?.Dispose();
+        NavigationTimer = null;
+    }
+
+    protected virtual void NavigationHandler() { }
+
+    public void SafetyStop()
+    {
+        NavState = NavigationState.SafetyStop;
+    }
+
+    public void CancelMovement()
+    {
+        NavState = NavigationState.Canceled;
+        Dispose();
+    }
+
+    public void Move(RobotNet.VDA5050.Order.OrderMsg order, bool hasLoad = false)
+    {
+        var nodes = order.Nodes;
+        var edges = order.Edges;
+        NavState = NavigationState.Initializing;
+
+        MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
+        MoveFuzzy = new FuzzyLogic();
+        MovePurePursuit = new PurePursuit()
+            .WithLookheadDistance(0.35)
+            .WithPath(nodes, edges, Visualization.Theta * Math.PI / 180);
+
+        (NavigationNode node, int index) = MovePurePursuit.GetOnNavNode(Visualization.X, Visualization.Y);
+        if (index >= MovePurePursuit.Waypoints_Value.Count - 1) return;
+
+        double angleFoward = Math.Atan2(MovePurePursuit.Waypoints_Value[index + 1].Y - node.Y, MovePurePursuit.Waypoints_Value[index + 1].X - node.X) * 180 / Math.PI;
+        double angleBacward = Math.Atan2(node.Y - MovePurePursuit.Waypoints_Value[index + 1].Y, node.X - MovePurePursuit.Waypoints_Value[index + 1].X) * 180 / Math.PI;
+        Rotate(node.Direction == RobotDirection.FORWARD ? angleFoward : angleBacward);
+    }
+
+    public void MoveStraight(double x, double y, bool hasLoad = false, RobotDirection? direction = null)
+    {
+        //var headRobotNode = new NavigationNode()
+        //{
+        //    X = Visualization.X * Math.Acos(Visualization.Theta * Math.PI / 180),
+        //    Y = Visualization.Y * Math.Asin(Visualization.Theta * Math.PI / 180),
+        //};
+        //var goalNode = new NavigationNode()
+        //{
+        //    NodeId = "RobotGoal",
+        //    X = x,
+        //    Y = y,
+        //};
+        //var currentRobotNode = new NavigationNode()
+        //{
+        //    NodeId = "RobotCurrentNode",
+        //    X = Visualization.X,
+        //    Y = Visualization.Y,
+        //};
+        //goalNode.Theta = SpaceCompute.GetVectorAngle(currentRobotNode.X, currentRobotNode.Y, headRobotNode.X, headRobotNode.Y, goalNode.X, goalNode.Y) > 90 ?
+        //                                            Math.Atan2(currentRobotNode.Y - goalNode.Y, currentRobotNode.X - goalNode.X) :
+        //                                            Math.Atan2(goalNode.Y - currentRobotNode.Y, goalNode.X - currentRobotNode.X);
+        //currentRobotNode.Theta = goalNode.Theta;
+
+        //MovePID = new PID().WithKp(1.5).WithKi(0.0001).WithKd(0.8);
+        //MoveFuzzy = new FuzzyLogic();
+        //MovePurePursuit = new PurePursuit()
+        //    .WithLookheadDistance(0.25)
+        //    .WithPath([currentRobotNode, goalNode], [new Edge()
+        //{
+        //    EdgeId = "Straight edge",
+        //    Trajectory = new Trajectory()
+        //    {
+        //        Degree = 1,
+        //        ControlPoints = []
+        //    },
+        //    StartNodeId = currentRobotNode.NodeId,
+        //    EndNodeId = goalNode.NodeId,
+        //}], Visualization.Theta);
+
+        //double Angle = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X);
+        //Rotate(Angle * 180 / Math.PI);
+        //UpdateOrder(goalNode.NodeId);
+    }
+
+    public void DockTo(IDetectSession session, bool hasLoad = false, RobotDirection? direction = null)
+    {
+    }
+
+    public void Pause()
+    {
+        ResumeState = NavState;
+        NavState = NavigationState.Paused;
+    }
+
+    public void Resume()
+    {
+        NavState = ResumeState;
+    }
+
+    public void Rotate(double angle)
+    {
+        RotatePID = new PID().WithKp(10).WithKi(0.01).WithKd(0.1);
+        TargetAngle = SpaceCompute.NormalizeDegreeAngle(angle);
+        NavState = NavigationState.Rotating;
+        HandleNavigationStart();
+    }
+
+    protected void UpdateGoal(string goalId)
+    {
+        MovePurePursuit?.UpdateGoal(goalId);
+    }
+
+    public void UpdateOrder(string newBaseNodeId)
+    {
+        var newBaseNode = MovePurePursuit?.OrderNodes.FirstOrDefault(n => n.NodeId == newBaseNodeId);
+        if (newBaseNode is not null && newBaseNode.NodeId != CurrentBaseNode?.NodeId)
+        {
+            CurrentBaseNode = newBaseNode;
+            var newGoalRotate = FindNextRotateGoal();
+            if (newGoalRotate is not null && newGoalRotate.NodeId != GoalRotate?.NodeId)
+            {
+                GoalRotate = newGoalRotate;
+                UpdateGoal(newGoalRotate.NodeId);
+            }
+        }
+    }
+
+    public void RefreshOrder(Node[] nodes, Edge[] edges)
+    {
+        return;
+    }
+
+    public void SetSpeed(double speed)
+    {
+        MaxVelocity = speed;
+    }
+
+    public void Dispose()
+    {
+        HandleNavigationStop();
+        VelocityController.Stop();
+        CurrentBaseNode = null;
+        MovePurePursuit = null;
+        MovePID = null;
+        RotatePID = null;
+        NavDriving = false;
+        OnNavigationFinished?.Invoke(NavState);
+        NavState = NavigationState.Idle;
+        GC.SuppressFinalize(this);
+    }
+
+    public void Refresh()
+    {
+        NavState = NavigationState.Idle;
+    }
+
+    public void Start()
+    {
+        return;
+    }
+
+    public void Stop()
+    {
+        Dispose();
+    }
+
+    /// <summary>
+    /// Tìm node có IsWaitingRotate đầu tiên trong path từ currentNode đến currentGoal
+    /// </summary>
+    protected OrderNode? FindNextRotateGoal()
+    {
+        if (CurrentBaseNode == null || MovePurePursuit is null || MovePurePursuit.OrderNodes.Length == 0) return null;
+
+        int goalIndex = Array.FindIndex(MovePurePursuit.OrderNodes, n => n.NodeId == CurrentBaseNode.NodeId);
+        if (goalIndex == -1) return null;
+
+        int lastNodeIdx = Array.FindIndex(MovePurePursuit.OrderNodes, n => n.NodeId == GoalRotate?.NodeId);
+        lastNodeIdx = lastNodeIdx == -1 ? 0 : lastNodeIdx + 1;
+
+        // Tìm từ node hiện tại đến goal
+        for (int i = lastNodeIdx; i <= goalIndex; i++)
+        {
+            var node = MovePurePursuit.OrderNodes[i];
+
+            // Tìm node có IsWaitingRotate và chưa xử lý
+            if (node.IsWaitRotating && !ProcessedRotations.Contains(node.NodeId))
+            {
+                return node;
+            }
+        }
+        return CurrentBaseNode;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Simulation/SimulationNavigationManager.cs

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

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Simulation/SimulationVelocity.cs

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

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Simulation/SimulationVisualization.cs

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

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/State/RobotEventType.cs

@@ -0,0 +1,71 @@
+namespace RobotNet10.RobotApp.Services.State;
+
+public enum RobotEventType
+{
+    // System Events
+    Initialize,
+    InitializeCompleted,
+    Shutdown,
+    ShutdownCompleted,
+    
+    // Mode Transition Events
+    EnterAuto,
+    EnterManual,
+    EnterService,
+    EnterStop,
+    EnterFault,
+    ExitFault,
+    
+    // Auto Mode Events
+    StartExecution,
+    PauseExecution,
+    ResumeExecution,
+    CancelExecution,
+    CompleteExecution,
+    StartRecovery,
+    CompleteRecovery,
+    RemoteOverride,
+    
+    // Execution Events - Moving
+    StartMoving,
+    StartNavigation,
+    StartAvoidance,
+    StartApproach,
+    StartTracking,
+    StartRepositioning,
+    CompleteMoving,
+    
+    // Execution Events - ACT
+    StartACT,
+    StartDocking,
+    CompleteDocking,
+    StartCharging,
+    CompleteCharging,
+    StartUndocking,
+    CompleteUndocking,
+    StartLoading,
+    CompleteLoading,
+    StartUnloading,
+    CompleteUnloading,
+    StartTechAction,
+    CompleteTechAction,
+    CompleteACT,
+    
+    // Stop Events
+    EmergencyStop,
+    BumperTriggered,
+    ProtectiveStop,
+    ManualStop,
+    ReleaseStop,
+    
+    // Fault Events
+    NavigationFault,
+    LocalizationFault,
+    ShielfFault,
+    BatteryFault,
+    DriverFault,
+    PeripheralsFault,
+    SafetyFault,
+    CommunicationFault,
+    FaultResolved,
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/State/RobotStateMachine.cs

@@ -0,0 +1,336 @@
+using Appccelerate.StateMachine;
+using Appccelerate.StateMachine.AsyncMachine;
+using Appccelerate.StateMachine.AsyncMachine.Events;
+
+namespace RobotNet10.RobotApp.Services.State;
+
+public class RobotStateMachine(Logger<RobotStateMachine> Logger, RobotStateMachineExecute StateExecute)
+{
+    private AsyncPassiveStateMachine<RobotStateType, RobotEventType>? _stateMachine;
+    private RobotStateType _currentState = RobotStateType.System;
+    public bool IsInitialized { get; private set; } = false;
+
+    public RobotStateType CurrentState => _currentState;
+    public event EventHandler<StateChangedEventArgs>? StateChanged;
+
+    // Dictionary để track hierarchy relationships cho helper methods
+    private readonly Dictionary<RobotStateType, RobotStateType> _stateHierarchies = [];
+
+    public async Task InitializeAsync()
+    {
+        if (IsInitialized) return;
+
+        var builder = new StateMachineDefinitionBuilder<RobotStateType, RobotEventType>();
+
+        // Build hierarchy map (chỉ cần track parent-child relationship)
+        BuildHierarchyMap();
+
+        // ===========================
+        // ROOT LEVEL - Hierarchical States
+        // ===========================
+
+        // System State Hierarchy
+        builder.In(RobotStateType.System)
+            .On(RobotEventType.EnterAuto).Goto(RobotStateType.Auto)
+            .On(RobotEventType.EnterManual).Goto(RobotStateType.Manual)
+            .On(RobotEventType.EnterService).Goto(RobotStateType.Service)
+            .On(RobotEventType.RemoteOverride).Goto(RobotStateType.Remote_Override)
+            .On(RobotEventType.EnterStop).Goto(RobotStateType.Stop)
+            .On(RobotEventType.EnterFault).Goto(RobotStateType.Fault);
+
+        builder.DefineHierarchyOn(RobotStateType.System)
+            .WithHistoryType(HistoryType.Deep)
+            .WithInitialSubState(RobotStateType.Initializing)
+            .WithSubState(RobotStateType.Standby)
+            .WithSubState(RobotStateType.Shutting_Down);
+
+        // Auto State Hierarchy
+        builder.In(RobotStateType.Auto)
+            .On(RobotEventType.EnterManual).Goto(RobotStateType.Manual)
+            .On(RobotEventType.EnterService).Goto(RobotStateType.Service)
+            .On(RobotEventType.RemoteOverride).Goto(RobotStateType.Remote_Override)
+            .On(RobotEventType.EnterStop).Goto(RobotStateType.Stop)
+            .On(RobotEventType.EnterFault).Goto(RobotStateType.Fault);
+
+        builder.DefineHierarchyOn(RobotStateType.Auto)
+            .WithHistoryType(HistoryType.Deep)
+            .WithInitialSubState(RobotStateType.Idle)
+            .WithSubState(RobotStateType.Executing)
+            .WithSubState(RobotStateType.Paused)
+            .WithSubState(RobotStateType.Canceling)
+            .WithSubState(RobotStateType.Recovering);
+
+        // Manual State
+        builder.In(RobotStateType.Manual)
+            .ExecuteOnEntry(() => { OnEnterState(RobotStateType.Manual); StateExecute.EntryManual(); })
+            .ExecuteOnExit(StateExecute.ExitManual)
+            .On(RobotEventType.EnterAuto).Goto(RobotStateType.Auto)
+            .On(RobotEventType.EnterService).Goto(RobotStateType.Service)
+            .On(RobotEventType.RemoteOverride).Goto(RobotStateType.Remote_Override)
+            .On(RobotEventType.EnterStop).Goto(RobotStateType.Stop)
+            .On(RobotEventType.EnterFault).Goto(RobotStateType.Fault);
+
+        // Service State
+        builder.In(RobotStateType.Service)
+            .ExecuteOnEntry(() => { OnEnterState(RobotStateType.Service); StateExecute.EntryService(); })
+            .ExecuteOnExit(StateExecute.ExitService)
+            .On(RobotEventType.EnterAuto).Goto(RobotStateType.Auto)
+            .On(RobotEventType.EnterManual).Goto(RobotStateType.Manual)
+            .On(RobotEventType.RemoteOverride).Goto(RobotStateType.Remote_Override)
+            .On(RobotEventType.EnterStop).Goto(RobotStateType.Stop)
+            .On(RobotEventType.EnterFault).Goto(RobotStateType.Fault);
+
+        // Stop State
+        builder.In(RobotStateType.Stop)
+            .ExecuteOnEntry(() => { OnEnterState(RobotStateType.Stop); StateExecute.EntryStop(); })
+            .ExecuteOnExit(StateExecute.ExitStop)
+            .On(RobotEventType.ReleaseStop).Goto(RobotStateType.System)
+            .On(RobotEventType.EnterFault).Goto(RobotStateType.Fault);
+
+        // Fault State
+        builder.In(RobotStateType.Fault)
+            .ExecuteOnEntry(() => { OnEnterState(RobotStateType.Fault); StateExecute.EntryFault(); })
+            .ExecuteOnExit(StateExecute.ExitFault)
+            .On(RobotEventType.ExitFault).Goto(RobotStateType.System);
+
+        // Remote_Override State (top-level, peer of Service)
+        builder.In(RobotStateType.Remote_Override)
+            .ExecuteOnEntry(() => { OnEnterState(RobotStateType.Remote_Override); StateExecute.EntryRemoteOverride(); })
+            .ExecuteOnExit(StateExecute.ExitRemoteOverride)
+            .On(RobotEventType.EnterAuto).Goto(RobotStateType.Auto)
+            .On(RobotEventType.EnterManual).Goto(RobotStateType.Manual)
+            .On(RobotEventType.EnterService).Goto(RobotStateType.Service)
+            .On(RobotEventType.EnterStop).Goto(RobotStateType.Stop)
+            .On(RobotEventType.EnterFault).Goto(RobotStateType.Fault);
+
+        // ===========================
+        // SYSTEM SUB-STATES
+        // ===========================
+
+        builder.In(RobotStateType.Initializing)
+            .ExecuteOnEntry(() => { OnEnterState(RobotStateType.Initializing); StateExecute.EntryInitializing(); })
+            .On(RobotEventType.InitializeCompleted).Goto(RobotStateType.Standby)
+            .On(RobotEventType.EnterFault).Goto(RobotStateType.Fault);
+
+        builder.In(RobotStateType.Standby)
+            .ExecuteOnEntry(() => { OnEnterState(RobotStateType.Standby); StateExecute.EntryStandby(); })
+            .On(RobotEventType.Shutdown).Goto(RobotStateType.Shutting_Down);
+
+        builder.In(RobotStateType.Shutting_Down)
+            .ExecuteOnEntry(() => { OnEnterState(RobotStateType.Shutting_Down); StateExecute.EntryShuttingDown(); })
+            .ExecuteOnExit(StateExecute.ExitShuttingDown)
+            .On(RobotEventType.ShutdownCompleted).Goto(RobotStateType.Standby);
+
+        // ===========================
+        // AUTO SUB-STATES
+        // ===========================
+
+        builder.In(RobotStateType.Idle)
+            .ExecuteOnEntry(() => { OnEnterState(RobotStateType.Idle); StateExecute.EntryIdle(); })
+            .On(RobotEventType.StartExecution).Goto(RobotStateType.Executing);
+
+        // Executing State Hierarchy
+        builder.In(RobotStateType.Executing)
+            .ExecuteOnEntry(() => { OnEnterState(RobotStateType.Executing); StateExecute.EntryExecuting(); })
+            .ExecuteOnExit(StateExecute.ExitExecuting)
+            .On(RobotEventType.PauseExecution).Goto(RobotStateType.Paused)
+            .On(RobotEventType.CancelExecution).Goto(RobotStateType.Canceling)
+            .On(RobotEventType.CompleteExecution).Goto(RobotStateType.Idle);
+
+        builder.DefineHierarchyOn(RobotStateType.Executing)
+            .WithHistoryType(HistoryType.Deep)
+            .WithInitialSubState(RobotStateType.Moving)
+            .WithSubState(RobotStateType.ACT);
+
+        builder.In(RobotStateType.Paused)
+            .ExecuteOnEntry(() => { OnEnterState(RobotStateType.Paused); StateExecute.EntryPaused(); })
+            .ExecuteOnExit(StateExecute.ExitPaused)
+            .On(RobotEventType.ResumeExecution).Goto(RobotStateType.Executing)
+            .On(RobotEventType.CancelExecution).Goto(RobotStateType.Canceling);
+
+        builder.In(RobotStateType.Canceling)
+            .ExecuteOnEntry(() => { OnEnterState(RobotStateType.Canceling); StateExecute.EntryCanceling(); })
+            .ExecuteOnExit(StateExecute.ExitCanceling)
+            .On(RobotEventType.CompleteExecution).Goto(RobotStateType.Idle);
+
+        builder.In(RobotStateType.Recovering)
+            .ExecuteOnEntry(() => { OnEnterState(RobotStateType.Recovering); StateExecute.EntryRecovering(); })
+            .On(RobotEventType.CompleteRecovery).Goto(RobotStateType.Idle);
+
+        // ===========================
+        // EXECUTING SUB-STATES
+        // ===========================
+
+        // Moving State Hierarchy
+        builder.In(RobotStateType.Moving)
+            .ExecuteOnEntry(() => { OnEnterState(RobotStateType.Moving); StateExecute.EntryMoving(); })
+            .ExecuteOnExit(StateExecute.ExitMoving)
+            .On(RobotEventType.StartACT).Goto(RobotStateType.ACT)
+            .On(RobotEventType.CompleteMoving).Goto(RobotStateType.Idle);
+
+        // ACT State Hierarchy
+        builder.In(RobotStateType.ACT)
+            .ExecuteOnEntry(() => { OnEnterState(RobotStateType.ACT); StateExecute.EntryACT(); })
+            .ExecuteOnExit(StateExecute.ExitACT)
+            .On(RobotEventType.StartMoving).Goto(RobotStateType.Moving)
+            .On(RobotEventType.CompleteACT).Goto(RobotStateType.Idle);
+
+        builder.DefineHierarchyOn(RobotStateType.ACT)
+            .WithHistoryType(HistoryType.Deep)
+            .WithInitialSubState(RobotStateType.Docking)
+            .WithSubState(RobotStateType.Docked)
+            .WithSubState(RobotStateType.Charging)
+            .WithSubState(RobotStateType.Undocking)
+            .WithSubState(RobotStateType.Loading)
+            .WithSubState(RobotStateType.Unloading)
+            .WithSubState(RobotStateType.TechAction);
+
+        // ===========================
+        // ACT SUB-STATES
+        // ===========================
+
+        builder.In(RobotStateType.Docking)
+            .ExecuteOnEntry(() => { OnEnterState(RobotStateType.Docking); StateExecute.EntryDocking(); })
+            .On(RobotEventType.CompleteDocking).Goto(RobotStateType.Docked);
+
+        builder.In(RobotStateType.Docked)
+            .ExecuteOnEntry(() => { OnEnterState(RobotStateType.Docked); StateExecute.EntryDocked(); })
+            .On(RobotEventType.StartCharging).Goto(RobotStateType.Charging)
+            .On(RobotEventType.StartUndocking).Goto(RobotStateType.Undocking)
+            .On(RobotEventType.StartLoading).Goto(RobotStateType.Loading)
+            .On(RobotEventType.StartUnloading).Goto(RobotStateType.Unloading);
+
+        builder.In(RobotStateType.Charging)
+            .ExecuteOnEntry(() => { OnEnterState(RobotStateType.Charging); StateExecute.EntryCharging(); })
+            .ExecuteOnExit(StateExecute.ExitCharging)
+            .On(RobotEventType.CompleteCharging).Goto(RobotStateType.Docked);
+
+        builder.In(RobotStateType.Undocking)
+            .ExecuteOnEntry(() => { OnEnterState(RobotStateType.Undocking); StateExecute.EntryUndocking(); })
+            .On(RobotEventType.CompleteUndocking).Goto(RobotStateType.Docking);
+
+        builder.In(RobotStateType.Loading)
+            .ExecuteOnEntry(() => { OnEnterState(RobotStateType.Loading); StateExecute.EntryLoading(); })
+            .On(RobotEventType.CompleteLoading).Goto(RobotStateType.Docked);
+
+        builder.In(RobotStateType.Unloading)
+            .ExecuteOnEntry(() => { OnEnterState(RobotStateType.Unloading); StateExecute.EntryUnloading(); })
+            .On(RobotEventType.CompleteUnloading).Goto(RobotStateType.Docked);
+
+        builder.In(RobotStateType.TechAction)
+            .ExecuteOnEntry(() => { OnEnterState(RobotStateType.TechAction); StateExecute.EntryTechAction(); })
+            .On(RobotEventType.CompleteTechAction).Goto(RobotStateType.Docked);
+
+        // ===========================
+        // CREATE STATE MACHINE
+        // ===========================
+
+        _stateMachine = builder
+            .WithInitialState(RobotStateType.System)
+            .Build()
+            .CreatePassiveStateMachine();
+
+        // Subscribe to state change events
+        _stateMachine.TransitionCompleted += OnTransitionCompleted;
+
+        // Set IsInitialized = true TRƯỚC khi Start() để tránh deadlock
+        // Vì EntryInitializing() có thể gọi ModuleInitializeAsync() chờ IsInitialized
+        IsInitialized = true;
+
+        // Start state machine
+        await _stateMachine.Start();
+
+        Logger.Info($"State Machine initialized successfully with current state: {CurrentState}");
+    }
+
+    /// <summary>
+    /// Build hierarchy map - chỉ để track parent-child relationships cho helper methods
+    /// Không cần build transitions vì Appccelerate tự động xử lý
+    /// </summary>
+    private void BuildHierarchyMap()
+    {
+        // System sub-states
+        _stateHierarchies[RobotStateType.Initializing] = RobotStateType.System;
+        _stateHierarchies[RobotStateType.Standby] = RobotStateType.System;
+        _stateHierarchies[RobotStateType.Shutting_Down] = RobotStateType.System;
+
+        // Auto sub-states
+        _stateHierarchies[RobotStateType.Idle] = RobotStateType.Auto;
+        _stateHierarchies[RobotStateType.Executing] = RobotStateType.Auto;
+        _stateHierarchies[RobotStateType.Paused] = RobotStateType.Auto;
+        _stateHierarchies[RobotStateType.Canceling] = RobotStateType.Auto;
+        _stateHierarchies[RobotStateType.Recovering] = RobotStateType.Auto;
+
+        // Executing sub-states
+        _stateHierarchies[RobotStateType.Moving] = RobotStateType.Executing;
+        _stateHierarchies[RobotStateType.ACT] = RobotStateType.Executing;
+
+        // ACT sub-states
+        _stateHierarchies[RobotStateType.Docking] = RobotStateType.ACT;
+        _stateHierarchies[RobotStateType.Docked] = RobotStateType.ACT;
+        _stateHierarchies[RobotStateType.Charging] = RobotStateType.ACT;
+        _stateHierarchies[RobotStateType.Undocking] = RobotStateType.ACT;
+        _stateHierarchies[RobotStateType.Loading] = RobotStateType.ACT;
+        _stateHierarchies[RobotStateType.Unloading] = RobotStateType.ACT;
+        _stateHierarchies[RobotStateType.TechAction] = RobotStateType.ACT;
+    }
+
+    public void Initialize() => InitializeAsync().GetAwaiter().GetResult();
+
+    public async Task FireAsync(RobotEventType eventType)
+    {
+        if (!IsInitialized)
+        {
+            Logger.Warning("State Machine not initialized!");
+            return;
+        }
+
+        try
+        {
+            if (_stateMachine != null)
+            {
+                await _stateMachine.Fire(eventType);
+            }
+        }
+        catch (Exception ex)
+        {
+            Logger.Error($"Fire event {eventType} error: {ex.Message}");
+        }
+    }
+
+    public void Fire(RobotEventType eventType) => FireAsync(eventType).GetAwaiter().GetResult();
+
+    public bool IsInState(RobotStateType state)
+    {
+        if (!IsInitialized) return false;
+        if (CurrentState == state) return true;
+        if(_stateHierarchies.TryGetValue(CurrentState, out RobotStateType parentState))
+        {
+            if(parentState == state) return true;
+
+            while (_stateHierarchies.TryGetValue(parentState, out parentState))
+            {
+                if (parentState == state) return true;
+            }
+        }
+
+        return false;
+    }
+
+    private void OnEnterState(RobotStateType state)
+    {
+        _currentState = state;
+    }
+
+    private void OnTransitionCompleted(object? sender, TransitionCompletedEventArgs<RobotStateType, RobotEventType> e)
+    {
+        Logger.Info($"State Transition: {e.StateId} -> Event: {e.EventId}");
+        StateChanged?.Invoke(this, new StateChangedEventArgs(e.StateId, e.EventId));
+    }
+}
+
+public class StateChangedEventArgs(RobotStateType newState, RobotEventType eventType) : EventArgs
+{
+    public RobotStateType NewState { get; } = newState;
+    public RobotEventType EventType { get; } = eventType;
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/State/RobotStateMachineExecute.cs

@@ -0,0 +1,890 @@
+using System.Threading;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Motion;
+using RobotNet10.RobotApp.Services.Robot;
+using RobotNet10.Shared.Geometry;
+
+namespace RobotNet10.RobotApp.Services.State;
+
+/// <summary>
+/// Class chứa các implementation methods cho Entry/Exit actions của State Machine
+/// Sử dụng IServiceProvider để có thể inject các services khác khi cần
+/// </summary>
+public class RobotStateMachineExecute(IServiceScopeFactory ServiceScopeFactory, IPlcController PlcController, Logger<RobotStateMachineExecute> Logger)
+{
+    // ===========================
+    // SYSTEM STATE HANDLERS
+    // ===========================
+
+    /// <summary>
+    /// Entry: System -> Initializing
+    /// Thực hiện khởi tạo các component của robot
+    /// </summary>
+    public async Task EntryInitializingAsync()
+    {
+        Logger.Info("==> Entry Initializing State");
+
+        try
+        {
+            // TODO: Khởi tạo các services
+            // - Kiểm tra kết nối phần cứng
+            // - Khởi tạo Driver
+            // - Khởi tạo Sensors
+            // - Load cấu hình
+
+            using var scope = ServiceScopeFactory.CreateAsyncScope();
+            var robotController = scope.ServiceProvider.GetRequiredService<RobotController>();
+            await robotController.ModuleInitializeAsync();
+        }
+        catch (Exception ex)
+        {
+            Logger.Error($"Initialization error: {ex.Message}");
+            throw;
+        }
+    }
+
+    public void EntryInitializing() => EntryInitializingAsync().GetAwaiter().GetResult();
+
+    /// <summary>
+    /// Entry: System -> Standby
+    /// Robot đã sẵn sàng hoạt động
+    /// </summary>
+    public void EntryStandby()
+    {
+        Logger.Info("==> Entry Standby State");
+        PlcController.SetSystemState(SystemState.IDLE);
+
+        using var scope = ServiceScopeFactory.CreateAsyncScope();
+        var stateService = scope.ServiceProvider.GetRequiredService<RobotStates>();
+        var visualizationService = scope.ServiceProvider.GetRequiredService<RobotVisualization>();
+        stateService.Start();
+        visualizationService.Start();
+    }
+
+    /// <summary>
+    /// Entry: System -> Shutting_Down
+    /// Bắt đầu quá trình tắt máy
+    /// </summary>
+    public async Task EntryShuttingDownAsync()
+    {
+        Logger.Info("==> Entry Shutting Down State");
+
+        try
+        {
+            // TODO:
+            // - Lưu trạng thái hiện tại
+            // - Dừng tất cả các task đang chạy
+            // - Ngắt kết nối an toàn
+
+            using var scope = ServiceScopeFactory.CreateAsyncScope();
+            var robotController = scope.ServiceProvider.GetRequiredService<RobotController>();
+            robotController.StopHandler();
+
+            Logger.Info("Shutdown completed");
+        }
+        catch (Exception ex)
+        {
+            Logger.Error($"Shutdown error: {ex.Message}");
+        }
+    }
+
+    public void EntryShuttingDown() => EntryShuttingDownAsync().GetAwaiter().GetResult();
+
+    /// <summary>
+    /// Exit: System -> Shutting_Down
+    /// </summary>
+    public void ExitShuttingDown()
+    {
+        Logger.Info("<== Exit Shutting Down State");
+    }
+
+    // ===========================
+    // AUTO MODE STATE HANDLERS
+    // ===========================
+
+    /// <summary>
+    /// Entry: Auto -> Idle
+    /// Robot sẵn sàng nhận nhiệm vụ mới
+    /// </summary>
+    public void EntryIdle()
+    {
+        Logger.Info("==> Entry Idle State");
+        PlcController.SetSystemState(SystemState.IDLE);
+    }
+
+    /// <summary>
+    /// Entry: Auto -> Executing
+    /// Bắt đầu thực hiện nhiệm vụ
+    /// </summary>
+    public void EntryExecuting()
+    {
+        Logger.Info("==> Entry Executing State");
+        using var scope = ServiceScopeFactory.CreateAsyncScope();
+        var PlcController = scope.ServiceProvider.GetRequiredService<IPlcController>();
+        PlcController.SetOperationState(OperationState.Move);
+        // TODO:
+        // - Lấy nhiệm vụ từ queue
+        // - Khởi tạo execution context
+        // - Bắt đầu timer theo dõi
+        // - Cập nhật VDA5050 state = "EXECUTING"
+    }
+
+    /// <summary>
+    /// Exit: Auto -> Executing
+    /// </summary>
+    public void ExitExecuting()
+    {
+        Logger.Info("<== Exit Executing State");
+
+        using var scope = ServiceScopeFactory.CreateAsyncScope();
+        var PlcController = scope.ServiceProvider.GetRequiredService<IPlcController>();
+        PlcController.SetOperationState(OperationState.None);
+
+        // TODO:
+        // - Cleanup execution context
+        // - Dừng timer
+    }
+
+    /// <summary>
+    /// Entry: Auto -> Paused
+    /// Tạm dừng thực hiện nhiệm vụ
+    /// </summary>
+    public void EntryPaused()
+    {
+        Logger.Info("==> Entry Paused State");
+
+        // TODO:
+        // - Lưu trạng thái hiện tại
+        // - Dừng robot
+        // - Cập nhật VDA5050 state = "PAUSED"
+
+        // Dừng robot
+        StopRobot();
+    }
+
+    /// <summary>
+    /// Exit: Auto -> Paused
+    /// Resume từ trạng thái pause
+    /// </summary>
+    public void ExitPaused()
+    {
+        Logger.Info("<== Exit Paused State");
+
+        // TODO:
+        // - Khôi phục trạng thái
+        // - Chuẩn bị tiếp tục thực hiện
+    }
+
+    /// <summary>
+    /// Entry: Auto -> Canceling
+    /// Đang hủy nhiệm vụ
+    /// </summary>
+    public void EntryCanceling()
+    {
+        Logger.Info("==> Entry Canceling State");
+
+        // TODO:
+        // - Dừng robot ngay lập tức
+        // - Hủy tất cả các task con
+        // - Cleanup resources
+
+        StopRobot();
+    }
+
+    /// <summary>
+    /// Exit: Auto -> Canceling
+    /// Hoàn tất việc hủy
+    /// </summary>
+    public void ExitCanceling()
+    {
+        Logger.Info("<== Exit Canceling State");
+
+        // TODO:
+        // - Gửi thông báo nhiệm vụ đã hủy
+        // - Reset execution context
+        // - Cập nhật VDA5050 với error/cancelled
+    }
+
+    /// <summary>
+    /// Entry: Auto -> Recovering
+    /// Đang khôi phục từ lỗi
+    /// </summary>
+    public async Task EntryRecoveringAsync()
+    {
+        Logger.Info("==> Entry Recovering State");
+
+        try
+        {
+            // TODO:
+            // - Phân tích lỗi
+            // - Thực hiện recovery procedure
+            // - Kiểm tra trạng thái hệ thống
+
+            Logger.Info("Analyzing error...");
+            await Task.Delay(100);
+
+            Logger.Info("Recovering system...");
+            await Task.Delay(500);
+
+            Logger.Info("Recovery succeeded");
+        }
+        catch (Exception ex)
+        {
+            Logger.Error($"Recovery error: {ex.Message}");
+        }
+    }
+
+    public void EntryRecovering() => EntryRecoveringAsync().GetAwaiter().GetResult();
+
+    /// <summary>
+    /// Entry: Auto -> Remote_Override (OVERRIDE)
+    /// Chuyển sang chế độ điều khiển từ xa - cho phép override tất cả safety
+    /// </summary>
+    public void EntryRemoteOverride()
+    {
+        Logger.Info("==> Entry Remote Override State (OVERRIDE)");
+        PlcController.SetSystemState(SystemState.OVERRIDE);
+        StopRobot();
+
+        // Set ManualControlService state to Override - cho phép điều khiển với full override
+        try
+        {
+            using var scope = ServiceScopeFactory.CreateScope();
+            var manualControlService = scope.ServiceProvider.GetRequiredService<ManualControlService>();
+            manualControlService.SetState(ManualControlState.Override);
+            Logger.Info("ManualControlService set to Override state");
+        }
+        catch (Exception ex)
+        {
+            Logger.Error($"Error setting ManualControlService to Override: {ex.Message}");
+        }
+    }
+
+    /// <summary>
+    /// Exit: Auto -> Remote_Override (OVERRIDE)
+    /// </summary>
+    public void ExitRemoteOverride()
+    {
+        Logger.Info("<== Exit Remote Override State (OVERRIDE)");
+
+        // Clear ManualControlService external state
+        try
+        {
+            using var scope = ServiceScopeFactory.CreateScope();
+            var manualControlService = scope.ServiceProvider.GetRequiredService<ManualControlService>();
+            manualControlService.ClearExternalState();
+            Logger.Info("ManualControlService external state cleared");
+        }
+        catch (Exception ex)
+        {
+            Logger.Error($"Error clearing ManualControlService external state: {ex.Message}");
+        }
+    }
+
+    // ===========================
+    // EXECUTING - MOVING STATE HANDLERS
+    // ===========================
+
+    /// <summary>
+    /// Entry: Executing -> Moving
+    /// Bắt đầu di chuyển
+    /// </summary>
+    public void EntryMoving()
+    {
+        Logger.Info("==> Entry Moving State");
+        PlcController.SetSystemState(SystemState.PROCCESSING);
+    }
+
+    /// <summary>
+    /// Exit: Executing -> Moving
+    /// </summary>
+    public void ExitMoving()
+    {
+        Logger.Info("<== Exit Moving State");
+
+        // TODO:
+        // - Dừng robot
+        // - Lưu vị trí cuối cùng
+
+        StopRobot();
+    }
+
+    /// <summary>
+    /// Entry: Moving -> Navigation
+    /// Chế độ navigation bình thường
+    /// </summary>
+    public void EntryNavigation()
+    {
+        Logger.Info("==> Entry Navigation State");
+
+        // TODO:
+        // - Load path từ planner
+        // - Bắt đầu path following
+        // - Monitor obstacles
+    }
+
+    /// <summary>
+    /// Entry: Moving -> Avoidance
+    /// Đang tránh chướng ngại vật
+    /// </summary>
+    public void EntryAvoidance()
+    {
+        Logger.Info("==> Entry Avoidance State");
+
+        // TODO:
+        // - Giảm tốc độ
+        // - Tính toán đường tránh
+        // - Theo dõi obstacle
+    }
+
+    /// <summary>
+    /// Exit: Moving -> Avoidance
+    /// </summary>
+    public void ExitAvoidance()
+    {
+        Logger.Info("<== Exit Avoidance State");
+
+        // TODO:
+        // - Quay lại tốc độ bình thường
+        // - Resume path chính
+    }
+
+    /// <summary>
+    /// Entry: Moving -> Approach
+    /// Tiếp cận mục tiêu (precision mode)
+    /// </summary>
+    public void EntryApproach()
+    {
+        Logger.Info("==> Entry Approach State");
+
+        // TODO:
+        // - Chuyển sang precision mode
+        // - Giảm tốc độ tối đa
+        // - Sử dụng sensors chính xác cao
+    }
+
+    /// <summary>
+    /// Entry: Moving -> Tracking
+    /// Theo dõi mục tiêu động
+    /// </summary>
+    public void EntryTracking()
+    {
+        Logger.Info("==> Entry Tracking State");
+
+        // TODO:
+        // - Bật target tracking
+        // - Theo dõi vị trí mục tiêu
+        // - Điều chỉnh trajectory theo realtime
+    }
+
+    /// <summary>
+    /// Entry: Moving -> Repositioning
+    /// Điều chỉnh lại vị trí
+    /// </summary>
+    public void EntryRepositioning()
+    {
+        Logger.Info("==> Entry Repositioning State");
+
+        // TODO:
+        // - Tính toán vị trí mong muốn
+        // - Di chuyển điều chỉnh nhỏ
+        // - Kiểm tra orientation
+    }
+
+    // ===========================
+    // EXECUTING - ACT STATE HANDLERS
+    // ===========================
+
+    /// <summary>
+    /// Entry: Executing -> ACT
+    /// Bắt đầu thực hiện action
+    /// </summary>
+    public void EntryACT()
+    {
+        Logger.Info("==> Entry ACT State");
+
+        // TODO:
+        // - Dừng robot hoàn toàn
+        // - Chuẩn bị thực hiện action
+        // - Kiểm tra vị trí chính xác
+
+        StopRobot();
+    }
+
+    /// <summary>
+    /// Exit: Executing -> ACT
+    /// </summary>
+    public void ExitACT()
+    {
+        Logger.Info("<== Exit ACT State");
+
+        // TODO:
+        // - Cleanup action resources
+        // - Kiểm tra kết quả action
+    }
+
+    /// <summary>
+    /// Entry: ACT -> Docking
+    /// Đang thực hiện docking
+    /// </summary>
+    public async Task EntryDockingAsync()
+    {
+        Logger.Info("==> Entry Docking State");
+        PlcController.SetSystemState(SystemState.DOCKING);
+
+        try
+        {
+            // TODO:
+            // - Tìm vị trí dock station
+            // - Align với dock
+            // - Di chuyển vào dock từ từ
+
+            Logger.Info("Searching for dock station...");
+            await Task.Delay(200);
+
+            Logger.Info("Aligning with dock...");
+            await Task.Delay(300);
+
+            Logger.Info("Moving into dock...");
+            await Task.Delay(500);
+
+            Logger.Info("Docking succeeded");
+        }
+        catch (Exception ex)
+        {
+            Logger.Error($"Docking error: {ex.Message}");
+            throw;
+        }
+    }
+
+    public void EntryDocking() => EntryDockingAsync().GetAwaiter().GetResult();
+
+    /// <summary>
+    /// Entry: ACT -> Docked
+    /// Đã docked thành công
+    /// </summary>
+    public void EntryDocked()
+    {
+        Logger.Info("==> Entry Docked State");
+        PlcController.SetSystemState(SystemState.DOCKING);
+    }
+
+    /// <summary>
+    /// Entry: ACT -> Charging
+    /// Đang sạc pin
+    /// </summary>
+    public async Task EntryChargingAsync()
+    {
+        Logger.Info("==> Entry Charging State");
+        PlcController.SetSystemState(SystemState.CHARGING);
+
+        try
+        {
+            // TODO:
+            // - Bắt đầu charging
+            // - Monitor battery level
+            // - Monitor charging current/voltage
+
+            Logger.Info("Connecting charging power...");
+            await Task.Delay(100);
+
+            Logger.Info("Starting battery charging...");
+            // Charging loop sẽ được xử lý bởi battery service
+        }
+        catch (Exception ex)
+        {
+            Logger.Error($"Charging error: {ex.Message}");
+            throw;
+        }
+    }
+
+    public void EntryCharging() => EntryChargingAsync().GetAwaiter().GetResult();
+
+    /// <summary>
+    /// Exit: ACT -> Charging
+    /// </summary>
+    public void ExitCharging()
+    {
+        Logger.Info("<== Exit Charging State");
+
+        // TODO:
+        // - Dừng charging
+        // - Ngắt kết nối nguồn an toàn
+        // - Log battery level
+    }
+
+    /// <summary>
+    /// Entry: ACT -> Undocking
+    /// Đang rời khỏi dock
+    /// </summary>
+    public async Task EntryUndockingAsync()
+    {
+        Logger.Info("==> Entry Undocking State");
+        PlcController.SetSystemState(SystemState.DOCKING);
+
+        try
+        {
+            // TODO:
+            // - Kiểm tra an toàn
+            // - Ngắt kết nối với dock
+            // - Di chuyển ra khỏi dock
+
+            Logger.Info("Checking safety...");
+            await Task.Delay(100);
+
+            Logger.Info("Disconnecting dock...");
+            await Task.Delay(200);
+
+            Logger.Info("Moving out of dock...");
+            await Task.Delay(500);
+
+            Logger.Info("Undocking succeeded");
+        }
+        catch (Exception ex)
+        {
+            Logger.Error($"Undocking error: {ex.Message}");
+            throw;
+        }
+    }
+
+    public void EntryUndocking() => EntryUndockingAsync().GetAwaiter().GetResult();
+
+    /// <summary>
+    /// Entry: ACT -> Loading
+    /// Đang tải hàng
+    /// </summary>
+    public async Task EntryLoadingAsync()
+    {
+        Logger.Info("==> Entry Loading State");
+        PlcController.SetSystemState(SystemState.PROCCESSING);
+
+        try
+        {
+            // TODO:
+            // - Kiểm tra payload capacity
+            // - Điều khiển loading mechanism
+            // - Xác nhận hàng đã được tải
+
+            Logger.Info("Preparing loading...");
+            await Task.Delay(200);
+
+            Logger.Info("Loading cargo...");
+            await Task.Delay(1000);
+
+            Logger.Info("Loading completed");
+        }
+        catch (Exception ex)
+        {
+            Logger.Error($"Loading error: {ex.Message}");
+            throw;
+        }
+    }
+
+    public void EntryLoading() => EntryLoadingAsync().GetAwaiter().GetResult();
+
+    /// <summary>
+    /// Entry: ACT -> Unloading
+    /// Đang dỡ hàng
+    /// </summary>
+    public async Task EntryUnloadingAsync()
+    {
+        Logger.Info("==> Entry Unloading State");
+        PlcController.SetSystemState(SystemState.PROCCESSING);
+
+        try
+        {
+            // TODO:
+            // - Kiểm tra vị trí unload
+            // - Điều khiển unloading mechanism
+            // - Xác nhận hàng đã được dỡ
+
+            Logger.Info("Preparing unloading...");
+            await Task.Delay(200);
+
+            Logger.Info("Unloading cargo...");
+            await Task.Delay(1000);
+
+            Logger.Info("Unloading completed");
+        }
+        catch (Exception ex)
+        {
+            Logger.Error($"Unloading error: {ex.Message}");
+            throw;
+        }
+    }
+
+    public void EntryUnloading() => EntryUnloadingAsync().GetAwaiter().GetResult();
+
+    /// <summary>
+    /// Entry: ACT -> TechAction
+    /// Thực hiện các action kỹ thuật đặc biệt
+    /// </summary>
+    public async Task EntryTechActionAsync()
+    {
+        Logger.Info("==> Entry Tech Action State");
+        PlcController.SetSystemState(SystemState.PROCCESSING);
+
+        try
+        {
+            // TODO:
+            // - Xác định loại tech action
+            // - Thực hiện action tương ứng
+            // - Log kết quả
+
+            Logger.Info("Executing tech action...");
+            await Task.Delay(500);
+
+            Logger.Info("Tech action completed");
+        }
+        catch (Exception ex)
+        {
+            Logger.Error($"Tech action error: {ex.Message}");
+            throw;
+        }
+    }
+
+    public void EntryTechAction() => EntryTechActionAsync().GetAwaiter().GetResult();
+
+    // ===========================
+    // MODE TRANSITION HANDLERS
+    // ===========================
+
+    /// <summary>
+    /// Entry: Manual Mode
+    /// Chuyển sang chế độ thủ công
+    /// </summary>
+    public void EntryManual()
+    {
+        Logger.Info("==> Entry Manual Mode");
+        PlcController.SetSystemState(SystemState.MANUAL);
+        StopRobot();
+    }
+
+    /// <summary>
+    /// Exit: Manual Mode
+    /// </summary>
+    public void ExitManual()
+    {
+        Logger.Info("<== Exit Manual Mode");
+
+        // TODO:
+        // - Kiểm tra an toàn
+        // - Tắt manual control
+        // - Bật lại autonomous control
+    }
+
+    /// <summary>
+    /// Entry: Service Mode (MAINTENANCE)
+    /// Chuyển sang chế độ bảo trì - cho phép điều khiển robot từ RF Handle
+    /// </summary>
+    public void EntryService()
+    {
+        Logger.Info("==> Entry Service Mode (MAINTENANCE)");
+        PlcController.SetSystemState(SystemState.MAINTENANCE);
+        StopRobot();
+
+        // Set ManualControlService state to Maintenance - cho phép điều khiển từ RF Handle
+        try
+        {
+            using var scope = ServiceScopeFactory.CreateScope();
+            var manualControlService = scope.ServiceProvider.GetRequiredService<ManualControlService>();
+            manualControlService.SetState(ManualControlState.Maintenance);
+            Logger.Info("ManualControlService set to Maintenance state");
+        }
+        catch (Exception ex)
+        {
+            Logger.Error($"Error setting ManualControlService to Maintenance: {ex.Message}");
+        }
+    }
+
+    /// <summary>
+    /// Exit: Service Mode (MAINTENANCE)
+    /// </summary>
+    public void ExitService()
+    {
+        Logger.Info("<== Exit Service Mode (MAINTENANCE)");
+
+        // Clear ManualControlService external state
+        try
+        {
+            using var scope = ServiceScopeFactory.CreateScope();
+            var manualControlService = scope.ServiceProvider.GetRequiredService<ManualControlService>();
+            manualControlService.ClearExternalState();
+            Logger.Info("ManualControlService external state cleared");
+        }
+        catch (Exception ex)
+        {
+            Logger.Error($"Error clearing ManualControlService external state: {ex.Message}");
+        }
+    }
+
+    /// <summary>
+    /// Entry: Stop State
+    /// Robot bị dừng (Emergency, Bumper, etc.)
+    /// </summary>
+    public void EntryStop()
+    {
+        Logger.Warning("==> Entry STOP State");
+        PlcController.SetSystemState(SystemState.PAUSED);
+        EmergencyStop();
+    }
+
+    /// <summary>
+    /// Exit: Stop State
+    /// Giải phóng stop
+    /// </summary>
+    public void ExitStop()
+    {
+        Logger.Info("<== Exit Stop State");
+
+        // Giống Lock (SERVICE) → Auto/Manual: M815 pulse + fault reset + EnableAsync (RobotController.ApplyResetFromPlc).
+        ApplyAlarmResetAndEnableMotorsLikeLock();
+    }
+
+    /// <summary>
+    /// Entry: Fault State
+    /// Robot gặp lỗi nghiêm trọng
+    /// </summary>
+    public void EntryFault()
+    {
+        Logger.Error("==> Entry FAULT State");
+        PlcController.SetSystemState(SystemState.ERROR);
+        EmergencyStop();
+    }
+
+    /// <summary>
+    /// Exit: Fault State
+    /// Đã khắc phục lỗi
+    /// </summary>
+    public void ExitFault()
+    {
+        Logger.Info("<== Exit Fault State");
+
+        // 1. Clear remaining fatal errors
+        try
+        {
+            using var scope = ServiceScopeFactory.CreateScope();
+            var errorManager = scope.ServiceProvider.GetRequiredService<IError>();
+            errorManager.ClearFatalErrors();
+        }
+        catch (Exception ex) { Logger.Error($"ExitFault: Error clearing errors: {ex.Message}"); }
+
+        // 2. PLC alarm reset + servo: cùng chuỗi như thoát Lock / ApplyResetFromPlc
+        ApplyAlarmResetAndEnableMotorsLikeLock();
+    }
+
+    /// <summary>
+    /// Pulse M815 (alarm reset trên PLC), sau đó FaultReset + chờ + EnableAsync — khớp RobotController.ApplyResetFromPlc (không gọi TryClearFault).
+    /// </summary>
+    private void ApplyAlarmResetAndEnableMotorsLikeLock()
+    {
+        Logger.Info("ApplyAlarmResetAndEnableMotorsLikeLock: M815 + fault reset + enable (như Lock → Auto/Manual)");
+        try { PlcController.WriteAlarmResetM815(); }
+        catch (Exception ex) { Logger.Warning($"WriteAlarmResetM815: {ex.Message}"); }
+
+        try
+        {
+            using var scope = ServiceScopeFactory.CreateScope();
+            var ik = scope.ServiceProvider.GetService<IInverseKinematics>();
+            if (ik == null)
+            {
+                Logger.Warning("ApplyAlarmResetAndEnableMotorsLikeLock: IInverseKinematics không có");
+                return;
+            }
+
+            ik.FaultReset();
+            Thread.Sleep(1500);
+            ik.FaultReset();
+            Thread.Sleep(800);
+            ik.EnableAsync(CancellationToken.None).GetAwaiter().GetResult();
+            if (ik.IsOperationEnabled)
+                Logger.Info("ApplyAlarmResetAndEnableMotorsLikeLock: động cơ OperationEnabled");
+            else
+                Logger.Warning("ApplyAlarmResetAndEnableMotorsLikeLock: chưa OperationEnabled sau EnableAsync");
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"ApplyAlarmResetAndEnableMotorsLikeLock: FaultReset/Enable — {ex.Message}");
+        }
+    }
+
+    // ===========================
+    // HELPER METHODS
+    // ===========================
+
+    /// <summary>
+    /// Dừng robot bình thường - gửi zero velocity đến IInverseKinematics
+    /// </summary>
+    private void StopRobot()
+    {
+        try
+        {
+            using var scope = ServiceScopeFactory.CreateScope();
+            var inverseKinematics = scope.ServiceProvider.GetService<IInverseKinematics>();
+
+            if (inverseKinematics == null)
+            {
+                Logger.Warning("IInverseKinematics not available, cannot stop robot");
+                return;
+            }
+
+            // Send zero velocity to stop the robot
+            // SetVelocity not available - commented out
+            // var zeroTwist = new Twist();
+            // inverseKinematics.SetVelocity(zeroTwist);
+
+            Logger.Info("Robot stopped (zero velocity intended)");
+        }
+        catch (AggregateException ex)
+        {
+            Logger.Error($"Stop robot error: {ex.InnerException?.Message ?? ex.Message}");
+        }
+        catch (Exception ex)
+        {
+            Logger.Error($"Stop robot error: {ex.Message}");
+        }
+    }
+
+    /// <summary>
+    /// Dừng khẩn cấp robot - gửi zero velocity và disable IInverseKinematics
+    /// </summary>
+    private void EmergencyStop()
+    {
+        try
+        {
+            using var scope = ServiceScopeFactory.CreateScope();
+            var inverseKinematics = scope.ServiceProvider.GetService<IInverseKinematics>();
+
+            if (inverseKinematics == null)
+            {
+                Logger.Warning("IInverseKinematics not available, cannot emergency stop robot");
+                return;
+            }
+
+            // Send zero velocity first
+            // SetVelocity not available - commented out
+            // var zeroTwist = new Twist();
+            // inverseKinematics.SetVelocity(zeroTwist);
+
+            // Disable the drive (if supported)
+            try
+            {
+                inverseKinematics.Disable();
+            }
+            catch (Exception disableEx)
+            {
+                Logger.Warning($"Could not disable IInverseKinematics: {disableEx.Message}");
+            }
+
+            Logger.Warning("EMERGENCY STOP! Robot stopped and disabled");
+        }
+        catch (AggregateException ex)
+        {
+            Logger.Error($"Emergency stop error: {ex.InnerException?.Message ?? ex.Message}");
+        }
+        catch (Exception ex)
+        {
+            Logger.Error($"Emergency stop error: {ex.Message}");
+        }
+    }
+}