update

RobotApp.Common.Shares/Enums/RobotDirection.cs

@@ -0,0 +1,7 @@
+namespace RobotApp.Common.Shares.Enums;
+
+public enum RobotDirection
+{
+    FORWARD,
+    BACKWARD,
+}

RobotApp.Common.Shares/Enums/StateType.cs

@@ -0,0 +1,39 @@
+namespace RobotApp.Common.Shares.Enums;
+
+public enum RootStateType
+{
+    Booting,
+    Operational,
+}
+
+public enum OperationalStateType
+{
+
+}
+
+public enum AutomationStateType
+{
+    Idle,
+    Executing,
+    Paused,
+    Charging,
+    Error,
+    Remote_Override,
+}
+
+public enum ManualStateType
+{
+    Idle,
+    Active,
+}
+
+public enum SafetyStateType
+{
+    Init,
+    Run_Ok,
+    SS1,
+    STO,
+    PDS,
+    SLS,
+    Error,
+}

RobotApp.Common.Shares/Enums/TrajectoryDegree.cs

@@ -0,0 +1,8 @@
+namespace RobotApp.Common.Shares.Enums;
+
+public enum TrajectoryDegree
+{
+    One,
+    Two,
+    Three,
+}

RobotApp.Common.Shares/MathExtensions.cs

@@ -0,0 +1,80 @@
+using RobotApp.Common.Shares.Model;
+
+namespace RobotApp.Common.Shares;
+
+public static class MathExtensions
+{
+    public static (double x, double y) CurveDegreeTwo(double t, double x1, double y1, double controlPointX, double controlPointY, double x2, double y2)
+    {
+        var x = (1 - t) * (1 - t) * x1 + 2 * t * (1 - t) * controlPointX + t * t * x2;
+        var y = (1 - t) * (1 - t) * y1 + 2 * t * (1 - t) * controlPointY + t * t * y2;
+        return (x, y);
+    }
+
+    public static (double x, double y) CurveDegreeThree(double t, double x1, double y1, double controlPoint1X, double controlPoint1Y, double controlPoint2X, double controlPoint2Y, double x2, double y2)
+    {
+        var x = Math.Pow(1 - t, 3) * x1 + 3 * Math.Pow(1 - t, 2) * t * controlPoint1X + 3 * Math.Pow(t, 2) * (1 - t) * controlPoint2X + Math.Pow(t, 3) * x2; ;
+        var y = Math.Pow(1 - t, 3) * y1 + 3 * Math.Pow(1 - t, 2) * t * controlPoint1Y + 3 * Math.Pow(t, 2) * (1 - t) * controlPoint2Y + Math.Pow(t, 3) * y2;
+        return (x, y);
+    }
+
+
+    public static (double x, double y) Curve(double t, EdgeCalculatorModel edge)
+    {
+        if (edge.TrajectoryDegree == Enums.TrajectoryDegree.One)
+        {
+            return (edge.X1 + t * (edge.X2 - edge.X1), edge.Y1 + t * (edge.Y2 - edge.Y1));
+        }
+        else if (edge.TrajectoryDegree == Enums.TrajectoryDegree.Two)
+        {
+            return CurveDegreeTwo(t, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.X2, edge.Y2);
+        }
+        else
+        {
+            return CurveDegreeThree(t, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.ControlPoint2X, edge.ControlPoint2Y, edge.X2, edge.Y2);
+        }
+    }
+
+    public static (double x, double y) Curve(this EdgeCalculatorModel edge, double t)
+    {
+        return Curve(t, edge);
+    }
+
+    public static double GetEdgeLength(this EdgeCalculatorModel edge)
+    {
+        double distance = 0;
+        if (edge.TrajectoryDegree == Enums.TrajectoryDegree.One)
+        {
+            distance = Math.Sqrt(Math.Pow(edge.X1 - edge.X2, 2) + Math.Pow(edge.Y1 - edge.Y2, 2));
+        }
+        else if (edge.TrajectoryDegree == Enums.TrajectoryDegree.Two)
+        {
+            var length = Math.Sqrt(Math.Pow(edge.X1 - edge.X2, 2) + Math.Pow(edge.Y1 - edge.Y2, 2));
+            if (length == 0) return 0;
+            double step = 0.1 / length;
+
+            for (double t = step; t <= 1.001; t += step)
+            {
+                (double x1, double y1) = CurveDegreeTwo(t - step, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.X2, edge.Y2);
+                (double x2, double y2) = CurveDegreeTwo(t, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.X2, edge.Y2);
+                distance += Math.Sqrt(Math.Pow(x1 - x2, 2) + Math.Pow(y1 - y2, 2));
+            }
+        }
+        else
+        {
+            var length = Math.Sqrt(Math.Pow(edge.X1 - edge.X2, 2) + Math.Pow(edge.Y1 - edge.Y2, 2));
+            if (length == 0) return 0;
+            double step = 0.1 / length;
+            for (double t = step; t <= 1.001; t += step)
+            {
+                var sTime = t - step;
+                (var sx, var sy) = CurveDegreeThree(1 - sTime, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.ControlPoint2X, edge.ControlPoint2Y, edge.X2, edge.Y2);
+                sTime = t;
+                (var ex, var ey) = CurveDegreeThree(1 - sTime, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.ControlPoint2X, edge.ControlPoint2Y, edge.X2, edge.Y2);
+
+                distance += Math.Sqrt(Math.Pow(sx - ex, 2) + Math.Pow(sy - ey, 2));
+            }
+        }
+        return distance;
+    }
+}

RobotApp.Common.Shares/Model/EdgeCalculatorModel.cs

@@ -0,0 +1,16 @@
+using RobotApp.Common.Shares.Enums;
+
+namespace RobotApp.Common.Shares.Model;
+
+public class EdgeCalculatorModel
+{
+    public double X1 { get; set; }
+    public double Y1 { get; set; }
+    public double X2 { get; set; }
+    public double Y2 { get; set; }
+    public TrajectoryDegree TrajectoryDegree { get; set; }
+    public double ControlPoint1X { get; set; }
+    public double ControlPoint1Y { get; set; }
+    public double ControlPoint2X { get; set; }
+    public double ControlPoint2Y { get; set; }
+}

RobotApp.VDA5050/State/ActionState.cs

@@ -2,8 +2,6 @@
 
 namespace RobotApp.VDA5050.State;
 
-#nullable disable
-
 public enum ActionStatus
 {
     WAITING,
@@ -15,11 +13,11 @@ public enum ActionStatus
 }
 public class ActionState
 {
-    public string ActionType { get; set; }
+    public string ActionType { get; set; } = string.Empty;
     [Required]
-    public string ActionId { get; set; }
-    public string ActionDescription { get; set; }
+    public string ActionId { get; set; } = string.Empty;
+    public string ActionDescription { get; set; } = string.Empty;
     [Required]
-    public string ActionStatus { get; set; }
-    public string ResultDescription { get; set; }
+    public string ActionStatus { get; set; } = string.Empty;
+    public string ResultDescription { get; set; } = string.Empty;
 }

RobotApp.VDA5050/State/EdgeState.cs

@@ -3,17 +3,15 @@ using System.ComponentModel.DataAnnotations;
 
 namespace RobotApp.VDA5050.State;
 
