update

RobotApp.Common.Shares/Enums/StateType.cs

@@ -43,6 +43,7 @@ public enum ServiceStateType
 public enum StopStateType
 {
     EMC,
+    Bumber,
     Protective,
     Manual,
 }

RobotApp.VDA5050/State/Error.cs

@@ -20,6 +20,7 @@ public class ErrorReferences
 public enum ErrorType
 {
     INITIALIZE_ORDER,
+    READ_PERIPHERAL_FAILURE,
 }
 
 public class Error

RobotApp/Interfaces/IDriver.cs

@@ -11,14 +11,14 @@ public interface IDriver
     bool IsReady { get; }
 
     /// <summary>
-    /// Tốc độ động cơ bên trái
+    /// Tốc độ di chuyển thẳng của robot (m/s)
     /// </summary>
-    double LeftVelocity { get; }
+    double LinearVelocity { get; }
 
     /// <summary>
-    /// Tốc độ động cơ bên phải
+    /// Tốc độ quay của robot (rad/s)
     /// </summary>
-    double RightVelocity { get; }
+    double AngularVelocity { get; }
 
     /// <summary>
     /// Điều khiển tốc độ động cơ trái và phải

RobotApp/Interfaces/IError.cs

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

RobotApp/Interfaces/IPeripheral.cs

@@ -1,35 +1,75 @@
-namespace RobotApp.Interfaces;
+using RobotApp.Common.Shares.Enums;
+
+namespace RobotApp.Interfaces;
 
 public enum PeripheralMode
 {
-    AUTOMATIC,
+    AUTO,
     MANUAL,
     SERVICE,
 }
+
+public enum PeripheralButton
+{
+    Start,
+    Reset,
+    Stop,
+}
+
+public enum SystemState
+{
+    INIT,
+    NOPOSE,
+    PAUSED,
+    IDLE,
+    PROCCESSING,
+    CHARGING,
+    OVERRIDE,
+    ERROR,
+    NONE,
+}
+
+public enum ProccessingState
+{
+    Move,
+    Lifting,
+    LiftRotating,
+    None,
+}
+
 public interface IPeripheral
 {
+    event Action<PeripheralMode>? OnPeripheralModeChanged;
+    event Action<PeripheralButton>? OnButtonPressed;
+    event Action<StopStateType>? OnStop;
+
     bool IsReady { get; }
     PeripheralMode PeripheralMode { get; }
+
     bool Emergency { get; }
     bool Bumper { get; }
+    bool LidarFrontProtectField { get; }
+    bool LidarBackProtectField { get; }
+    bool LidarFrontTimProtectField { get; }
+
     bool LiftedUp { get; }
     bool LiftedDown { get; }
     bool LiftHome { get; }
+
     bool LeftMotorReady { get; }
     bool RightMotorReady { get; }
     bool LiftMotorReady { get; }
-    bool SwitchLock { get; }
+
-    bool SwitchManual { get; }
-    bool SwitchAuto { get; }
     bool ButtonStart { get; }
     bool ButtonStop { get; }
     bool ButtonReset { get; }
+
     bool HasLoad { get; }
     bool MutedBase { get; }
     bool MutedLoad { get; }
-    bool StartedCharging { get; }
+    bool EnabledCharging { get; }
 
-    bool SetSytemState();
+    void SetSytemState(SystemState state);
-    bool SetProccessState();
+    void SetProccessState(ProccessingState state);
-    bool SetOnCharging();
+    void SetOnCharging(bool value);
 }

RobotApp/Interfaces/ISafety.cs

@@ -1,18 +1,21 @@
 namespace RobotApp.Interfaces;
 
+public enum SafetySpeed
+{
+    Very_Slow,
+    Slow,
+    Normal,
+    Medium,
+    Optimal,
+    Fast,
+    Very_Fast
+}
+
 public interface ISafety
 {
-    bool SpeedVerySlow { get; }
+    event Action<SafetySpeed>? OnSafetySpeedChanged;
-    bool SpeedSlow { get; }
+    SafetySpeed SafetySpeed { get; }
-    bool SpeedNormal { get; }
+    void SetMutedLoad(bool muted);
-    bool SpeedMedium { get; }
+    void SetMutedBase(bool muted);
-    bool SpeedOptimal { get; }
+    void SetHorizontalLoad(bool value);
-    bool SpeedFast { get; }
-    bool SpeedVeryFast { get; }
-    bool LidarFrontProtectField { get; }
-    bool LidarBackProtectField { get; }
-    bool LidarFrontTimProtectField { get; }
-    bool SetMutedLoad(bool muted);
-    bool SetMutedBase(bool muted);
-    bool SetHorizontalLoad();
 }

RobotApp/Interfaces/ISensorIMU.cs

@@ -1,17 +0,0 @@
-namespace RobotApp.Interfaces;
-
-/// <summary>
-/// Interface cảm biến IMU
-/// </summary>
-public interface ISensorIMU
-{
-    /// <summary>
-    /// Trạng thái sẵn sàng của cảm biến IMU
-    /// </summary>
-    bool IsReady { get; }
-
-    /// <summary>
-    /// Góc xoay của robot (đơn vị độ)
-    /// </summary>
-    double Angle { get; }
-}

RobotApp/Program.cs

