Initial commit

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