-#nullable disable
-
 public class EdgeState
 {
 
     [Required]
-    public string EdgeId { get; set; }
+    public string EdgeId { get; set; } = string.Empty;
     [Required]
     public int SequenceId { get; set; }
-    public string EdgeDescription { get; set; }
+    public string EdgeDescription { get; set; } = string.Empty;
     [Required]
     public bool Released { get; set; }
-    public Trajectory Trajectory { get; set; }
+    public Trajectory Trajectory { get; set; } = new();
 }

RobotApp.VDA5050/State/Error.cs

@@ -2,28 +2,33 @@
 
 namespace RobotApp.VDA5050.State;
 
-#nullable disable
-
 public enum ErrorLevel
 {
     NONE,
     WARNING,
     FATAL
 }
+
 public class ErrorReferences
 {
     [Required]
-    public string ReferenceKey { get; set; }
+    public string ReferenceKey { get; set; } = string.Empty;
     [Required]
-    public string ReferenceValue { get; set; }
+    public string ReferenceValue { get; set; } = string.Empty;
 }
+
+public enum ErrorType
+{
+    INITIALIZE_ORDER,
+}
+
 public class Error
 {
     [Required]
-    public string ErrorType { get; set; }
-    public ErrorReferences[] ErrorReferences { get; set; }
-    public string ErrorDescription { get; set; }
-    public string ErrorHint { get; set; }
+    public string ErrorType { get; set; } = string.Empty;
+    public ErrorReferences[] ErrorReferences { get; set; } = [];
+    public string ErrorDescription { get; set; } = string.Empty;
+    public string ErrorHint { get; set; } = string.Empty;
     [Required]
-    public string ErrorLevel { get; set; }
+    public string ErrorLevel { get; set; } = string.Empty;
 }

RobotApp.VDA5050/State/Information.cs

@@ -2,7 +2,6 @@
 
 namespace RobotApp.VDA5050.State;
 
-#nullable disable
 
 public enum InfoLevel
 {
@@ -12,16 +11,16 @@ public enum InfoLevel
 public class InfomationReferences
 {
     [Required]
-    public string ReferenceKey { get; set; }
+    public string ReferenceKey { get; set; } = string.Empty;
     [Required]
-    public string ReferenceValue { get; set; }
+    public string ReferenceValue { get; set; } = string.Empty;
 }
 public class Information
 {
     [Required]
-    public string InfoType { get; set; }
-    public InfomationReferences[] InfoReferences { get; set; }
-    public string InfoDescription { get; set; }
+    public string InfoType { get; set; } = string.Empty;
+    public InfomationReferences[] InfoReferences { get; set; } = [];
+    public string InfoDescription { get; set; } = string.Empty;
     [Required]
-    public string InfoLevel { get; set; }
+    public string InfoLevel { get; set; } = string.Empty;
 }

RobotApp.VDA5050/State/Load.cs

@@ -2,15 +2,13 @@
 
 namespace RobotApp.VDA5050.State;
 
-#nullable disable
-
 public class Load
 {
-    public string LoadId { get; set; }
-    public string LoadType { get; set; }
-    public string LoadPosition { get; set; }
-    public BoundingBoxReference BoundingBoxReference { get; set; }
-    public LoadDimensions LoadDimensions { get; set; }
+    public string LoadId { get; set; } = string.Empty;
+    public string LoadType { get; set; } = string.Empty;
+    public string LoadPosition { get; set; } = string.Empty;
+    public BoundingBoxReference BoundingBoxReference { get; set; } = new();
+    public LoadDimensions LoadDimensions { get; set; } = new();
     public double Weight { get; set; } 
 
 }

RobotApp.VDA5050/State/NodeState.cs

@@ -1,20 +1,18 @@
-using RobotApp.VDA5050.Order;
-using System.ComponentModel.DataAnnotations;
+using System.ComponentModel.DataAnnotations;
 
 namespace RobotApp.VDA5050.State;
 
-#nullable disable
 
 public class NodeState
 {
     [Required]
-    public string NodeId { get; set; }
+    public string NodeId { get; set; } = string.Empty;
     [Required]
     public int SequenceId { get; set; }
-    public string NodeDescription { get; set; }
+    public string NodeDescription { get; set; } = string.Empty;
     [Required]
     public bool Released { get; set; }
-    public NodePosition NodePosition { get; set; }
+    public NodePosition NodePosition { get; set; } = new();
 }
 
 public class NodePosition
@@ -25,6 +23,6 @@ public class NodePosition
     public double Y { get; set; }
     public double Theta { get; set; }
     [Required]
-    public string MapId { get; set; } = "";
+    public string MapId { get; set; } = string.Empty;
 
 }

RobotApp.VDA5050/State/StateMsg.cs

@@ -3,8 +3,6 @@ using System.ComponentModel.DataAnnotations;
 
 namespace RobotApp.VDA5050.State;
 