@@ -47,7 +47,6 @@ builder.Services.AddIdentity<ApplicationUser, ApplicationRole>(options =>
 builder.Services.AddSingleton<IEmailSender<ApplicationUser>, IdentityNoOpEmailSender>();
 
 builder.Services.AddSingleton(typeof(RobotApp.Services.Logger<>));
-builder.Services.AddSingleton<RobotConnection>();
 
 builder.Services.AddRobotSimulation();
 builder.Services.AddRobot();

RobotApp/RobotApp.csproj

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

RobotApp/Services/ModbusTcpClient.cs

@@ -7,7 +7,7 @@ namespace RobotApp.Services;
 
 public class ModbusTcpClient(string IpAddress, int Port, byte ClientId) : IDisposable
 {
-    public bool IsConnected => !disposed && tcpClient != null && tcpClient.Client.Connected && stream != null;
+    public bool IsConnected => !disposed && tcpClient != null && tcpClient.Client.Connected && tcpClient.Connected && stream != null;
 
     private TcpClient? tcpClient;
     private NetworkStream? stream;

RobotApp/Services/Robot/RobotConfiguration.cs

@@ -1,22 +1,25 @@
 using RobotApp.Common.Shares.Enums;
+using RobotApp.Services.Robot.Simulation;
 
 namespace RobotApp.Services.Robot;
 
 public class RobotConfiguration
 {
-    public static string SerialNumber { get; set; } = "Robot-Demo";
+    public string SerialNumber { get; set; } = "Robot-Demo";
-    public static NavigationType NavigationType { get; set; } = NavigationType.Differential;
+    public NavigationType NavigationType { get; set; } = NavigationType.Differential;
-    public static VDA5050.VDA5050Setting VDA5050Setting { get; set; } = new()
+    public VDA5050.VDA5050Setting VDA5050Setting { get; set; } = new()
     {
-        HostServer = "172.20.235.170",
+        HostServer = "172.20.235.176",
-        Port = 1885,
+        Port = 1883,
         UserName = "robotics",
         Password = "robotics",
         Manufacturer = "PhenikaaX",
         Version = "0.0.1",
         PublishRepeat = 2,
     };
-    public static string PLCAddress { get; set; } = "127.0.0.1";
+    public string PLCAddress { get; set; } = "127.0.0.1";
-    public static int PLCPort { get; set; } = 502;
+    public int PLCPort { get; set; } = 502;
-    public static int PLCUnitId { get; set; } = 1;
+    public byte PLCUnitId { get; set; } = 1;
+    public bool IsSimulation { get; set; } = true;
+    public SimulationModel SimulationModel { get; set; } = new();
 }

RobotApp/Services/Robot/RobotConnection.cs

@@ -6,13 +6,13 @@ using System.Text.Json;
 
 namespace RobotApp.Services.Robot;
 
-public class RobotConnection(Logger<RobotConnection> Logger, Logger<MQTTClient> MQTTClientLogger)
+public class RobotConnection(RobotConfiguration RobotConfiguration, Logger<RobotConnection> Logger, Logger<MQTTClient> MQTTClientLogger)
 {
     private readonly VDA5050Setting VDA5050Setting = RobotConfiguration.VDA5050Setting;
     private MQTTClient? MqttClient;
 
     public bool IsConnected => MqttClient is not null && MqttClient.IsConnected;
-    public readonly static string SerialNumber = RobotConfiguration.SerialNumber;
+    public readonly string SerialNumber = RobotConfiguration.SerialNumber;
 
     private void OrderChanged(string data)
     {

RobotApp/Services/Robot/RobotController.cs

@@ -6,7 +6,7 @@ using RobotApp.VDA5050.Order;
 
 namespace RobotApp.Services.Robot;
 
-public class RobotController(IOrder OrderManager,
+public partial class RobotController(IOrder OrderManager,
                              INavigation NavigationManager,
                              IInstantActions ActionManager,
                              IBattery BatteryManager,
@@ -25,25 +25,18 @@ public class RobotController(IOrder OrderManager,
     private WatchTimer<RobotController>? UpdateStateTimer;
     private const int UpdateStateInterval = 1000;
 
-    protected override Task ExecuteAsync(CancellationToken stoppingToken)
+    protected override async Task ExecuteAsync(CancellationToken stoppingToken)
     {
-        InitializationingHandler();
+        await InitializationingHandler(stoppingToken);
 
         UpdateStateTimer = new(UpdateStateInterval, UpdateStateHandler, Logger);
         UpdateStateTimer.Start();
-        return Task.CompletedTask;
     }
 
-    private void InitializationingHandler()
+    public override async Task StopAsync(CancellationToken cancellationToken)
     {
-        try
+        StopHandler();
-        {
+        await base.StopAsync(cancellationToken);
-            StateManager.InitializeHierarchyStates();
-        }
-        catch
-        {
-
-        }
     }
 
     private void UpdateStateHandler()
@@ -57,12 +50,12 @@ public class RobotController(IOrder OrderManager,
         {
             try
             {
+                if (PeripheralManager.PeripheralMode != PeripheralMode.AUTO) throw new OrderException(RobotErrors.Error1006(PeripheralManager.PeripheralMode));
                 if (!string.IsNullOrEmpty(OrderManager.OrderId))
                 {
                     if (order.OrderId != OrderManager.OrderId) throw new OrderException(RobotErrors.Error1001(OrderManager.OrderId, order.OrderId));
                     OrderManager.UpdateOrder(order.OrderUpdateId, order.Nodes, order.Edges);
                 }
-                else if (PeripheralManager.PeripheralMode != PeripheralMode.AUTOMATIC) throw new OrderException(RobotErrors.Error1006(PeripheralManager.PeripheralMode));
                 else if (ActionManager.HasActionRunning) throw new OrderException(RobotErrors.Error1007());
                 else if (ErrorManager.HasFatalError) throw new OrderException(RobotErrors.Error1008());
                 else if (NavigationManager.Driving) throw new OrderException(RobotErrors.Error1009());

RobotApp/Services/Robot/RobotControllerInitialize.cs

@@ -0,0 +1,110 @@
+using RobotApp.Common.Shares.Enums;
+using RobotApp.Interfaces;
+
+namespace RobotApp.Services.Robot;
+
+public partial class RobotController
+{
+    private readonly AutoResetEvent StartButtonPressedEvent = new(false);
+    private readonly AutoResetEvent StopButtonPressedEvent = new(false);
+    private readonly AutoResetEvent ResetButtonPressedEvent = new(false);
+    private async Task InitializationingHandler(CancellationToken cancellationToken)
+    {
+        try
+        {
+            StateManager.InitializeHierarchyStates();
+            PeripheralManager.SetSytemState(SystemState.INIT);
+            PeripheralManager.OnPeripheralModeChanged += SwichModeChanged;
+            PeripheralManager.OnButtonPressed += OnButtonPressed;
+            PeripheralManager.OnStop += OnStop;
+
+            while (!cancellationToken.IsCancellationRequested)
+            {
+                if (StateManager.CurrentStateName == Enum.GetName(SystemStateType.Initializing))
+                {
+                    if (PeripheralManager.IsReady &&
+                        //PeripheralManager.LiftMotorReady &&
+                        //PeripheralManager.LeftMotorReady &&
+                        //PeripheralManager.RightMotorReady &&
+                        BatteryManager.IsReady && 
+                        Localization.IsReady && 
+                        NavigationManager.IsReady) break;
+                }
+                await Task.Delay(1000, cancellationToken);
+            }
+            Logger.Info("Robot đã khởi tạo xong. Đang kết nối tới Fleet Manager.");
+
+            await ConnectionManager.StartConnection(cancellationToken);
+            Logger.Info("Robot đã kết nối tới Fleet Manager.");
+            StateManager.TransitionTo(SystemStateType.Standby);
+
+            StartButtonPressedEvent.Reset();
+            Logger.Info("Chờ nút Start được nhấn để vào chế độ hoạt động...");
+            if (StartButtonPressedEvent.WaitOne())
+            {
+                StateManager.TransitionTo(RootStateType.Auto);
+                PeripheralManager.SetSytemState(SystemState.IDLE);
+            }
+        }
+        catch (Exception ex)
+        {
+            Logger.Error($"Khởi tạo RobotController xảy ra lỗi: {ex.Message} - {ex.StackTrace}");
+        }
+    }
+
+    private void StopHandler()
+    {
+        UpdateStateTimer?.Stop();
+        PeripheralManager.OnPeripheralModeChanged -= SwichModeChanged;
+        PeripheralManager.OnButtonPressed -= OnButtonPressed;
+        PeripheralManager.OnStop -= OnStop;
+    }
+
+    private void SwichModeChanged(PeripheralMode mode)
+    {
+        switch (mode)
+        {
+            case PeripheralMode.AUTO:
+                if (StartButtonPressedEvent.WaitOne())
+                {
+                    StateManager.TransitionTo(RootStateType.Auto);
+                    PeripheralManager.SetSytemState(SystemState.IDLE);
+                }
+                break;
+            case PeripheralMode.MANUAL:
+                StateManager.TransitionTo(RootStateType.Manual);
+                PeripheralManager.SetSytemState(SystemState.NONE);
+                break;
+            case PeripheralMode.SERVICE:
+                StateManager.TransitionTo(RootStateType.Service);
+                PeripheralManager.SetSytemState(SystemState.NONE);
+                break;
+            default:
+                break;
+        }
+    }
+
+    private void OnButtonPressed(PeripheralButton buttonPressed)
+    {
+        switch(buttonPressed)
+        {
+            case PeripheralButton.Start:
+                StartButtonPressedEvent.Set();
+                break;
+            case PeripheralButton.Reset:
+                ResetButtonPressedEvent.Set();
+                break;
+            case PeripheralButton.Stop:
+                StopButtonPressedEvent.Set();
+                break;
+            default:
+                break;
+        }
+    }
+
+    private void OnStop(StopStateType state)
+    {
+        StateManager.TransitionToStop(state);
+        PeripheralManager.SetSytemState(SystemState.PAUSED);
+    }
+}

RobotApp/Services/Robot/RobotDriver.cs

@@ -0,0 +1,22 @@
+using RobotApp.Interfaces;
+
+namespace RobotApp.Services.Robot;
+
+public class RobotDriver : IDriver
+{
+    public bool IsReady => throw new NotImplementedException();
+
+    public double LinearVelocity => throw new NotImplementedException();
+
+    public double AngularVelocity => throw new NotImplementedException();
+
+    public bool ControlVelocity(double left, double right)
+    {
+        throw new NotImplementedException();
+    }
+
+    public bool Stop(bool isFree)
+    {
+        throw new NotImplementedException();
+    }
+}

RobotApp/Services/Robot/RobotErrors.cs

@@ -1,13 +1,14 @@
-using Microsoft.AspNetCore.Components;
+using RobotApp.Interfaces;
-using RobotApp.Interfaces;
 using RobotApp.VDA5050.State;
 
 namespace RobotApp.Services.Robot;
 
-public class RobotErrors
+public class RobotErrors : IError
 {
     private readonly List<Error> Errors = [];
 
+    public bool HasFatalError => Errors.Any(e => e.ErrorLevel == ErrorLevel.FATAL.ToString());
+
     public void AddError(Error error, TimeSpan? clearAfter = null)
     {
         lock (Errors)
@@ -22,12 +23,36 @@ public class RobotErrors
                 await Task.Delay(clearAfter.Value);
                 lock (Errors)
                 {
-                    Errors.RemoveAll(e => e.ErrorType == error.ErrorType);
+                    Errors.RemoveAll(e => e.ErrorHint == error.ErrorHint);
                 }
             });
         }
     }
 
+    public void DeleteErrorType(string errorType)
+    {
+        lock (Errors)
+        {
+            Errors.RemoveAll(e => e.ErrorType == errorType);
+        }
+    }
+
+    public void DeleteErrorHint(string errorHint)
+    {
+        lock (Errors)
+        {
+            Errors.RemoveAll(e => e.ErrorHint == errorHint);
+        }
+    }
+
+    public void ClearAllErrors()
+    {
+        lock (Errors)
+        {
+            Errors.Clear();
+        }
+    }
+
     public static Error CreateError(ErrorType type, string hint, ErrorLevel level, string description)
     {
         return new Error()
@@ -70,4 +95,11 @@ public class RobotErrors
         => CreateError(ErrorType.INITIALIZE_ORDER, "1014", ErrorLevel.WARNING, $"Edge {edgeId} chứa StartNodeId {nodeId} không tồn tại trong Nodes");
     public static Error Error1015(string edgeId, string nodeId)
         => CreateError(ErrorType.INITIALIZE_ORDER, "1015", ErrorLevel.WARNING, $"Edge {edgeId} chứa {nodeId} không tồn tại trong Nodes");
+
+    public static Error Error2001()
+        => CreateError(ErrorType.READ_PERIPHERAL_FAILURE, "2001", 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 Error Error2002()
+        => CreateError(ErrorType.READ_PERIPHERAL_FAILURE, "2002", 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 Error Error2003()
+        => CreateError(ErrorType.READ_PERIPHERAL_FAILURE, "2003", ErrorLevel.FATAL, "Mất kết nối với hệ thống ngoại vi(PLC)");
 }

RobotApp/Services/Robot/RobotInfomations.cs

@@ -1,6 +1,8 @@
-namespace RobotApp.Services.Robot
+using RobotApp.Interfaces;
+
+namespace RobotApp.Services.Robot
 {
-    public class RobotInfomations
+    public class RobotInfomations : IInfomation
     {
     }
 }

RobotApp/Services/Robot/RobotLocalization.cs

@@ -17,7 +17,7 @@ public class Xloc
     public bool IsReady { get; set; }
 }
 
-public class RobotLocalization(IConfiguration Configuration, SimulationVisualization SimVisualization) : ILocalization
+public class RobotLocalization(RobotConfiguration RobotConfiguration, SimulationVisualization SimVisualization) : ILocalization
 {
     public double X => IsSimulation ? SimVisualization.X : Xloc.X;
 
@@ -39,7 +39,7 @@ public class RobotLocalization(IConfiguration Configuration, SimulationVisualiza
 
 
     private readonly Xloc Xloc = new();
-    private readonly bool IsSimulation = Configuration.GetValue<bool>("Robot:Simulation", false);
+    private readonly bool IsSimulation = RobotConfiguration.IsSimulation;
 
     public MessageResult ActivateMap(string mapName)
     {

RobotApp/Services/Robot/RobotNavigation.cs

@@ -1,11 +1,10 @@
 using RobotApp.Interfaces;
-using RobotApp.Services.Robot.Simulation;
 using RobotApp.Services.Robot.Simulation.Navigation;
 using RobotApp.VDA5050.Order;
 
 namespace RobotApp.Services.Robot;
 
-public class RobotNavigation(SimulationModel SimModel) : INavigation
+public class RobotNavigation : INavigation
 {
     public bool IsReady => IsSimulation;
     public bool Driving => IsSimulation ? SimNavigation is not null ? SimNavigation.Driving : false : false;
@@ -14,10 +13,17 @@ public class RobotNavigation(SimulationModel SimModel) : INavigation
     public double Omega => IsSimulation ? SimNavigation is not null ? SimNavigation.Omega : 0 : 0;
     public NavigationState State => IsSimulation ? SimNavigation is not null ? SimNavigation.State : NavigationState.None : NavigationState.None;
 
+    private readonly SimulationNavigation? SimNavigation;
+    private readonly bool IsSimulation;
 
-    private SimulationNavigation? SimNavigation;
+    public RobotNavigation(RobotConfiguration RobotConfiguration, IServiceProvider ServiceProvider)
-    private bool IsSimulation => SimModel.Enable;
+    {
-
+        IsSimulation = RobotConfiguration.IsSimulation;
+        if (IsSimulation)
+        {
+            SimNavigation = SimulationNavigationManager.GetNavigation(RobotConfiguration.SimulationModel.NavigationType, ServiceProvider);
+        }
+    }
 
     public void CancelMovement()
     {

RobotApp/Services/Robot/RobotPeripheral.cs

@@ -1,77 +1,268 @@
-using RobotApp.Interfaces;
+using RobotApp.Common.Shares.Enums;
+using RobotApp.Interfaces;
 
 namespace RobotApp.Services.Robot;
 
-public class RobotPeripheral : BackgroundService, IPeripheral, ISafety
+public partial class RobotPeripheral(RobotConfiguration RobotConfiguration, IError ErrorManager, Logger<RobotPeripheral> Logger) : BackgroundService, IPeripheral, ISafety
 {
-    public PeripheralMode PeripheralMode { get; private set; } = PeripheralMode.SERVICE;
+    public bool IsReady { get; private set; } = false;
-    public bool IsReady { get; private set; } = true;
-    public bool Emergency => throw new NotImplementedException();
-    public bool Bumper => throw new NotImplementedException();
-    public bool LiftedUp => throw new NotImplementedException();
-    public bool LiftedDown => throw new NotImplementedException();
-    public bool LiftHome => throw new NotImplementedException();
-    public bool LidarFrontProtectField => throw new NotImplementedException();
-    public bool LidarBackProtectField => throw new NotImplementedException();
-    public bool LidarFrontTimProtectField => throw new NotImplementedException();
-    public bool LeftMotorReady => throw new NotImplementedException();
-    public bool RightMotorReady => throw new NotImplementedException();
-    public bool LiftMotorReady => throw new NotImplementedException();
-    public bool SwitchLock => throw new NotImplementedException();
-    public bool SwitchManual => throw new NotImplementedException();
-    public bool SwitchAuto => throw new NotImplementedException();
-    public bool ButtonStart => throw new NotImplementedException();
-    public bool ButtonStop => throw new NotImplementedException();
-    public bool ButtonReset => throw new NotImplementedException();
-    public bool HasLoad => throw new NotImplementedException();
-    public bool MutedBase => throw new NotImplementedException();
-    public bool MutedLoad => throw new NotImplementedException();
-    public bool StartedCharging => throw new NotImplementedException();
-    public bool SpeedVerySlow => throw new NotImplementedException();
-    public bool SpeedSlow => throw new NotImplementedException();
-    public bool SpeedNormal => throw new NotImplementedException();
-    public bool SpeedMedium => throw new NotImplementedException();
-    public bool SpeedOptimal => throw new NotImplementedException();
-    public bool SpeedFast => throw new NotImplementedException();
-    public bool SpeedVeryFast => throw new NotImplementedException();
 
-    public bool SetHorizontalLoad()
+    public event Action<SafetySpeed>? OnSafetySpeedChanged;
+    public event Action<PeripheralMode>? OnPeripheralModeChanged;
+    public event Action<PeripheralButton>? OnButtonPressed;
+    public event Action<StopStateType>? OnStop;
+
+    private const int ReaderInterval = 100;
+    private WatchTimer<RobotPeripheral>? PeripheralReaderTimer;
+
+    public void SetHorizontalLoad(bool value)
     {
-        throw new NotImplementedException();
+        WritePLC(client => client.WriteSingleCoil(SetHorizontalLoadAddress, value));
     }
 
-    public bool SetMutedBase(bool muted)
+    public void SetMutedBase(bool muted)
     {
-        throw new NotImplementedException();
+        WritePLC(client => client.WriteSingleCoil(SetMutedBaseAddress, muted));
     }
 
-    public bool SetMutedLoad(bool muted)
+    public void SetMutedLoad(bool muted)
     {
-        throw new NotImplementedException();
+        WritePLC(client => client.WriteSingleCoil(SetMutedLoadAddress, muted));
     }
 
-    public bool SetOnCharging()
+    public void SetOnCharging(bool value)
     {
-        throw new NotImplementedException();
+        WritePLC(client => client.WriteSingleCoil(EnableChargingAddress, value));
     }
 
-    public bool SetProccessState()
+    public void SetProccessState(ProccessingState state)
     {
-        throw new NotImplementedException();
+        WritePLC(client =>
+        {
+            switch (state)
+            {
+                case ProccessingState.Move:
+                    client.WriteMultipleCoils(RobotExecuteStartAddress, RobotExecuteMoveState);
+                    break;
+                case ProccessingState.Lifting:
+                    client.WriteMultipleCoils(RobotExecuteStartAddress, RobotExecuteLiftingState);
+                    break;
+                case ProccessingState.LiftRotating:
+                    client.WriteMultipleCoils(RobotExecuteStartAddress, RobotExecuteLiftRotatingState);
+                    break;
+                case ProccessingState.None:
+                    client.WriteMultipleCoils(RobotExecuteStartAddress, RobotExecuteClearState);
+                    break;
+                default:
+                    break;
+            }
+        });
     }
 
-    public bool SetSytemState()
+    public void SetSytemState(SystemState state)
     {
-        throw new NotImplementedException();
+        WritePLC(client =>
+        {
+            if (state != SystemState.ERROR) client.WriteSingleCoil(RobotStateErrorAddress, false);
+            else client.WriteMultipleCoils(RobotStateStartAddress, RobotClearState);
+            switch (state)
+            {
+                case SystemState.INIT:
+                    client.WriteMultipleCoils(RobotStateStartAddress, RobotInitState);
+                    break;
+                case SystemState.NOPOSE:
+                    client.WriteMultipleCoils(RobotStateStartAddress, RobotNoPoseState);
+                    break;
+                case SystemState.PAUSED:
+                    client.WriteMultipleCoils(RobotStateStartAddress, RobotPauseState);
+                    break;
+                case SystemState.IDLE:
+                    client.WriteMultipleCoils(RobotStateStartAddress, RobotIdleState);
+                    break;
+                case SystemState.PROCCESSING:
+                    client.WriteMultipleCoils(RobotStateStartAddress, RobotProccessingState);
+                    break;
+                case SystemState.CHARGING:
+                    client.WriteMultipleCoils(RobotStateStartAddress, RobotCharingState);
+                    break;
+                case SystemState.ERROR:
+                    client.WriteSingleCoil(RobotStateErrorAddress, true);
+                    break;
+                default:
+                    break;
+            }
+        });
     }
 
-    protected override Task ExecuteAsync(CancellationToken stoppingToken)
+    private void WritePLC(Action<ModbusTcpClient> writeAction)
     {
-        throw new NotImplementedException();
+        if (IsReady && !RobotConfiguration.IsSimulation)
+        {
+            try
+            {
+                if (ModbusTcpClient.TryConnect(RobotConfiguration.PLCAddress, RobotConfiguration.PLCPort, RobotConfiguration.PLCUnitId, out ModbusTcpClient? client) && client is not null)
+                {
+                    writeAction(client);
+                    client.Dispose();
+                    ErrorManager.DeleteErrorHint("2002");
+                }
+            }
+            catch (Exception ex)
+            {
+                ErrorManager.AddError(RobotErrors.Error2002(), TimeSpan.FromSeconds(10));
+                Logger.Error($"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): {ex.Message}");
+            }
+        }
     }
 
-    public override Task StopAsync(CancellationToken cancellationToken)
+    private void ReadButtons(ModbusTcpClient client)
     {
-        return base.StopAsync(cancellationToken);
+        bool[] buttons = client.ReadCoils(ButtonStartAddress, 3);
+        if (buttons.Length == 3)
+        {
+            ButtonStart = buttons[0];
+            ButtonStop = buttons[1];
+            ButtonReset = buttons[2];
+            if (ButtonStart) OnButtonPressed?.Invoke(PeripheralButton.Start);
+            if (ButtonStop) OnButtonPressed?.Invoke(PeripheralButton.Stop);
+            if (ButtonReset) OnButtonPressed?.Invoke(PeripheralButton.Reset);
+        }
+    }
+
+    private void ReadSwitch(ModbusTcpClient client)
+    {
+        bool[] switchs = client.ReadCoils(SwicthStartAddress, 3);
+        if (switchs.Length == 3)
+        {
+            var oldMode = PeripheralMode;
+            if (switchs[0] && PeripheralMode != PeripheralMode.SERVICE)
+            {
+                PeripheralMode = PeripheralMode.SERVICE;
+            }
+            else if (switchs[1] && PeripheralMode != PeripheralMode.AUTO)
+            {
+                PeripheralMode = PeripheralMode.AUTO;
+            }
+            else if (switchs[2] && PeripheralMode != PeripheralMode.MANUAL)
+            {
+                PeripheralMode = PeripheralMode.MANUAL;
+            }
+            if (oldMode != PeripheralMode) OnPeripheralModeChanged?.Invoke(PeripheralMode);
+        }
+    }
+
+    private void ReadSafetySpeed(ModbusTcpClient client)
+    {
+        bool[] speed = client.ReadCoils(SpeedStartAddress, 7);
+        if (speed.Length == 7)
+        {
+            var speedMap = new[]
+            {
+                (bit: speed[0], speed: SafetySpeed.Very_Slow),
+                (bit: speed[1], speed: SafetySpeed.Slow),
+                (bit: speed[2], speed: SafetySpeed.Normal),
+                (bit: speed[3], speed: SafetySpeed.Medium),
+                (bit: speed[4], speed: SafetySpeed.Optimal),
+                (bit: speed[5], speed: SafetySpeed.Fast),
+                (bit: speed[6], speed: SafetySpeed.Very_Fast)
+            };
+            var activeSpeed = speedMap.FirstOrDefault(s => s.bit);
+            if (activeSpeed.speed != SafetySpeed)
+            {
+                SafetySpeed = activeSpeed.speed;
+                OnSafetySpeedChanged?.Invoke(SafetySpeed);
+            }
+        }
+    }
+
+    private void ReadMotorReady(ModbusTcpClient client)
+    {
+        bool[] motors = client.ReadCoils(MotorReadyStartAddress, 3);
+        if (motors.Length == 3)
+        {
+            LeftMotorReady = motors[0];
+            RightMotorReady = motors[1];
+            LiftMotorReady = motors[2];
+        }
+    }
+
+    private void ReadSensors(ModbusTcpClient client)
+    {
+        bool[] sensors = client.ReadCoils(SafetyStartAddress, 5);
+        if (sensors.Length == 5)
+        {
+            Emergency = sensors[0];
+            Bumper = sensors[1];
+            LiftedUp = sensors[2];
+            LiftedDown = sensors[3];
+            LiftHome = sensors[4];
+        }
+    }
+
+    private void PeripheralReaderTimer_Elapsed()
+    {
+        if (IsReady && !RobotConfiguration.IsSimulation)
+        {
+            try
+            {
+                if (ModbusTcpClient.TryConnect(RobotConfiguration.PLCAddress, RobotConfiguration.PLCPort, RobotConfiguration.PLCUnitId, out ModbusTcpClient? client) && client is not null)
+                {
+                    MutedBase = client.ReadCoils(MutedBaseAddress, 1)[0];
+                    MutedLoad = client.ReadCoils(MutedLoadAddress, 1)[0];
+                    EnabledCharging = client.ReadCoils(EnabledChargingAddress, 1)[0];
+                    HasLoad = client.ReadCoils(HasLoadAddress, 1)[0];
+                    LidarBackProtectField = client.ReadCoils(LidarBackProtectFieldAddress, 1)[0];
+                    LidarFrontProtectField = client.ReadCoils(LidarFrontProtectFieldAddress, 1)[0];
+                    LidarFrontTimProtectField = client.ReadCoils(LidarFrontTimProtectFieldAddress, 1)[0];
+                    ReadSensors(client);
+                    ReadSwitch(client);
+                    ReadSafetySpeed(client);
+                    ReadMotorReady(client);
+                    ReadButtons(client);
+                    client.Dispose();
+                }
+                ErrorManager.DeleteErrorHint("2001");
+
+            }
+            catch (Exception ex)
+            {
+                ErrorManager.AddError(RobotErrors.Error2001());
+                Logger.Error($"Có lỗi xảy ra trong quá trình đọc tín hiệu từ hệ thống ngoại vi(PLC): {ex.Message}");
+            }
+        }
+    }
+
+    protected override async Task ExecuteAsync(CancellationToken stoppingToken)
+    {
+        await Task.Yield();
+        while (!stoppingToken.IsCancellationRequested && !RobotConfiguration.IsSimulation)
+        {
+            try
+            {
+                if (ModbusTcpClient.TryConnect(RobotConfiguration.PLCAddress, RobotConfiguration.PLCPort, RobotConfiguration.PLCUnitId, out ModbusTcpClient? client) && client is not null)
+                {
+                    client.Dispose();
+                    break;
+                }
+                await Task.Delay(2000, stoppingToken);
+            }
+            catch (TaskCanceledException) { break; }
+            catch (Exception ex)
+            {
+                Logger.Error($"Có lỗi xảy ra trong quá tình kết nối với hệ thống ngoại vi(PLC): {ex.Message}");
+                await Task.Delay(5000, stoppingToken);
+            }
+        }
+        IsReady = true;
+        PeripheralReaderTimer = new(ReaderInterval, PeripheralReaderTimer_Elapsed, Logger);
+        PeripheralReaderTimer.Start();
+    }
+
+    public override async Task StopAsync(CancellationToken cancellationToken)
+    {
+        await Task.Yield();
+        PeripheralReaderTimer?.Dispose();
+        IsReady = false;
+        _ = base.StopAsync(cancellationToken);
     }
 }

RobotApp/Services/Robot/RobotPeripheralAddress.cs

@@ -0,0 +1,68 @@
+namespace RobotApp.Services.Robot;
+
+public partial class RobotPeripheral
+{
+    public static readonly ushort SafetyStartAddress = 0x0835;
+    public static readonly ushort EmergencyAddress = 0x0835;
+    public static readonly ushort BumperAddress = 0x0836;
+    public static readonly ushort LiftedUpAddress = 0x0837;
+    public static readonly ushort LiftedDownAddress = 0x0838;
+    public static readonly ushort LiftHomeAddress = 0x0839;
+
+    public static readonly ushort LidarFrontProtectFieldAddress = 0x0830;
+    public static readonly ushort LidarBackProtectFieldAddress = 0x0831;
+    public static readonly ushort LidarFrontTimProtectFieldAddress = 0x083A;
+
+    public static readonly ushort MotorReadyStartAddress = 0x0823;
+    public static readonly ushort LeftMotorReadyAddress = 0x0823;
+    public static readonly ushort RightMotorReadyAddress = 0x0824;
+    public static readonly ushort LiftMotorReadyAddress = 0x0825;
+
+    public static readonly ushort SwicthStartAddress = 0x0814;
+    public static readonly ushort SwitchLockAddress = 0x0814;
+    public static readonly ushort SwitchAutoAddress = 0x0815;
+    public static readonly ushort SwitchManualAddress = 0x0816;
+
+    public static readonly ushort ButtonStartAddress = 0x0840;
+    public static readonly ushort StartButtonAddress = 0x0840;
+    public static readonly ushort StopButtonAddress = 0x0841;
+    public static readonly ushort ResetButtonAddress = 0x0842;
+
+    public static readonly ushort SetHorizontalLoadAddress = 0x09CC;
+    public static readonly ushort HasLoadAddress = 0x09D2;
+    public static readonly ushort EnabledChargingAddress = 0x0854;
+    public static readonly ushort EnableChargingAddress = 0x0853;
+
+    public static readonly ushort SetMutedBaseAddress = 0x09CE;
+    public static readonly ushort MutedBaseAddress = 0x09CF;
+
+    public static readonly ushort SetMutedLoadAddress = 0x09D0;
+    public static readonly ushort MutedLoadAddress = 0x09D1;
+
+    public static readonly ushort SpeedStartAddress = 0x09C2;
+    public static readonly ushort SpeedVerySlowAddress = 0x09C2;
+    public static readonly ushort SpeedSlowAddress = 0x09C3;
+    public static readonly ushort SpeedNormalAddress = 0x09C4;
+    public static readonly ushort SpeedMediumAddress = 0x09C5;
+    public static readonly ushort SpeedOptimalAddress = 0x09C6;
+    public static readonly ushort SpeedFastAddress = 0x09C7;
+    public static readonly ushort SpeedVeryFastAddress = 0x09C8;
+
+    public static readonly ushort RobotStateStartAddress = 0x092C;
+    public static readonly bool[] RobotClearState = [false, false, false, false, false, false, false];
+    public static readonly bool[] RobotInitState = [true, false, false, false, false, false, false];
+    public static readonly bool[] RobotNoPoseState = [false, true, false, false, false, false, false];
+    public static readonly bool[] RobotPauseState = [false, false, true, false, false, false, false];
+    public static readonly bool[] RobotIdleState = [false, false, false, true, false, false, false];
+    public static readonly bool[] RobotProccessingState = [false, false, false, false, true, false, false];
+    public static readonly bool[] RobotCharingState = [false, false, false, false, false, false, true];
+
+    public static readonly ushort RobotStateErrorAddress = 0x0936;
+
+    public static readonly ushort RobotExecuteStartAddress = 0x0933;
+    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];
+
+}

RobotApp/Services/Robot/RobotPeripheralData.cs

@@ -0,0 +1,32 @@
+using RobotApp.Interfaces;
+
+namespace RobotApp.Services.Robot;
+
+public partial class RobotPeripheral
+{
+    public PeripheralMode PeripheralMode { get; private set; } = PeripheralMode.SERVICE;
+    public bool Emergency { get; private set; }
+    public bool Bumper { get; private set; }
+    public bool LiftedUp { get; private set; }
+    public bool LiftedDown { get; private set; }
+    public bool LiftHome { get; private set; }
+
+    public bool LidarFrontProtectField { get; private set; }
+    public bool LidarBackProtectField { get; private set; }
+    public bool LidarFrontTimProtectField { get; private set; }
+
+    public bool LeftMotorReady { get; private set; }
+    public bool RightMotorReady { get; private set; }
+    public bool LiftMotorReady { get; private set; }
+
+    public bool ButtonStart { get; private set; }
+    public bool ButtonStop { get; private set; }
+    public bool ButtonReset { get; private set; }
+
+    public bool HasLoad { get; private set; }
+    public bool MutedBase { get; private set; }
+    public bool MutedLoad { get; private set; }
+    public bool EnabledCharging { get; private set; }
+
+    public SafetySpeed SafetySpeed { get; private set; }
+}

RobotApp/Services/Robot/RobotVisualization.cs

@@ -1,14 +1,17 @@
-using RobotApp.Interfaces;
+using RobotApp.Common.Shares;
+using RobotApp.Interfaces;
+using RobotApp.VDA5050;
 using RobotApp.VDA5050.Visualization;
 
 namespace RobotApp.Services.Robot;
 
-public class RobotVisualization(ILocalization Localization)
+public class RobotVisualization(ILocalization Localization, INavigation Navigation, RobotConfiguration RobotConfiguration, RobotConnection RobotConnection, Logger<RobotVisualization> Logger) : BackgroundService
 {
-    public VisualizationMsg Visualization => GetVisualizationMsg();
+    public string SerialNumber { get; set; } = RobotConfiguration.SerialNumber;
-    public string SerialNumber { get; set; } = string.Empty;
-
     private uint HeaderId;
+
+    private WatchTimerAsync<RobotVisualization>? UpdateTimer;
+    private const int UpdateInterval = 50;
     private VisualizationMsg GetVisualizationMsg()
     {
         return new VisualizationMsg()
@@ -25,10 +28,39 @@ public class RobotVisualization(ILocalization Localization)
             },
             Velocity = new Velocity()
             {
-                Vx = 0,
+                Vx = Navigation.VelocityX,
-                Vy = 0,
+                Vy = Navigation.VelocityY,
-                Omega = 0   
+                Omega = Navigation.Omega
             }
         };
     }
+
+    private async Task UpdateHandler()
+    {
+        try
+        {
+            await RobotConnection.Publish(VDA5050Topic.VISUALIZATION.ToTopicString(), System.Text.Json.JsonSerializer.Serialize(GetVisualizationMsg(), JsonOptionExtends.Write));
+        }
+        catch { }
+    }
+
+    protected override async Task ExecuteAsync(CancellationToken stoppingToken)
+    {
+        while(!stoppingToken.IsCancellationRequested)
+        {
+            if (RobotConnection.IsConnected) break;
+            await Task.Delay(1000);
+        }
+        if(!stoppingToken.IsCancellationRequested)
+        {
+            UpdateTimer = new(UpdateInterval, UpdateHandler, Logger);
+            UpdateTimer.Start();
+        }
+    }
+
+    public override Task StopAsync(CancellationToken cancellationToken)
+    {
+        UpdateTimer?.Dispose();
+        return base.StopAsync(cancellationToken);
+    }
 }

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

@@ -47,20 +47,20 @@ public class DifferentialNavigation : SimulationNavigation
                         var deviation = CurrentBaseNode.NodeId == SimulationOrderNodes[^1].NodeId ? 0.02 : 0.1;
                         if (DistanceToCheckingNode > deviation)
                         {
-                            double SpeedTarget = MovePID.PID_step(DistanceToCheckingNode, SimulationModel.MaxVelocity, 0, CycleHandlerMilliseconds / 1000.0);
+                            double SpeedTarget = MovePID.PID_step(DistanceToCheckingNode, RobotConfiguration.SimulationModel.MaxVelocity, 0, CycleHandlerMilliseconds / 1000.0);
                             double AngularVel = MovePurePursuit.PurePursuit_step(Visualization.X, Visualization.Y, Visualization.Theta * Math.PI / 180);
                             AngularVel *= NavigationPath[MovePurePursuit.OnNodeIndex].Direction == RobotDirection.FORWARD ? 1 : -1;
                             (double AngularVelocityLeft, double AngularVelocityRight) = MoveFuzzy.Fuzzy_step(SpeedTarget, AngularVel, CycleHandlerMilliseconds / 1000.0);
 
                             if (NavigationPath[MovePurePursuit.OnNodeIndex].Direction == RobotDirection.FORWARD)
                             {
-                                AngularVelocityLeft /= SimulationModel.RadiusWheel;
+                                AngularVelocityLeft /= RobotConfiguration.SimulationModel.RadiusWheel;
-                                AngularVelocityRight = AngularVelocityRight / SimulationModel.RadiusWheel * -1;
+                                AngularVelocityRight = AngularVelocityRight / RobotConfiguration.SimulationModel.RadiusWheel * -1;
                             }
                             else
                             {
-                                AngularVelocityLeft = AngularVelocityLeft / SimulationModel.RadiusWheel * -1;
+                                AngularVelocityLeft = AngularVelocityLeft / RobotConfiguration.SimulationModel.RadiusWheel * -1;
-                                AngularVelocityRight /= SimulationModel.RadiusWheel;
+                                AngularVelocityRight /= RobotConfiguration.SimulationModel.RadiusWheel;
                             }
                             VelocityController.SetSpeed(AngularVelocityLeft, AngularVelocityRight, CycleHandlerMilliseconds / 1000.0);
                         }

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

@@ -1,12 +1,9 @@
-using Microsoft.AspNetCore.Components;
+using RobotApp.Common.Shares;
-using RobotApp.Common.Shares;
 using RobotApp.Common.Shares.Enums;
 using RobotApp.Interfaces;
 using RobotApp.Services.Exceptions;
 using RobotApp.Services.Robot.Simulation.Navigation.Algorithm;
 using RobotApp.VDA5050.Order;
-using RobotApp.VDA5050.State;
-using System.IO;
 
 namespace RobotApp.Services.Robot.Simulation.Navigation;
 
@@ -39,7 +36,7 @@ public class SimulationNavigation : INavigation, IDisposable
 
     protected readonly SimulationVisualization Visualization;
     protected readonly SimulationVelocity VelocityController;
-    protected readonly SimulationModel SimulationModel;
+    protected readonly RobotConfiguration RobotConfiguration;
     protected readonly RobotPathPlanner PathPlanner;
     protected NavigationNode[] NavigationPath = [];
     protected NavigationNode? CurrentBaseNode;
@@ -53,9 +50,9 @@ public class SimulationNavigation : INavigation, IDisposable
         using var scope = ServiceProvider.CreateScope();
         Logger = scope.ServiceProvider.GetRequiredService<Logger<SimulationNavigation>>();
         Visualization = scope.ServiceProvider.GetRequiredService<SimulationVisualization>();
-        SimulationModel = scope.ServiceProvider.GetRequiredService<SimulationModel>();
+        RobotConfiguration = scope.ServiceProvider.GetRequiredService<RobotConfiguration>();
         PathPlanner = scope.ServiceProvider.GetRequiredService<RobotPathPlanner>();
-        VelocityController = new(Visualization, SimulationModel);
+        VelocityController = new(Visualization, RobotConfiguration.SimulationModel);
     }
 
     protected void HandleNavigationStart()

RobotApp/Services/Robot/Simulation/SimulationExtensions.cs

@@ -7,7 +7,6 @@ public static class SimulationExtensions
     public static IServiceCollection AddRobotSimulation(this IServiceCollection services)
     {
         services.AddSingleton<SimulationVisualization>();
-        services.AddSingleton<SimulationModel>();
         return services;
     }
 }

RobotApp/Services/Robot/Simulation/SimulationModel.cs

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

RobotApp/Services/Robot/Simulation/SimulationVisualization.cs

@@ -1,6 +1,6 @@
 namespace RobotApp.Services.Robot.Simulation;
 
-public class SimulationVisualization(SimulationModel Model)
+public class SimulationVisualization(RobotConfiguration RobotConfiguration)
 {
     public double X { get; private set; }
     public double Y { get; private set; }
@@ -9,8 +9,8 @@ public class SimulationVisualization(SimulationModel Model)
     public double Vy { get; private set; }
     public double Omega { get; private set; }
 
-    private readonly double RadiusWheel = Model.RadiusWheel;
+    private readonly double RadiusWheel = RobotConfiguration.SimulationModel.RadiusWheel;
-    private readonly double RadiusRobot = Model.Width / 2;
+    private readonly double RadiusRobot = RobotConfiguration.SimulationModel.Width / 2;
 
     public (double x, double y, double angle) UpdatePosition(double wL, double wR, double time)
     {

RobotApp/Services/RobotExtensions.cs

@@ -10,8 +10,22 @@ public static class RobotExtensions
     public static IServiceCollection AddRobot(this IServiceCollection services)
     {
         services.AddSingleton<RobotStateMachine>();
+        services.AddSingleton<RobotConfiguration>();
+        services.AddSingleton<RobotPathPlanner>();
+        services.AddSingleton<RobotConnection>();
 
+        services.AddInterfaceServiceSingleton<IBattery, RobotBattery>();
+        services.AddInterfaceServiceSingleton<IDriver, RobotDriver>();
+        services.AddInterfaceServiceSingleton<IError, RobotErrors>();
+        services.AddInterfaceServiceSingleton<IInfomation, RobotInfomations>();
+        services.AddInterfaceServiceSingleton<IInstantActions, RobotAction>();
         services.AddInterfaceServiceSingleton<ILocalization, RobotLocalization>();
+        services.AddInterfaceServiceSingleton<INavigation, RobotNavigation>();
+        services.AddHostedInterfaceServiceSingleton<IPeripheral, ISafety, RobotPeripheral>();
+        services.AddInterfaceServiceSingleton<IOrder, RobotOrderController>();
+
+        services.AddHostedServiceSingleton<RobotController>();
+        services.AddHostedServiceSingleton<RobotVisualization>();
         return services;
     }
 
@@ -36,4 +50,13 @@ public static class RobotExtensions
         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;
+    }
 }

RobotApp/Services/State/AutoState.cs

@@ -4,4 +4,32 @@ namespace RobotApp.Services.State;
 
 public class AutoState<T>(AutoStateType state, IRobotState? superState = null) : RobotState<T>(state, superState) where T : Enum
 {
+    public override void Enter()
+    {
+        base.Enter();
+        switch (Name)
+        {
+            case AutoStateType.Idle:
+            case AutoStateType.Holding:
+            case AutoStateType.Paused:
+            case AutoStateType.Canceling:
+            case AutoStateType.Recovering:
+            case AutoStateType.Remote_Override:
+            case AutoStateType.Executing:
+                if (HistoryState is not null && SubStates.Contains(HistoryState)) ActiveSubState = HistoryState;
+                else ActiveSubState = SubStates.FirstOrDefault(s => s.Name.Equals(ExecutingStateType.Moving));
+                ActiveSubState?.Enter();
+                break;
+        }
+    }
+    public override void Exit()
+    {
+        if (Name.Equals(AutoStateType.Executing) && ActiveSubState != null)
+        {
+            HistoryState = ActiveSubState;
+        }
+        ActiveSubState?.Exit();
+        ActiveSubState = null;
+        base.Exit();
+    }
 }

RobotApp/Services/State/FaultState.cs

@@ -4,4 +4,12 @@ namespace RobotApp.Services.State;
 
 public class FaultState<T>(FaultStateType state, IRobotState? superState = null) : RobotState<T>(state, superState) where T : Enum
 {
+    public override bool CanTransitionTo(IRobotState targetState)
+    {
+        if (targetState == null) return false;
+        return targetState.Name.Equals(RootStateType.System) ||
+               targetState.Name.Equals(RootStateType.Manual) ||
+               targetState.Name.Equals(RootStateType.Service) ||
+               targetState.Name.Equals(RootStateType.Stop);
+    }
 }

RobotApp/Services/State/ManualState.cs

@@ -4,4 +4,14 @@ namespace RobotApp.Services.State;
 
 public class ManualState<T>(ManualStateType state, IRobotState? superState = null) : RobotState<T>(state, superState) where T : Enum
 {
+    public override bool CanTransitionTo(IRobotState targetState)
+    {
+        if (targetState == null) return false;
+
+        return targetState.Name.Equals(RootStateType.Auto) ||
+               targetState.Name.Equals(RootStateType.Service) ||
+               targetState.Name.Equals(RootStateType.System) ||
+               targetState.Name.Equals(RootStateType.Stop) ||
+               targetState.Name.Equals(RootStateType.Fault);
+    }
 }

RobotApp/Services/State/RobotStateMachine.cs

@@ -1,4 +1,5 @@
 using RobotApp.Common.Shares.Enums;
+using RobotApp.Interfaces;
 
 namespace RobotApp.Services.State;
 
@@ -6,45 +7,13 @@ public record RobotStateMachine(Logger<RobotStateMachine> Logger) : IDisposable
 {
     private readonly Dictionary<Type, Dictionary<Enum, IRobotState>> StateRegistry = [];
 
-    public IRobotState? CurrentState { get; private set; }
+    public IRobotState CurrentState { get; private set; } = null!;
     public event Action<IRobotState?, IRobotState>? OnStateChanged;
     public bool IsInitialized = false;
+    public string CurrentStateName => CurrentState.Name.ToString();
 
     private readonly Lock StateLock = new();
 
-    private void PrintStateAdded()
-    {
-        Console.WriteLine("=== All Registered States ===");
-        var allStates = StateRegistry.Values
-            .SelectMany(dict => dict.Values)
-            .Where(state => state.SuperState == null)
-            .OrderBy(state => state.Type.Name)
-            .ThenBy(state => state.Name.ToString());
-
-        foreach (var rootState in allStates)
-        {
-            PrintStateSimple(rootState, 0);
-        }
-
-        var totalStates = StateRegistry.Values.Sum(dict => dict.Count);
-        Console.WriteLine($"\nTotal: {totalStates} states registered");
-        Console.WriteLine("==============================\n");
-    }
-
-    private void PrintStateSimple(IRobotState state, int depth)
-    {
-        var indent = new string(' ', depth * 4);
-        var marker = state == CurrentState ? " [CURRENT]" : "";
-        var activeMarker = state.SuperState?.ActiveSubState == state ? " [ACTIVE]" : "";
-
-        Console.WriteLine($"{indent}- {state.Name}{marker}{activeMarker}");
-
-        foreach (var subState in state.SubStates.OrderBy(s => s.Name.ToString()))
-        {
-            PrintStateSimple(subState, depth + 1);
-        }
-    }
-
     public void InitializeHierarchyStates()
     {
         if (IsInitialized) return;
@@ -62,17 +31,28 @@ public record RobotStateMachine(Logger<RobotStateMachine> Logger) : IDisposable
         SetupFaultState();
 
         var systemState = GetState(RootStateType.System);
-        lock (StateLock)
-        {
         systemState.Enter();
-            CurrentState = systemState;
-        }
 
-        PrintStateAdded();
         IsInitialized = true;
         Logger.Info($"Khởi tạo thành công State Machine với State hiện tại: {GetCurrentStatePath()}");
     }
 
+    private void OnStateEnter(IRobotState state)
+    {
+        lock (StateLock)
+        {
+            CurrentState = state;
+        }
+    }
+
+    private void OnStateExit(IRobotState state)
+    {
+        lock (StateLock)
+        {
+            
+        }
+    }
+
     private void SetupSystemState()
     {
         var systemlState = GetState(RootStateType.System) ?? throw new InvalidOperationException($"Failed to get state RootStateType.Auto");
@@ -142,12 +122,14 @@ public record RobotStateMachine(Logger<RobotStateMachine> Logger) : IDisposable
         var emcStopState = new StopState<StopStateType>(StopStateType.EMC, stopState);
         var protectiveState = new StopState<StopStateType>(StopStateType.Protective, stopState);
         var manualStopState = new StopState<StopStateType>(StopStateType.Manual, stopState);
+        var bumberStopState = new StopState<StopStateType>(StopStateType.Bumber, stopState);
 
-        stopState.SubStates.AddRange([emcStopState, protectiveState, manualStopState]);
+        stopState.SubStates.AddRange([emcStopState, protectiveState, manualStopState, bumberStopState]);
 
         RegisterState(emcStopState);
         RegisterState(protectiveState);
         RegisterState(manualStopState);
+        RegisterState(bumberStopState);
     }
 
     private void SetupFaultState()
@@ -251,6 +233,8 @@ public record RobotStateMachine(Logger<RobotStateMachine> Logger) : IDisposable
     {
         var enumType = typeof(T);
         StateRegistry.TryAdd(enumType, []);
+        state.OnEnter += OnStateEnter;
+        state.OnExit += OnStateExit;
         StateRegistry[enumType][state.Name] = state;
     }
 
@@ -314,6 +298,38 @@ public record RobotStateMachine(Logger<RobotStateMachine> Logger) : IDisposable
         }
     }
 
+    public bool TransitionToStop(StopStateType stopType)
+    {
+        var stopState = GetState(RootStateType.Stop);
+        var subStopState = stopState.SubStates.FirstOrDefault(s => s.Name.Equals(stopType));
+        if(subStopState is not null)
+        {
+            var transitionToStop = TransitionTo(RootStateType.Stop);
+            if (transitionToStop)
+            {
+                subStopState.Enter();
+                return true;
+            }
+        }
+        return false;
+    }
+
+    public bool TransitionToFault(FaultStateType faultType)
+    {
+        var faultState = GetState(RootStateType.Fault);
+        var subFaultState = faultState.SubStates.FirstOrDefault(s => s.Name.Equals(faultType));
+        if (subFaultState is not null)
+        {
+            var transitionToStop = TransitionTo(RootStateType.Fault);
+            if (transitionToStop)
+            {
+                subFaultState.Enter();
+                return true;
+            }
+        }
+        return false;
+    }
+
     public void Update()
     {
         if (!IsInitialized)

RobotApp/Services/State/RootState.cs

@@ -6,6 +6,7 @@ public class RootState<T>(RootStateType state, IRobotState? superState = null) :
 {
     public override void Enter()
     {
+        base.Enter();
         switch (Name)
         {
             case RootStateType.System:
@@ -13,24 +14,99 @@ public class RootState<T>(RootStateType state, IRobotState? superState = null) :
                 ActiveSubState?.Enter();
                 break;
             case RootStateType.Auto:
-                if(HistoryState is not null) ActiveSubState = HistoryState;
+                if (HistoryState is not null && SubStates.Contains(HistoryState)) ActiveSubState = HistoryState;
                 else ActiveSubState = SubStates.FirstOrDefault(s => s.Name.Equals(AutoStateType.Idle));
                 ActiveSubState?.Enter();
                 break;
+            case RootStateType.Manual:
+                ActiveSubState = SubStates.FirstOrDefault(s => s.Name.Equals(ManualStateType.Idle));
+                ActiveSubState?.Enter();
+                break;
+            case RootStateType.Service:
+                ActiveSubState = SubStates.FirstOrDefault(s => s.Name.Equals(ServiceStateType.Idle));
+                ActiveSubState?.Enter();
+                break;
+            case RootStateType.Stop:
+                break;
+
+            case RootStateType.Fault:
+                break;
         }
     }
 
     public override void Exit()
     {
+        if (Name.Equals(RootStateType.Auto) && ActiveSubState != null)
+        {
+            HistoryState = ActiveSubState;
+        }
+
+        ActiveSubState?.Exit();
+        ActiveSubState = null;
+
+        base.Exit();
     }
 
     public override bool CanTransitionTo(IRobotState targetState)
     {
-        return true;
+        if (targetState == null) return false;
+        if (targetState.Name.Equals(RootStateType.Stop) || targetState.Name.Equals(RootStateType.Fault)) return true;
+
+        return Name switch
+        {
+            RootStateType.System => CanTransitionFromSystem(targetState),
+            RootStateType.Auto => CanTransitionFromAuto(targetState),
+            RootStateType.Manual => CanTransitionFromManual(targetState),
+            RootStateType.Service => CanTransitionFromService(targetState),
+            RootStateType.Stop => CanTransitionFromStop(targetState),
+            RootStateType.Fault => CanTransitionFromFault(targetState),
+            _ => false
+        };
     }
 
     public override void Update()
     {
-        ActiveSubState?.Update();
+        base.Update();
+    }
+
+    private static bool CanTransitionFromSystem(IRobotState targetState)
+    {
+        return targetState.Name.Equals(RootStateType.Auto) ||
+               targetState.Name.Equals(RootStateType.Manual) ||
+               targetState.Name.Equals(RootStateType.Service);
+    }
+
+    private static bool CanTransitionFromAuto(IRobotState targetState)
+    {
+        return targetState.Name.Equals(RootStateType.Manual) ||
+               targetState.Name.Equals(RootStateType.Service) ||
+               targetState.Name.Equals(RootStateType.System);
+    }
+
+    private static bool CanTransitionFromManual(IRobotState targetState)
+    {
+        return targetState.Name.Equals(RootStateType.Auto) ||
+               targetState.Name.Equals(RootStateType.Service) ||
+               targetState.Name.Equals(RootStateType.System);
+    }
+
+    private static bool CanTransitionFromService(IRobotState targetState)
+    {
+        return targetState.Name.Equals(RootStateType.Auto) ||
+               targetState.Name.Equals(RootStateType.Manual) ||
+               targetState.Name.Equals(RootStateType.System);
+    }
+
+    private static bool CanTransitionFromStop(IRobotState targetState)
+    {
+        return targetState.Name.Equals(RootStateType.System) ||
+               targetState.Name.Equals(RootStateType.Manual);
+    }
+
+    private static bool CanTransitionFromFault(IRobotState targetState)
+    {
+        return targetState.Name.Equals(RootStateType.System) ||
+               targetState.Name.Equals(RootStateType.Manual) ||
+               targetState.Name.Equals(RootStateType.Service);
     }
 }

RobotApp/Services/State/ServiceState.cs

@@ -4,4 +4,14 @@ namespace RobotApp.Services.State;
 
 public class ServiceState<T>(ServiceStateType state, IRobotState? superState = null) : RobotState<T>(state, superState) where T : Enum
 {
+    public override bool CanTransitionTo(IRobotState targetState)
+    {
+        if (targetState == null) return false;
+
+        return targetState.Name.Equals(RootStateType.Auto) ||
+               targetState.Name.Equals(RootStateType.Service) ||
+               targetState.Name.Equals(RootStateType.System) ||
+               targetState.Name.Equals(RootStateType.Stop) ||
+               targetState.Name.Equals(RootStateType.Fault);
+    }
 }

RobotApp/Services/State/StopState.cs

@@ -1,8 +1,15 @@
 using RobotApp.Common.Shares.Enums;
 
-namespace RobotApp.Services.State
+namespace RobotApp.Services.State;
-{
+
 public class StopState<T>(StopStateType state, IRobotState? superState = null) : RobotState<T>(state, superState) where T : Enum
 {
+    public override bool CanTransitionTo(IRobotState targetState)
+    {
+        if (targetState == null) return false;
+        return targetState.Name.Equals(RootStateType.System) ||
+               targetState.Name.Equals(RootStateType.Manual) ||
+               targetState.Name.Equals(RootStateType.Service) ||
+               targetState.Name.Equals(RootStateType.Fault);
     }
 }

RobotApp/Services/State/SystemState.cs

@@ -4,18 +4,32 @@ namespace RobotApp.Services.State;
 
 public class SystemState<T>(SystemStateType state, IRobotState? superState = null) : RobotState<T>(state, superState) where T : Enum
 {
-    public override void Enter()
-    {
-    }
-    public override void Exit()
-    {
-    }
     public override bool CanTransitionTo(IRobotState targetState)
     {
-        return true;
+        if (targetState == null) return false;
-    }
+
-    public override void Update()
+        switch (Name)
         {
-        ActiveSubState?.Update();
+            case SystemStateType.Initializing:
+                return targetState.Name.Equals(SystemStateType.Standby) ||
+                       targetState.Name.Equals(RootStateType.Fault) ||
+                       targetState.Name.Equals(RootStateType.Stop);
+
+            case SystemStateType.Standby:
+                return targetState.Name.Equals(RootStateType.Auto) ||
+                       targetState.Name.Equals(RootStateType.Manual) ||
+                       targetState.Name.Equals(RootStateType.Service) ||
+                       targetState.Name.Equals(SystemStateType.Shutting_Down) ||
+                       targetState.Name.Equals(RootStateType.Stop) ||
+                       targetState.Name.Equals(RootStateType.Fault);
+
+            case SystemStateType.Shutting_Down:
+                return targetState.Name.Equals(RootStateType.Stop) ||
+                       targetState.Name.Equals(SystemStateType.Initializing);
+            default:
+                break;
+        }
+
+        return false;
     }
 }