-#nullable disable
-
 public enum OperatingMode
 {
     AUTOMATIC,
@@ -17,22 +15,21 @@ public class StateMsg
 {
     [Required]
     public uint HeaderId { get; set; }
+    public string Timestamp { get; set; } = DateTime.Now.ToString("yyyy-MM-ddTHH:mm:ss.fffZ");
     [Required]
-    public string Timestamp { get; set; }
+    public string Version { get; set; } = "1.0.0";
     [Required]
-    public string Version { get; set; }
+    public string Manufacturer { get; set; } = "PhenikaaX";
     [Required]
-    public string Manufacturer { get; set; }
-    [Required]
-    public string SerialNumber { get; set; }
+    public string SerialNumber { get; set; } = string.Empty;
     public Map[] Maps { get; set; } = [];
     [Required]
-    public string OrderId { get; set; }
+    public string OrderId { get; set; } = string.Empty;
     [Required]
     public int OrderUpdateId { get; set; }
-    public string ZoneSetId { get; set; }
+    public string ZoneSetId { get; set; } = string.Empty;
     [Required]
-    public string LastNodeId { get; set; }
+    public string LastNodeId { get; set; } = string.Empty;
     [Required]
     public int LastNodeSequenceId { get; set; }
     [Required]
@@ -41,7 +38,7 @@ public class StateMsg
     public bool NewBaseRequest { get; set; }
     public double DistanceSinceLastNode { get; set; }
     [Required]
-    public string OperatingMode { get; set; }
+    public string OperatingMode { get; set; } = string.Empty;
     [Required]
     public NodeState[] NodeStates { get; set; } = [];
     [Required]

RobotApp.VDA5050/Visualization/AgvPosition.cs

@@ -2,8 +2,6 @@
 
 namespace RobotApp.VDA5050.Visualization;
 
-#nullable disable
-
 public class AgvPosition
 {
     [Required]
@@ -11,7 +9,7 @@ public class AgvPosition
     [Required]
     public double Y { get; set; }
     [Required]
-    public string MapId { get; set; }
+    public string MapId { get; set; } = string.Empty;
     [Required]
     public double Theta { get; set; }
     [Required]

RobotApp.VDA5050/Visualization/Visualizationmsg.cs

@@ -1,16 +1,16 @@
-namespace RobotApp.VDA5050.Visualization;
+using System.ComponentModel.DataAnnotations;
 
-#nullable disable
+namespace RobotApp.VDA5050.Visualization;
 
 public class VisualizationMsg
 {
     public uint HeaderId { get; set; }
-    public string Timestamp { get; set; }
-    public string Version { get; set; }
-    public string Manufacturer { get; set; }
-    public string SerialNumber { get; set; }
-    public string MapId { get; set; }
-    public string MapDescription { get; set; }
+    public string Timestamp { get; set; } = DateTime.Now.ToString("yyyy-MM-ddTHH:mm:ss.fffZ");
+    public string Version { get; set; } = "1.0.0";
+    public string Manufacturer { get; set; } = "PhenikaaX";
+    public string SerialNumber { get; set; } = string.Empty;
+    public string MapId { get; set; } = string.Empty;
+    public string MapDescription { get; set; } = string.Empty;
     public AgvPosition AgvPosition { get; set; } = new();
     public Velocity Velocity { get; set; } = new();
 }

RobotApp/Interfaces/IBattery.cs

@@ -0,0 +1,5 @@
+namespace RobotApp.Interfaces;
+
+public interface IBattery
+{
+}

RobotApp/Interfaces/IError.cs

@@ -0,0 +1,9 @@
+using RobotApp.VDA5050.State;
+
+namespace RobotApp.Interfaces;
+
+public interface IError
+{
+    bool HasFatalError { get; }
+    void AddError(Error error, TimeSpan? clearAfter = null);
+}

RobotApp/Interfaces/IInfomation.cs

@@ -0,0 +1,5 @@
+namespace RobotApp.Interfaces;
+
+public interface IInfomation
+{
+}

RobotApp/Interfaces/IInstanceActions.cs

@@ -1,5 +1,14 @@
-namespace RobotApp.Interfaces;
+using RobotApp.VDA5050.State;
+using Action = RobotApp.VDA5050.InstantAction.Action;
+
+namespace RobotApp.Interfaces;
 
 public interface IInstanceActions
 {
+    ActionState[] ActionStates { get; }
+    bool HasActionRunning { get; }
+    bool AddOrderActions(Action[] actions);
+    bool StartAction(string actionId);
+    bool AddInstanceAction(Action action);
+    bool StopAction();
 }

RobotApp/Interfaces/INavigation.cs

@@ -4,6 +4,9 @@ namespace RobotApp.Interfaces;
 
 public enum NavigationState
 {
+    None,
+    Initializing,
+    Initialized,
     Idle,
     Moving,
     Rotating,
@@ -13,7 +16,7 @@ public enum NavigationState
 
 public enum NavigationProccess
 {
-    Started,
+    None,
     InProgress,
     Completed,
     Failed,
@@ -22,12 +25,13 @@ public enum NavigationProccess
 
 public interface INavigation
 {
+    bool Driving { get; }
     NavigationState State { get; }
-    NavigationProccess Proccess { get; }
-    bool Move(Node nodes, Edge edges);
-    bool MoveStraight(double x, double y);
-    bool Rotate(double angle);
-    bool Paused();
-    bool Resume();  
-    bool CancelMovement();
+    void Move(Node[] nodes, Edge[] edges);
+    void MoveStraight(double x, double y);
+    void Rotate(double angle);
+    void Paused();
+    void Resume();
+    void UpdateOrder(int lastBaseSequence);
+    void CancelMovement();
 }

RobotApp/Interfaces/IOrder.cs

@@ -1,15 +1,19 @@
-using RobotApp.VDA5050.State;
+using RobotApp.VDA5050.Order;
+using RobotApp.VDA5050.State;
 
 namespace RobotApp.Interfaces;
 
 public interface IOrder
 {
     string OrderId { get; }
-    VDA5050.InstantAction.Action[] Actions { get; }
+    int OrderUpdateId { get; }
+    string LastNodeId { get; }
+    int LastNodeSequenceId { get; }
+
     NodeState[] NodeStates { get; }
     EdgeState[] EdgeStates { get; }
 
-    bool StartOrder();
-    bool UpdateOrder();
-    bool StopOrder();
+    void StartOrder(string orderId, Node[] nodes, Edge[] edges);
+    void UpdateOrder(int orderUpdateId, Node[] nodes, Edge[] edges);
+    void StopOrder();
 }

RobotApp/Interfaces/IPeripheral.cs

@@ -0,0 +1,14 @@
+namespace RobotApp.Interfaces;
+
+public enum OperatingMode
+{
+    AUTOMATIC,
+    MANUAL,
+    SEMIAUTOMATIC,
+    TEACHIN,
+    SERVICE,
+}
+public interface IPeripheral
+{
+    OperatingMode OperatingMode { get; }
+}

RobotApp/Interfaces/IRFHandler.cs

@@ -0,0 +1,6 @@
+namespace RobotApp.Interfaces
+{
+    public class IRFHandler
+    {
+    }
+}

RobotApp/Interfaces/ISafety.cs

@@ -0,0 +1,5 @@
+namespace RobotApp.Interfaces;
+
+public interface ISafety
+{
+}

RobotApp/Modbus/ModbusException.cs

@@ -0,0 +1,16 @@
+namespace RobotApp.Modbus;
+
+public class ModbusException : Exception
+{
+    public ModbusException()
+    {
+    }
+    public ModbusException(string message)
+        : base(message)
+    {
+    }
+    public ModbusException(string message, Exception inner)
+        : base(message, inner)
+    {
+    }
+}

RobotApp/Modbus/ModbusTcpClient.cs

@@ -0,0 +1,350 @@
+using Microsoft.AspNetCore.Connections;
+using System.Net;
+using System.Net.Sockets;
+
+namespace RobotApp.Modbus;
+
+public class ModbusTcpClient(string IpAddress, int Port, byte ClientId) : IDisposable
+{
+    public bool IsConnected => !disposed && tcpClient != null && tcpClient.Client.Connected && stream != null;
+
+    private TcpClient? tcpClient;
+    private NetworkStream? stream;
+    private bool disposed = false;
+
+    private uint transactionIdentifierInternal = 0;
+    private const int connectTimeout = 3000;
+    private const int readTimeout = 500;
+    private const int writeTimeout = 500;
+    private int numberOfRetries { get; set; } = 3;
+
+    ~ModbusTcpClient()
+    {
+        Dispose(false);
+    }
+
+    public static bool TryConnect(string ipAddress, int port, byte clientId, out ModbusTcpClient? client)
+    {
+        ModbusTcpClient modbusTcpClient = new(ipAddress, port, clientId);
+        try
+        {
+            if (modbusTcpClient.Connect())
+            {
+                client = modbusTcpClient;
+                return true;
+            }
+            else
+            {
+                modbusTcpClient?.Dispose();
+                client = null;
+                return false;
+            }
+        }
+        catch
+        {
+            modbusTcpClient.Dispose();
+            client = null;
+            return false;
+        }
+
+    }
+
+    public static bool TryConnect(string ipAddress, out ModbusTcpClient? client) => TryConnect(ipAddress, 502, 1, out client);
+
+    public bool Connect()
+    {
+        try
+        {
+            tcpClient = new TcpClient();
+            var result = tcpClient.BeginConnect(IpAddress, Port, null, null);
+            if (!result.AsyncWaitHandle.WaitOne(connectTimeout))
+            {
+                tcpClient?.Close();
+                tcpClient?.Dispose();
+                tcpClient = null;
+                return false;
+            }
+            tcpClient.EndConnect(result);
+
+            stream = tcpClient.GetStream();
+            stream.ReadTimeout = readTimeout;
+            return true;
+        }
+        catch (Exception ex)
+        {
+            Dispose();
+            throw new ModbusException("connection error", ex);
+        }
+    }
+
+    public void Dispose()
+    {
+        Dispose(true);
+        GC.SuppressFinalize(this);
+    }
+
+    protected virtual void Dispose(bool disposing)
+    {
+        if (!disposed)
+        {
+            if (disposing)
+            {
+                stream?.Close();
+                stream?.Dispose();
+                tcpClient?.Close();
+                tcpClient?.Dispose();
+            }
+            stream = null;
+            tcpClient = null;
+            disposed = true;
+        }
+    }
+
+    private byte[] Write(byte functionCode, UInt16 startingAddress, UInt16 quantity, CancellationToken? cancellationToken = null)
+        => Write(functionCode, startingAddress, quantity, [], cancellationToken);
+    private byte[] Write(byte functionCode, UInt16 startingAddress, UInt16 quantity, byte[] multipleData, CancellationToken? cancellationToken = null)
+    {
+        try
+        {
+            if (!IsConnected || stream is null)
+            {
+                if (!Connect() || !IsConnected || stream is null) throw new ConnectionAbortedException();
+            }
+
+            transactionIdentifierInternal++;
+            byte[] writeData = new byte[12 + multipleData.Length];
+            var dataLength = multipleData.Length + 6;
+            writeData[0] = (byte)(transactionIdentifierInternal >> 8);
+            writeData[1] = (byte)(transactionIdentifierInternal & 0xFF);
+            writeData[2] = 0x00;
+            writeData[3] = 0x00;
+            writeData[4] = (byte)(dataLength >> 8);
+            writeData[5] = (byte)(dataLength & 0xFF);
+            writeData[6] = ClientId;
+            writeData[7] = functionCode;
+            writeData[8] = (byte)(startingAddress >> 8);
+            writeData[9] = (byte)(startingAddress & 0xFF);
+            writeData[10] = (byte)(quantity >> 8);
+            writeData[11] = (byte)(quantity & 0xFF);
+            if (multipleData.Length > 0)
+            {
+                Array.Copy(multipleData, 0, writeData, 12, multipleData.Length);
+            }
+
+            stream.Write(writeData, 0, writeData.Length);
+
+            byte[] readData = new byte[256];
+            int NumberOfBytes = stream.Read(readData, 0, readData.Length);
+            int attempts = 0;
+            const int maxAttempts = writeTimeout / 10;
+            while (NumberOfBytes == 0 && attempts ++ < maxAttempts)
+            {
+                cancellationToken?.ThrowIfCancellationRequested();
+                NumberOfBytes = stream.Read(readData, 0, readData.Length);
+                if (NumberOfBytes == 0) Thread.Sleep(10);
+            }
+            if (NumberOfBytes == 0) throw new TimeoutException("No response from server");
+            if (writeData[0] != readData[0] || writeData[1] != readData[1]) throw new ModbusException("Transaction Identifier not match");
+            if (writeData[2] != readData[2] || writeData[3] != readData[3]) throw new ModbusException("Protocol Identifier not match");
+            if (writeData[6] != readData[6]) throw new ModbusException("Client ID not match");
+            if (writeData[7] + 0x80 == readData[7])
+            {
+                throw readData[8] switch
+                {
+                    0x01 => new ModbusException("Function code not supported by master"),
+                    0x02 => new ModbusException("Starting address invalid or starting address + quantity invalid"),
+                    0x03 => new ModbusException("quantity invalid"),
+                    0x04 => new ModbusException("error reading"),
+                    _ => new ModbusException($"Function code error: 0x{(int)readData[7]:X2}"),
+                };
+            }
+            else if (writeData[7] != readData[7])
+            {
+                throw new Exception("Function code not match");
+            }
+            dataLength = readData[4] << 8;
+            dataLength += readData[5];
+            if (dataLength != NumberOfBytes - 6) throw new Exception("Length Field not match");
+
+            var receiveData = new byte[NumberOfBytes - 9];
+            Array.Copy(readData, 9, receiveData, 0, receiveData.Length);
+            return receiveData;
+        }
+        catch (Exception ex) when (!(ex is ModbusException || ex is TimeoutException))
+        {
+            throw new ModbusException("Communication error", ex);
+        }
+    }
+
+    public bool[] ReadDiscreteInputs(UInt16 startingAddress, UInt16 quantity, CancellationToken? cancellationToken = null)
+    {
+        if (startingAddress > 65535 | quantity > 2000)
+        {
+            throw new ArgumentException("Starting address must be 0 - 65535; quantity must be 0 - 2000");
+        }
+        var data = Write(0x02, startingAddress, quantity, cancellationToken);
+        if (data.Length - 1 < ((quantity - 1) / 8)) return [];
+        bool[] response = new bool[quantity];
+        for (int i = 0; i < quantity; i++)
+        {
+            int intData = data[i / 8];
+            int mask = Convert.ToInt32(Math.Pow(2, (i % 8)));
+            response[i] = Convert.ToBoolean((intData & mask) / mask);
+        }
+        return response;
+    }
+
+    public bool[] ReadCoils(UInt16 startingAddress, UInt16 quantity, CancellationToken? cancellationToken = null)
+    {
+        if (startingAddress > 65535 | quantity > 2000)
+        {
+            throw new ArgumentException("Starting address must be 0 - 65535; quantity must be 0 - 2000");
+        }
+        var data = Write(0x01, startingAddress, quantity, cancellationToken);
+        if (data.Length - 1 < ((quantity - 1) / 8)) return [];
+        bool[] response = new bool[quantity];
+        for (int i = 0; i < quantity; i++)
+        {
+            int intData = data[i / 8];
+            int mask = Convert.ToInt32(Math.Pow(2, (i % 8)));
+            response[i] = Convert.ToBoolean((intData & mask) / mask);
+        }
+        return (response);
+    }
+
+    public int[] ReadHoldingRegisters(UInt16 startingAddress, UInt16 quantity, CancellationToken? cancellationToken = null)
+    {
+        if (startingAddress > 65535 | quantity > 125)
+        {
+            throw new ArgumentException("Starting address must be 0 - 65535; quantity must be 0 - 125");
+        }
+        var data = Write(0x03, startingAddress, quantity, cancellationToken);
+        if (data.Length < quantity * 2) return [];
+        int[] response = new int[quantity];
+        for (int i = 0; i < quantity; i++)
+        {
+            response[i] = (data[i * 2] << 8) | data[i * 2 + 1];
+        }
+        return (response);
+    }
+
+    public int[] ReadInputRegisters(UInt16 startingAddress, UInt16 quantity, CancellationToken? cancellationToken = null)
+    {
+        if (startingAddress > 65535 | quantity > 125)
+        {
+            throw new ArgumentException("Starting address must be 0 - 65535; quantity must be 0 - 125");
+        }
+        var data = Write(0x04, startingAddress, quantity, cancellationToken);
+        if (data.Length < quantity * 2) return [];
+        int[] response = new int[quantity];
+        for (int i = 0; i < quantity; i++)
+        {
+            response[i] = (data[i * 2] << 8) | data[i * 2 + 1];
+        }
+        return (response);
+    }
+
+    public void WriteSingleCoil(UInt16 startingAddress, bool value, CancellationToken? cancellationToken = null)
+    {
+        Write(0x05, startingAddress, value ? (UInt16)0xFF00 : (UInt16)0x0000, cancellationToken);
+    }
+
+    public void WriteSingleRegister(UInt16 startingAddress, UInt16 value, CancellationToken? cancellationToken = null)
+    {
+        Write(0x06, startingAddress, value, cancellationToken);
+    }
+
+    public void WriteMultipleCoils(UInt16 startingAddress, bool[] values, CancellationToken? cancellationToken = null)
+    {
+        if (values == null || values.Length == 0)
+            throw new ArgumentException("Values cannot be null or empty", nameof(values));
+        if (values.Length > 1968)
+            throw new ArgumentException("Too many coils (max 1968)", nameof(values));
+        if (startingAddress > 65535)
+            throw new ArgumentException("Starting address must be 0-65535", nameof(startingAddress));
+        if (startingAddress + values.Length > 65536)
+            throw new ArgumentException("Address range exceeds 65535", nameof(startingAddress));
+
+        byte byteCount = (byte)((values.Length + 7) / 8);
+        var data = new byte[1 + byteCount];
+        data[0] = byteCount;
+        for (int i = 0; i < values.Length; i++)
+        {
+            if (values[i])
+            {
+                data[1 + (i / 8)] |= (byte)(1 << (i % 8));
+            }
+        }
+        Write(0x0F, startingAddress, (UInt16)values.Length, data, cancellationToken);
+    }
+
+    public void WriteMultipleRegisters(UInt16 startingAddress, UInt16[] values, CancellationToken? cancellationToken = null)
+    {
+        if (values == null || values.Length == 0)
+            throw new ArgumentException("Values cannot be null or empty", nameof(values));
+        if (values.Length > 123)
+            throw new ArgumentException("Too many registers (max 123)", nameof(values));
+        if (startingAddress > 65535)
+            throw new ArgumentException("Starting address must be 0-65535", nameof(startingAddress));
+        if (startingAddress + values.Length > 65536)
+            throw new ArgumentException("Address range exceeds 65535", nameof(startingAddress));
+
+        byte byteCount = (byte)(values.Length * 2);
+        var data = new byte[1 + byteCount];
+        data[0] = byteCount;
+        for (int i = 0; i < values.Length; i++)
+        {
+            data[1 + i * 2] = (byte)(values[i] >> 8);      
+            data[2 + i * 2] = (byte)(values[i] & 0xFF); 
+        }
+        Write(0x10, startingAddress, (UInt16)values.Length, data, cancellationToken);
+    }
+
+    public int[] ReadWriteMultipleRegisters(UInt16 startingAddressRead, UInt16 quantityRead, UInt16 startingAddressWrite, UInt16[] values, CancellationToken? cancellationToken = null)
+    {
+        if (values == null || values.Length == 0)
+            throw new ArgumentException("Values cannot be null or empty", nameof(values));
+        if (quantityRead == 0 || quantityRead > 125)
+            throw new ArgumentException("Read quantity must be 1-125", nameof(quantityRead));
+        if (values.Length > 121)
+            throw new ArgumentException("Write quantity must be 1-121", nameof(values));
+        if (startingAddressRead > 65535 || startingAddressWrite > 65535)
+            throw new ArgumentException("Addresses must be 0-65535");
+        if (startingAddressRead + quantityRead > 65536 || startingAddressWrite + values.Length > 65536)
+            throw new ArgumentException("Address ranges exceed 65535");
+
+        var writeData = new byte[5 + values.Length * 2];
+        writeData[0] = (byte)(startingAddressWrite >> 8);
+        writeData[1] = (byte)(startingAddressWrite & 0xFF);
+        writeData[2] = (byte)(values.Length >> 8);
+        writeData[3] = (byte)(values.Length & 0xFF);
+        writeData[4] = (byte)(values.Length * 2);
+        for (int i = 0; i < values.Length; i++)
+        {
+            writeData[5 + i * 2] = (byte)(values[i] >> 8);
+            writeData[6 + i * 2] = (byte)(values[i] & 0xFF);
+        }
+        var receivedData = Write(0x17, startingAddressRead, quantityRead, writeData, cancellationToken);
+        if (receivedData.Length < quantityRead * 2) return [];
+        var response = new int[quantityRead];
+        for (int i = 0; i < quantityRead; i++)
+        {
+            response[i] = (receivedData[i * 2] << 8) | receivedData[i * 2 + 1];
+        }
+        return response;
+    }
+
+    public bool Available(int timeout)
+    {
+        System.Net.NetworkInformation.Ping pingSender = new ();
+        IPAddress address = System.Net.IPAddress.Parse(IpAddress);
+
+        string data = "phenikaaX";
+        byte[] buffer = System.Text.Encoding.ASCII.GetBytes(data);
+
+        System.Net.NetworkInformation.PingReply reply = pingSender.Send(address, timeout, buffer);
+
+        return reply.Status == System.Net.NetworkInformation.IPStatus.Success;
+    }
+
+}

RobotApp/Modbus/RegisterOrder.cs

@@ -0,0 +1,8 @@
+namespace RobotApp.Modbus;
+
+
+public enum RegisterOrder 
+{
+    LowHigh = 0,
+    HighLow = 1 
+}

RobotApp/RobotApp.csproj

@@ -24,6 +24,7 @@
   </ItemGroup>
 
   <ItemGroup>
+    <Folder Include="Services\Robot\Simulation\" />
     <Folder Include="wwwroot\lib\" />
   </ItemGroup>
 

RobotApp/Services/Exceptions/ActionException.cs

@@ -0,0 +1,5 @@
+namespace RobotApp.Services.Exceptions;
+
+public class ActionException : OrderException
+{
+}

RobotApp/Services/Exceptions/OrderException.cs

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

RobotApp/Services/MQTTClient.cs

@@ -27,7 +27,6 @@ public class MQTTClient : IAsyncDisposable
         Logger = logger;
 
         MqttClientFactory = new MqttClientFactory();
-        MqttClient = MqttClientFactory.CreateMqttClient();
         MqttClientOptions = MqttClientFactory.CreateClientOptionsBuilder()
                             .WithTcpServer(setting.HostServer, setting.Port)
                             .WithCredentials(setting.UserName, setting.Password)
@@ -38,6 +37,11 @@ public class MQTTClient : IAsyncDisposable
                             .WithTopicFilter(f => f.WithTopic(VDA5050Topic.ORDER.ToTopicString()))
                             .WithTopicFilter(f => f.WithTopic(VDA5050Topic.INSTANTACTIONS.ToTopicString()))
                             .Build();
+    }
+
+    public async Task ConnectAsync(CancellationToken cancellationToken = default)
+    {
+        MqttClient = MqttClientFactory.CreateMqttClient();
         MqttClient.DisconnectedAsync += async delegate (MqttClientDisconnectedEventArgs args)
         {
             if (args.ClientWasConnected && !IsReconnecing)
@@ -52,15 +56,10 @@ public class MQTTClient : IAsyncDisposable
                 IsReconnecing = false;
             }
         };
-    }
-
-    public async Task ConnectAsync(CancellationToken cancellationToken = default)
-    {
         while (!cancellationToken.IsCancellationRequested)
         {
             try
             {
-                MqttClient ??= MqttClientFactory.CreateMqttClient();
                 var connection = await MqttClient.ConnectAsync(MqttClientOptions, cancellationToken);
                 if (connection.ResultCode != MqttClientConnectResultCode.Success || !MqttClient.IsConnected)
                     Logger.Warning($"Không thể kết nối tới broker do: {connection.ReasonString}");

RobotApp/Services/Navigation/NavigationController.cs

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

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

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

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

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

RobotApp/Services/Robot/Navigation/DifferentialNavigation.cs

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

RobotApp/Services/Robot/Navigation/NavigationController.cs

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

RobotApp/Services/Robot/Navigation/NavigationManager.cs

@@ -1,4 +1,4 @@
-namespace RobotApp.Services.Navigation
+namespace RobotApp.Services.Robot.Navigation
 {
     public class NavigationManager
     {

RobotApp/Services/Robot/Navigation/NavigationNode.cs

@@ -0,0 +1,13 @@
+using RobotApp.Common.Shares.Enums;
+
+namespace RobotApp.Services.Robot.Navigation;
+
+public class NavigationNode
+{
+    public string NodeId { get; set; } = string.Empty;
+    public double X { get; set; }
+    public double Y { get; set; }
+    public double Theta { get; set; }
+    public RobotDirection Direction { get; set; }
+    public string Description { get; set; } = string.Empty;
+}

RobotApp/Services/Robot/RobotAction.cs

@@ -1,6 +1,31 @@
-namespace RobotApp.Services.Robot
+using RobotApp.Interfaces;
+using RobotApp.VDA5050.State;
+
+namespace RobotApp.Services.Robot;
+
+public class RobotAction : IInstanceActions
 {
-    public class RobotAction
+    public ActionState[] ActionStates { get; private set; } = [];
+
+    public bool HasActionRunning => throw new NotImplementedException();
+
+    public bool AddInstanceAction(VDA5050.InstantAction.Action action)
     {
+        throw new NotImplementedException();
+    }
+
+    public bool AddOrderActions(VDA5050.InstantAction.Action[] actions)
+    {
+        throw new NotImplementedException();
+    }
+
+    public bool StartAction(string actionId)
+    {
+        throw new NotImplementedException();
+    }
+
+    public bool StopAction()
+    {
+        throw new NotImplementedException();
     }
 }

RobotApp/Services/Robot/RobotBattery.cs

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

RobotApp/Services/Robot/RobotController.cs

@@ -1,6 +1,96 @@
-namespace RobotApp.Services.Robot
+using RobotApp.Interfaces;
+using RobotApp.Services.Exceptions;
+using RobotApp.VDA5050.Order;
+using RobotApp.VDA5050.State;
+
+namespace RobotApp.Services.Robot;
+
+public class RobotController(IOrder OrderManager,
+                             INavigation NavigationManager,
+                             IInstanceActions ActionManager,
+                             IBattery BatteryManager,
+                             ILocalization Localization,
+                             IPeripheral PeripheralManager,
+                             ISafety SafetyManager,
+                             IError ErrorManager,
+                             IInfomation InfomationManager,
+                             Logger<RobotController> Logger,
+                             RobotConnection ConnectionManager) : BackgroundService
 {
-    public class RobotController
+    private readonly Mutex NewOrderMutex = new();
+    private readonly Mutex NewInstanceMutex = new();
+
+    private WatchTimer<RobotController>? UpdateStateTimer;
+    private const int UpdateStateInterval = 1000;
+
+    protected override Task ExecuteAsync(CancellationToken stoppingToken)
     {
+        UpdateStateTimer = new(UpdateStateInterval, UpdateStateHandler, Logger);
+        UpdateStateTimer.Start();
+        return Task.CompletedTask;
+    }
+
+    private void UpdateStateHandler()
+    {
+        // xử lý cập nhật trạng thái robot và gửi thông tin qua kết nối
+    }
+
+    public void NewOrderUpdated(OrderMsg order)
+    {
+        if (NewOrderMutex.WaitOne(2000))
+        {
+            try
+            {
+                if (!string.IsNullOrEmpty(OrderManager.OrderId))
+                {
+                    if (order.OrderId != OrderManager.OrderId) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1001", ErrorLevel.WARNING, $"Có order đang được thực hiện. OrderId: {OrderManager.OrderId}, OrderId mới: {order.OrderId}"));
+                    OrderManager.UpdateOrder(order.OrderUpdateId, order.Nodes, order.Edges);
+                }
+                else if (PeripheralManager.OperatingMode != Interfaces.OperatingMode.AUTOMATIC) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1006", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi chế độ vận hành không phải là TỰ ĐỘNG. Chế độ hiện tại: {PeripheralManager.OperatingMode}"));
+                else if(ActionManager.HasActionRunning) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1007", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi có action đang thực hiện. Vui lòng chờ hoàn thành các action hiện tại."));
+                else if(ErrorManager.HasFatalError) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1008", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi có lỗi nghiêm trọng. Vui lòng kiểm tra và xử lý lỗi."));
+                else if(NavigationManager.Driving) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1009", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi robot đang di chuyển. Vui lòng dừng robot."));
+                else OrderManager.StartOrder(order.OrderId, order.Nodes, order.Edges);
+            }
+            catch (OrderException orEx)
+            {
+                if (orEx.Error is not null)
+                {
+                    ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10));
+                    Logger.Warning($"Lỗi khi xử lí Order: {orEx.Error.ErrorDescription}");
+                }
+            }
+            catch (Exception ex)
+            {
+                Logger.Warning($"Lỗi khi xử lí Order: {ex.Message}");
+            }
+            finally
+            {
+                NewOrderMutex.ReleaseMutex();
+            }
+        }
+    }
+
+    public void NewInstanceActionUpdated()
+    {
+        if (NewInstanceMutex.WaitOne(2000))
+        {
+            try
+            {
+
+            }
+            catch (ActionException acEx)
+            {
+
+            }
+            catch (Exception ex)
+            {
+
+            }
+            finally
+            {
+                NewInstanceMutex.ReleaseMutex();
+            }
+        }
     }
 }

RobotApp/Services/Robot/RobotErrors.cs

@@ -0,0 +1,41 @@
+using RobotApp.VDA5050.State;
+using RobotApp.VDA5050.Type;
+
+namespace RobotApp.Services.Robot;
+
+public class RobotErrors
+{
+    private readonly List<Error> Errors = [];
+
+    public void AddError(Error error, TimeSpan? clearAfter = null)
+    {
+        lock (Errors)
+        {
+            Errors.Add(error);
+        }
+        if (clearAfter is not null && clearAfter.HasValue)
+        {
+            if (clearAfter.Value < TimeSpan.Zero) throw new ArgumentOutOfRangeException(nameof(clearAfter), "TimeSpan cannot be negative.");
+            _ = Task.Run(async () =>
+            {
+                await Task.Delay(clearAfter.Value);
+                lock (Errors)
+                {
+                    Errors.RemoveAll(e => e.ErrorType == error.ErrorType);
+                }
+            });
+        }
+    }
+
+    public static Error CreateError(ErrorType type, string hint, ErrorLevel level, string description)
+    {
+        return new Error()
+        {
+            ErrorType = type.ToString(),
+            ErrorLevel = level.ToString(),
+            ErrorDescription = description,
+            ErrorHint = hint,
+            ErrorReferences = []
+        };
+    }
+}

RobotApp/Services/Robot/RobotFactsheet.cs

@@ -29,6 +29,6 @@ public class RobotFactsheet(RobotConnection RobotConnection) : BackgroundService
             await Task.Delay(1000, stoppingToken);
             if (RobotConnection.IsConnected) break;
         }
-
+        await PubFactsheet();
     }
 }

RobotApp/Services/Robot/RobotInfomations.cs

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

RobotApp/Services/Robot/RobotLoads.cs

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

RobotApp/Services/Robot/RobotOrderController.cs

@@ -1,30 +1,182 @@
 using RobotApp.Interfaces;
+using RobotApp.Services.Exceptions;
+using RobotApp.VDA5050.Order;
 using RobotApp.VDA5050.State;
+using Action = RobotApp.VDA5050.InstantAction.Action;
 
 namespace RobotApp.Services.Robot;
 
-public class RobotOrderController : IOrder
+public class RobotOrderController(INavigation NavigationManager, IInstanceActions ActionManager, IError ErrorManager, Logger<RobotOrderController> Logger) : IOrder
 {
-    public string OrderId => throw new NotImplementedException();
+    public string OrderId { get; private set; } = string.Empty;
+    public int OrderUpdateId { get; private set; }
+    public NodeState[] NodeStates { get; private set; } = [];
+    public EdgeState[] EdgeStates { get; private set; } = [];
+    public string LastNodeId { get; private set; } = string.Empty;
+    public int LastNodeSequenceId { get; private set; }
 
-    public VDA5050.InstantAction.Action[] Actions => throw new NotImplementedException();
+    private readonly Dictionary<string, Action[]> OrderActions = [];
+    private readonly Mutex OrderMutex = new();
 
-    public NodeState[] NodeStates => throw new NotImplementedException();
+    protected const int CycleHandlerMilliseconds = 100;
+    private WatchTimer<RobotOrderController>? OrderTimer;
 
-    public EdgeState[] EdgeStates => throw new NotImplementedException();
-
-    public bool StartOrder()
+    private int BaseSequenceId = 0;
+    public void StartOrder(string orderId, Node[] nodes, Edge[] edges)
     {
-        throw new NotImplementedException();
+        if (OrderMutex.WaitOne(2000))
+        {
+            try
+            {
+                if (!string.IsNullOrEmpty(OrderId) && orderId != OrderId) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1001", ErrorLevel.WARNING, $"Có order đang được thực hiện. OrderId: {OrderId}, OrderId mới: {orderId}"));
+                if (nodes.Length < 2) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1002", ErrorLevel.WARNING, $"Order Nodes không hợp lệ. Kích thước: {nodes.Length}"));
+                if (edges.Length < 1) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1003", ErrorLevel.WARNING, $"Order Edges không hợp lệ. Kích thước: {edges.Length}"));
+                if (edges.Length != nodes.Length - 1) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1004", ErrorLevel.WARNING, $"Order không hợp lệ do kích thước giữa Nodes và Edges không phù hợp. Kích thước Edges: {edges.Length}, kích thước nodes: {nodes.Length}"));
+                if (NavigationManager.State != NavigationState.Idle) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1012", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi hệ thống điều hướng không ở trạng thái sẵn sàng. Trạng thái hiện tại: {NavigationManager.State}"));
+
+                for (int i = 0; i < nodes.Length; i++)
+                {
+                    if (nodes[i].Actions is not null && nodes[i].Actions.Length > 0)
+                    {
+                        foreach (var item in nodes[i].Actions)
+                        {
+                            item.ActionDescription += $"\n NodeId: {nodes[i].NodeId}";
+                        }
+                        OrderActions.Add(nodes[i].NodeId, nodes[i].Actions);
+                    }
+                    if (i < nodes.Length - 1 && edges[i] is not null && edges[i].Length > 0)
+                    {
+                        foreach (var item in edges[i].Actions)
+                        {
+                            item.ActionDescription += $"\n NodeId: {nodes[i].NodeId}";
+                        }
+                        OrderActions.TryAdd(nodes[i].NodeId, edges[i].Actions);
+                    }
+                    if (nodes[i].SequenceId != i) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1010", ErrorLevel.WARNING, $"Order Nodes không đúng thứ tự. NodeId: {nodes[i].NodeId}, SequenceId: {nodes[i].SequenceId}, Vị trí đúng: {i}"));
+                    if (i < nodes.Length - 1 &&  edges[i].SequenceId != i) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1011", ErrorLevel.WARNING, $"Order Edges không đúng thứ tự. EdgeId: {edges[i].EdgeId}, SequenceId: {edges[i].SequenceId}, Vị trí đúng: {i}"));
+                    if (nodes[i].Released) BaseSequenceId = nodes[i].SequenceId;
                 }
 
-    public bool UpdateOrder()
+                NodeStates = nodes.Select(n => new NodeState
                 {
-        throw new NotImplementedException();
+                    NodeId = n.NodeId,
+                    Released = n.Released,
+                    SequenceId = n.SequenceId,
+                    NodeDescription = n.NodeDescription,
+                    NodePosition = new()
+                    {
+                        X = n.NodePosition.X,
+                        Y = n.NodePosition.Y,
+                        Theta = n.NodePosition.Theta,
+                        MapId = n.NodePosition.MapId
+                    }
+                }).ToArray();
+                EdgeStates = edges.Select(e => new EdgeState
+                {
+                    EdgeId = e.EdgeId,
+                    Released = e.Released,
+                    EdgeDescription = e.EdgeDescription,
+                    SequenceId = e.SequenceId,
+                    Trajectory = e.Trajectory
+                }).ToArray();
+
+                OrderId = orderId;
+                ActionManager.AddOrderActions([.. OrderActions.Values.SelectMany(a => a)]);
+                NavigationManager.Move(nodes, edges);
+                HandleNavigationStart();
+            }
+            catch (OrderException orEx)
+            {
+                if (orEx.Error is not null)
+                {
+                    ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10));
+                    Logger.Warning($"Lỗi khi khởi tạo Order: {orEx.Error.ErrorDescription}");
+                }
+            }
+            catch (Exception ex)
+            {
+                Logger.Warning($"Lỗi khi khởi tạo Order: {ex.Message}");
+            }
+            finally
+            {
+                OrderMutex.ReleaseMutex();
+            }
+        }
+        Logger.Warning($"Lỗi khi khởi tạo Order: có order đang được khởi tạo chưa xong");
     }
 
-    public bool StopOrder()
+    public void UpdateOrder(int orderUpdateId,Node[] nodes, Edge[] edges)
     {
-        throw new NotImplementedException();
+        if (OrderMutex.WaitOne(2000))
+        {
+            try
+            {
+                if (string.IsNullOrEmpty(OrderId)) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1005", ErrorLevel.WARNING, $"Không có order đang được thực hiện."));
+                if (orderUpdateId > OrderUpdateId)
+                {
+                    OrderUpdateId = orderUpdateId;
+                    // Check Order Update hợp lệ
+                    // Check Order update Hoziron hay không
+                }
+            }
+            catch (OrderException orEx)
+            {
+                if (orEx.Error is not null)
+                {
+                    ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10));
+                    Logger.Warning($"Lỗi khi cập nhật Order: {orEx.Error.ErrorDescription}");
+                }
+            }
+            catch (Exception ex)
+            {
+                Logger.Warning($"Lỗi khi cập nhật Order: {ex.Message}");
+            }
+            finally
+            {
+                OrderMutex.ReleaseMutex();
+            }
+        }
+        Logger.Warning($"Lỗi khi cập nhật Order: có order đang được cập nhật chưa xong");
+    }
+
+    public void StopOrder()
+    {
+        HandleNavigationStop();
+        NavigationManager.CancelMovement();
+
+        OrderId = string.Empty;
+        OrderActions.Clear();
+        OrderUpdateId = 0;
+        BaseSequenceId = 0;
+        LastNodeSequenceId = 0;
+        NodeStates = [];
+        EdgeStates = [];
+    }
+
+    private void HandleNavigationStart()
+    {
+        OrderTimer = new(CycleHandlerMilliseconds, OrderHandler, Logger);
+        OrderTimer.Start();
+    }
+
+    private void HandleNavigationStop()
+    {
+        OrderTimer?.Dispose();
+    }
+
+    private void OrderHandler()
+    {
+        if(!string.IsNullOrEmpty(OrderId) && BaseSequenceId > 0)
+        {
+            if(NavigationManager.State == NavigationState.Initializing)
+            {
+                // khởi tạo Order
+                
+            }
+            else
+            {
+                // xử lí khi có Order
+            }
+        }
     }
 }

RobotApp/Services/Robot/RobotPathPlanner.cs

@@ -0,0 +1,88 @@
+using RobotApp.Common.Shares;
+using RobotApp.Common.Shares.Enums;
+using RobotApp.Common.Shares.Model;
+using RobotApp.Services.Exceptions;
+using RobotApp.Services.Robot.Navigation;
+using RobotApp.VDA5050.Order;
+using RobotApp.VDA5050.State;
+
+namespace RobotApp.Services.Robot;
+
+public class RobotPathPlanner(IConfiguration Configuration)
+{
+    private readonly double ResolutionSplit = Configuration.GetValue("PathPlanning:ResolutionSplit", 0.1);
+
+    public NavigationNode[] GetNavigationNode(double currentTheta, Node[] nodes, Edge[] edges)
+    {
+        if (nodes.Length < 2) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1002", ErrorLevel.WARNING, $"Order Nodes không hợp lệ. Kích thước: {nodes.Length}"));
+        if (edges.Length < 1) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1003", ErrorLevel.WARNING, $"Order Edges không hợp lệ. Kích thước: {edges.Length}"));
+        if (edges.Length != nodes.Length - 1) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1004", ErrorLevel.WARNING, $"Order không hợp lệ do kích thước giữa Nodes và Edges không phù hợp. Kích thước Edges: {edges.Length}, kích thước nodes: {nodes.Length}"));
+
+        return [];
+    }
+
+    public NavigationNode[] PathSplit(NavigationNode[] nodes, Edge[] edges)
+    {
+        if (nodes.Length < 2) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1002", ErrorLevel.WARNING, $"Order Nodes không hợp lệ. Kích thước: {nodes.Length}"));
+        if (edges.Length < 1) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1003", ErrorLevel.WARNING, $"Order Edges không hợp lệ. Kích thước: {edges.Length}"));
+        if (edges.Length != nodes.Length - 1) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1004", ErrorLevel.WARNING, $"Order không hợp lệ do kích thước giữa Nodes và Edges không phù hợp. Kích thước Edges: {edges.Length}, kích thước nodes: {nodes.Length}"));
+
+        List<NavigationNode> navigationNode = [new()
+        {
+            NodeId = nodes[0].NodeId,
+            X = nodes[0].X,
+            Y = nodes[0].Y,
+            Theta = nodes[0].Theta,
+            Direction = nodes[0].Direction,
+            Description = nodes[0].Description
+        }];
+        foreach (var edge in edges)
+        {
+            var startNode = nodes.FirstOrDefault(n => n.NodeId == edge.StartNodeId);
+            var endNode = nodes.FirstOrDefault(n => n.NodeId == edge.EndNodeId);
+            if (startNode is null) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1014", ErrorLevel.WARNING, $"Edge chứa StartNodeId không tồn tại trong Nodes . StartNodeId: {edge.StartNodeId}"));
+            if (endNode is null) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1015", ErrorLevel.WARNING, $"Edge chứa EndNodeId không tồn tại trong Nodes . EndNodeId: {edge.EndNodeId}"));
+
+            var EdgeCalculatorModel = new EdgeCalculatorModel()
+            {
+                X1 = startNode.X,
+                Y1 = startNode.Y,
+                X2 = endNode.X,
+                Y2 = endNode.Y,
+                ControlPoint1X = edge.Trajectory.ControlPoints.Length > 0 ? edge.Trajectory.ControlPoints[0].X : 0,
+                ControlPoint1Y = edge.Trajectory.ControlPoints.Length > 0 ? edge.Trajectory.ControlPoints[0].Y : 0,
+                ControlPoint2X = edge.Trajectory.ControlPoints.Length > 1 ? edge.Trajectory.ControlPoints[1].X : 0,
+                ControlPoint2Y = edge.Trajectory.ControlPoints.Length > 1 ? edge.Trajectory.ControlPoints[1].Y : 0,
+                TrajectoryDegree = edge.Trajectory.Degree == 1 ? TrajectoryDegree.One : edge.Trajectory.Degree == 2 ? TrajectoryDegree.Two :TrajectoryDegree.Three
+            };
+
+            double length = EdgeCalculatorModel.GetEdgeLength();
+            if (length <= 0) continue;
+            double step = ResolutionSplit / length;
+
+            for (double t = step; t <= 1 - step; t += step)
+            {
+                (double x, double y) = EdgeCalculatorModel.Curve(t);
+                navigationNode.Add(new()
+                {
+                    NodeId = string.Empty,
+                    X = x,
+                    Y = y,
+                    Theta = startNode.Theta,
+                    Direction = startNode.Direction,
+                    Description = string.Empty,
+                });
+            }
+            navigationNode.Add(new()
+            {
+                NodeId = endNode.NodeId,
+                X = endNode.X,
+                Y = endNode.Y,
+                Theta = endNode.Theta,
+                Direction = endNode.Direction,
+                Description = nodes[0].Description
+            });
+        }
+        return [..navigationNode];
+    }
+}

RobotApp/Services/Robot/RobotSafety.cs

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

RobotApp/Services/Robot/RobotStates.cs

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

RobotApp/Services/State/RobotState.cs

@@ -0,0 +1,8 @@
+namespace RobotApp.Services.State;
+
+public abstract class RobotState<T>(T type) where T : Enum
+{
+    public readonly T Type = type;
+    public virtual void Enter() { }
+    public virtual void Exit() { }
+}

RobotApp/Services/WatchTimer.cs

@@ -19,6 +19,7 @@ public class WatchTimer<T>(int Interval, Action Callback, Logger<T>? Logger) : I
             Watch.Stop();
             if (Watch.ElapsedMilliseconds >= Interval || Interval - Watch.ElapsedMilliseconds <= 50)
             {
+                if(Watch.ElapsedMilliseconds > Interval) Logger?.Warning($"WatchTimer Warning: Elapsed time {Watch.ElapsedMilliseconds}ms exceeds interval {Interval}ms.");
                 Timer?.Change(Interval, Timeout.Infinite);
             }
             else