diff --git a/RobotApp.Common.Shares/Enums/RobotDirection.cs b/RobotApp.Common.Shares/Enums/RobotDirection.cs new file mode 100644 index 0000000..4d52fd9 --- /dev/null +++ b/RobotApp.Common.Shares/Enums/RobotDirection.cs @@ -0,0 +1,7 @@ +namespace RobotApp.Common.Shares.Enums; + +public enum RobotDirection +{ + FORWARD, + BACKWARD, +} diff --git a/RobotApp.Common.Shares/Enums/StateType.cs b/RobotApp.Common.Shares/Enums/StateType.cs new file mode 100644 index 0000000..c9b3b75 --- /dev/null +++ b/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, +} \ No newline at end of file diff --git a/RobotApp.Common.Shares/Enums/TrajectoryDegree.cs b/RobotApp.Common.Shares/Enums/TrajectoryDegree.cs new file mode 100644 index 0000000..658543a --- /dev/null +++ b/RobotApp.Common.Shares/Enums/TrajectoryDegree.cs @@ -0,0 +1,8 @@ +namespace RobotApp.Common.Shares.Enums; + +public enum TrajectoryDegree +{ + One, + Two, + Three, +} diff --git a/RobotApp.Common.Shares/MathExtensions.cs b/RobotApp.Common.Shares/MathExtensions.cs new file mode 100644 index 0000000..54d57fc --- /dev/null +++ b/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; + } +} diff --git a/RobotApp.Common.Shares/Model/EdgeCalculatorModel.cs b/RobotApp.Common.Shares/Model/EdgeCalculatorModel.cs new file mode 100644 index 0000000..001bf22 --- /dev/null +++ b/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; } +} diff --git a/RobotApp.VDA5050/State/ActionState.cs b/RobotApp.VDA5050/State/ActionState.cs index df853e7..420be95 100644 --- a/RobotApp.VDA5050/State/ActionState.cs +++ b/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; } diff --git a/RobotApp.VDA5050/State/EdgeState.cs b/RobotApp.VDA5050/State/EdgeState.cs index 9d02a0c..9382df6 100644 --- a/RobotApp.VDA5050/State/EdgeState.cs +++ b/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(); } diff --git a/RobotApp.VDA5050/State/Error.cs b/RobotApp.VDA5050/State/Error.cs index eabc52d..9fb6eb1 100644 --- a/RobotApp.VDA5050/State/Error.cs +++ b/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; } diff --git a/RobotApp.VDA5050/State/Information.cs b/RobotApp.VDA5050/State/Information.cs index 287f695..06aa480 100644 --- a/RobotApp.VDA5050/State/Information.cs +++ b/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; } diff --git a/RobotApp.VDA5050/State/Load.cs b/RobotApp.VDA5050/State/Load.cs index 4f32e25..08e2317 100644 --- a/RobotApp.VDA5050/State/Load.cs +++ b/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 double Weight { 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; } } diff --git a/RobotApp.VDA5050/State/NodeState.cs b/RobotApp.VDA5050/State/NodeState.cs index 7b884d6..58c670c 100644 --- a/RobotApp.VDA5050/State/NodeState.cs +++ b/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; } \ No newline at end of file diff --git a/RobotApp.VDA5050/State/StateMsg.cs b/RobotApp.VDA5050/State/StateMsg.cs index d06e68f..308fc1c 100644 --- a/RobotApp.VDA5050/State/StateMsg.cs +++ b/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] diff --git a/RobotApp.VDA5050/Visualization/AgvPosition.cs b/RobotApp.VDA5050/Visualization/AgvPosition.cs index a19e59e..07c5957 100644 --- a/RobotApp.VDA5050/Visualization/AgvPosition.cs +++ b/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] diff --git a/RobotApp.VDA5050/Visualization/Visualizationmsg.cs b/RobotApp.VDA5050/Visualization/Visualizationmsg.cs index 3bac23f..2001c8b 100644 --- a/RobotApp.VDA5050/Visualization/Visualizationmsg.cs +++ b/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(); } diff --git a/RobotApp/Interfaces/IBattery.cs b/RobotApp/Interfaces/IBattery.cs new file mode 100644 index 0000000..bcb2b29 --- /dev/null +++ b/RobotApp/Interfaces/IBattery.cs @@ -0,0 +1,5 @@ +namespace RobotApp.Interfaces; + +public interface IBattery +{ +} diff --git a/RobotApp/Interfaces/IError.cs b/RobotApp/Interfaces/IError.cs new file mode 100644 index 0000000..ba09ab1 --- /dev/null +++ b/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); +} diff --git a/RobotApp/Interfaces/IInfomation.cs b/RobotApp/Interfaces/IInfomation.cs new file mode 100644 index 0000000..bd79f4a --- /dev/null +++ b/RobotApp/Interfaces/IInfomation.cs @@ -0,0 +1,5 @@ +namespace RobotApp.Interfaces; + +public interface IInfomation +{ +} diff --git a/RobotApp/Interfaces/IInstanceActions.cs b/RobotApp/Interfaces/IInstanceActions.cs index 3fb168f..87f9825 100644 --- a/RobotApp/Interfaces/IInstanceActions.cs +++ b/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(); } diff --git a/RobotApp/Interfaces/INavigation.cs b/RobotApp/Interfaces/INavigation.cs index 76a73c9..3e09fd1 100644 --- a/RobotApp/Interfaces/INavigation.cs +++ b/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(); } diff --git a/RobotApp/Interfaces/IOrder.cs b/RobotApp/Interfaces/IOrder.cs index c2f27f4..fc6c16e 100644 --- a/RobotApp/Interfaces/IOrder.cs +++ b/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(); } diff --git a/RobotApp/Interfaces/IPeripheral.cs b/RobotApp/Interfaces/IPeripheral.cs new file mode 100644 index 0000000..d8bb49a --- /dev/null +++ b/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; } +} diff --git a/RobotApp/Interfaces/IRFHandler.cs b/RobotApp/Interfaces/IRFHandler.cs new file mode 100644 index 0000000..d5d6d7c --- /dev/null +++ b/RobotApp/Interfaces/IRFHandler.cs @@ -0,0 +1,6 @@ +namespace RobotApp.Interfaces +{ + public class IRFHandler + { + } +} diff --git a/RobotApp/Interfaces/ISafety.cs b/RobotApp/Interfaces/ISafety.cs new file mode 100644 index 0000000..b92ef96 --- /dev/null +++ b/RobotApp/Interfaces/ISafety.cs @@ -0,0 +1,5 @@ +namespace RobotApp.Interfaces; + +public interface ISafety +{ +} diff --git a/RobotApp/Modbus/ModbusException.cs b/RobotApp/Modbus/ModbusException.cs new file mode 100644 index 0000000..665b664 --- /dev/null +++ b/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) + { + } +} diff --git a/RobotApp/Modbus/ModbusTcpClient.cs b/RobotApp/Modbus/ModbusTcpClient.cs new file mode 100644 index 0000000..b101acf --- /dev/null +++ b/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; + } + +} \ No newline at end of file diff --git a/RobotApp/Modbus/RegisterOrder.cs b/RobotApp/Modbus/RegisterOrder.cs new file mode 100644 index 0000000..401fbd4 --- /dev/null +++ b/RobotApp/Modbus/RegisterOrder.cs @@ -0,0 +1,8 @@ +namespace RobotApp.Modbus; + + +public enum RegisterOrder +{ + LowHigh = 0, + HighLow = 1 +} diff --git a/RobotApp/RobotApp.csproj b/RobotApp/RobotApp.csproj index ebfb1b0..c1d3f80 100644 --- a/RobotApp/RobotApp.csproj +++ b/RobotApp/RobotApp.csproj @@ -24,6 +24,7 @@ + diff --git a/RobotApp/Services/Exceptions/ActionException.cs b/RobotApp/Services/Exceptions/ActionException.cs new file mode 100644 index 0000000..4348012 --- /dev/null +++ b/RobotApp/Services/Exceptions/ActionException.cs @@ -0,0 +1,5 @@ +namespace RobotApp.Services.Exceptions; + +public class ActionException : OrderException +{ +} diff --git a/RobotApp/Services/Exceptions/OrderException.cs b/RobotApp/Services/Exceptions/OrderException.cs new file mode 100644 index 0000000..aaf044f --- /dev/null +++ b/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; } +} diff --git a/RobotApp/Services/MQTTClient.cs b/RobotApp/Services/MQTTClient.cs index 6415deb..20aba0f 100644 --- a/RobotApp/Services/MQTTClient.cs +++ b/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,13 +37,18 @@ 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) { IsReconnecing = true; Logger.Warning("Mất kết nối tới broker, đang cố gắng kết nối lại..."); - if (MqttClient.IsConnected) await MqttClient.DisconnectAsync(); + if (MqttClient.IsConnected) await MqttClient.DisconnectAsync(); MqttClient.Dispose(); await ConnectAsync(); @@ -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}"); diff --git a/RobotApp/Services/Navigation/NavigationController.cs b/RobotApp/Services/Navigation/NavigationController.cs deleted file mode 100644 index 9da57cf..0000000 --- a/RobotApp/Services/Navigation/NavigationController.cs +++ /dev/null @@ -1,5 +0,0 @@ -namespace RobotApp.Services.Navigation; - -public class NavigationController -{ -} diff --git a/RobotApp/Services/Navigation/Algorithm/FuzzyLogic.cs b/RobotApp/Services/Robot/Navigation/Algorithm/FuzzyLogic.cs similarity index 99% rename from RobotApp/Services/Navigation/Algorithm/FuzzyLogic.cs rename to RobotApp/Services/Robot/Navigation/Algorithm/FuzzyLogic.cs index b6bb273..817ed22 100644 --- a/RobotApp/Services/Navigation/Algorithm/FuzzyLogic.cs +++ b/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 { diff --git a/RobotApp/Services/Navigation/Algorithm/PID.cs b/RobotApp/Services/Robot/Navigation/Algorithm/PID.cs similarity index 94% rename from RobotApp/Services/Navigation/Algorithm/PID.cs rename to RobotApp/Services/Robot/Navigation/Algorithm/PID.cs index e74333c..d28112b 100644 --- a/RobotApp/Services/Navigation/Algorithm/PID.cs +++ b/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 { diff --git a/RobotApp/Services/Navigation/Algorithm/PurePursuit.cs b/RobotApp/Services/Robot/Navigation/Algorithm/PurePursuit.cs similarity index 100% rename from RobotApp/Services/Navigation/Algorithm/PurePursuit.cs rename to RobotApp/Services/Robot/Navigation/Algorithm/PurePursuit.cs diff --git a/RobotApp/Services/Robot/Navigation/DifferentialNavigation.cs b/RobotApp/Services/Robot/Navigation/DifferentialNavigation.cs new file mode 100644 index 0000000..bc88f3c --- /dev/null +++ b/RobotApp/Services/Robot/Navigation/DifferentialNavigation.cs @@ -0,0 +1,22 @@ +using RobotApp.Interfaces; + +namespace RobotApp.Services.Robot.Navigation; + +public class DifferentialNavigation(Logger navLogger, + Logger 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); + } + } +} diff --git a/RobotApp/Services/Robot/Navigation/NavigationController.cs b/RobotApp/Services/Robot/Navigation/NavigationController.cs new file mode 100644 index 0000000..c2d8e78 --- /dev/null +++ b/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 Logger) : INavigation +{ + public NavigationState State { get; private set; } = NavigationState.None; + public bool Driving { get; private set; } + + protected const int CycleHandlerMilliseconds = 20; + private WatchTimer? 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; + } +} diff --git a/RobotApp/Services/Navigation/NavigationManager.cs b/RobotApp/Services/Robot/Navigation/NavigationManager.cs similarity index 51% rename from RobotApp/Services/Navigation/NavigationManager.cs rename to RobotApp/Services/Robot/Navigation/NavigationManager.cs index 7575190..f6b38c2 100644 --- a/RobotApp/Services/Navigation/NavigationManager.cs +++ b/RobotApp/Services/Robot/Navigation/NavigationManager.cs @@ -1,4 +1,4 @@ -namespace RobotApp.Services.Navigation +namespace RobotApp.Services.Robot.Navigation { public class NavigationManager { diff --git a/RobotApp/Services/Robot/Navigation/NavigationNode.cs b/RobotApp/Services/Robot/Navigation/NavigationNode.cs new file mode 100644 index 0000000..ccfc219 --- /dev/null +++ b/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; +} diff --git a/RobotApp/Services/Robot/RobotAction.cs b/RobotApp/Services/Robot/RobotAction.cs index 0c280d3..ea5ad31 100644 --- a/RobotApp/Services/Robot/RobotAction.cs +++ b/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(); } } diff --git a/RobotApp/Services/Robot/RobotBattery.cs b/RobotApp/Services/Robot/RobotBattery.cs new file mode 100644 index 0000000..d145843 --- /dev/null +++ b/RobotApp/Services/Robot/RobotBattery.cs @@ -0,0 +1,6 @@ +namespace RobotApp.Services.Robot +{ + public class RobotBattery + { + } +} diff --git a/RobotApp/Services/Robot/RobotController.cs b/RobotApp/Services/Robot/RobotController.cs index 482457c..6d422d5 100644 --- a/RobotApp/Services/Robot/RobotController.cs +++ b/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 Logger, + RobotConnection ConnectionManager) : BackgroundService { - public class RobotController + private readonly Mutex NewOrderMutex = new(); + private readonly Mutex NewInstanceMutex = new(); + + private WatchTimer? 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(); + } + } } } diff --git a/RobotApp/Services/Robot/RobotErrors.cs b/RobotApp/Services/Robot/RobotErrors.cs new file mode 100644 index 0000000..132e6d4 --- /dev/null +++ b/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 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 = [] + }; + } +} diff --git a/RobotApp/Services/Robot/RobotFactsheet.cs b/RobotApp/Services/Robot/RobotFactsheet.cs index 07ac2c7..7168631 100644 --- a/RobotApp/Services/Robot/RobotFactsheet.cs +++ b/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(); } } diff --git a/RobotApp/Services/Robot/RobotInfomations.cs b/RobotApp/Services/Robot/RobotInfomations.cs new file mode 100644 index 0000000..ad5299b --- /dev/null +++ b/RobotApp/Services/Robot/RobotInfomations.cs @@ -0,0 +1,6 @@ +namespace RobotApp.Services.Robot +{ + public class RobotInfomations + { + } +} diff --git a/RobotApp/Services/Robot/RobotLoads.cs b/RobotApp/Services/Robot/RobotLoads.cs new file mode 100644 index 0000000..bdbcc27 --- /dev/null +++ b/RobotApp/Services/Robot/RobotLoads.cs @@ -0,0 +1,6 @@ +namespace RobotApp.Services.Robot +{ + public class RobotLoads + { + } +} diff --git a/RobotApp/Services/Robot/RobotOrderController.cs b/RobotApp/Services/Robot/RobotOrderController.cs index 12a5dcf..fe8762a 100644 --- a/RobotApp/Services/Robot/RobotOrderController.cs +++ b/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 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 OrderActions = []; + private readonly Mutex OrderMutex = new(); - public NodeState[] NodeStates => throw new NotImplementedException(); + protected const int CycleHandlerMilliseconds = 100; + private WatchTimer? 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; + } + + NodeStates = nodes.Select(n => new NodeState + { + 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 UpdateOrder() + 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 bool StopOrder() + public void StopOrder() { - throw new NotImplementedException(); + 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 + } + } } } diff --git a/RobotApp/Services/Robot/RobotPathPlanner.cs b/RobotApp/Services/Robot/RobotPathPlanner.cs new file mode 100644 index 0000000..18b1828 --- /dev/null +++ b/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 = [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]; + } +} diff --git a/RobotApp/Services/Robot/RobotSafety.cs b/RobotApp/Services/Robot/RobotSafety.cs new file mode 100644 index 0000000..c90f7e4 --- /dev/null +++ b/RobotApp/Services/Robot/RobotSafety.cs @@ -0,0 +1,6 @@ +namespace RobotApp.Services.Robot +{ + public class RobotSafety + { + } +} diff --git a/RobotApp/Services/Robot/RobotStates.cs b/RobotApp/Services/Robot/RobotStates.cs new file mode 100644 index 0000000..f47bdfc --- /dev/null +++ b/RobotApp/Services/Robot/RobotStates.cs @@ -0,0 +1,5 @@ +namespace RobotApp.Services.Robot; + +public class RobotStates +{ +} diff --git a/RobotApp/Services/RobotStateMachineService.cs b/RobotApp/Services/RobotStateMachineService.cs new file mode 100644 index 0000000..e69de29 diff --git a/RobotApp/Services/State/RobotState.cs b/RobotApp/Services/State/RobotState.cs new file mode 100644 index 0000000..78c63e8 --- /dev/null +++ b/RobotApp/Services/State/RobotState.cs @@ -0,0 +1,8 @@ +namespace RobotApp.Services.State; + +public abstract class RobotState(T type) where T : Enum +{ + public readonly T Type = type; + public virtual void Enter() { } + public virtual void Exit() { } +} \ No newline at end of file diff --git a/RobotApp/Services/State/RobotStateMachine.cs b/RobotApp/Services/State/RobotStateMachine.cs new file mode 100644 index 0000000..e69de29 diff --git a/RobotApp/Services/WatchTimer.cs b/RobotApp/Services/WatchTimer.cs index 08ff331..3b2d0aa 100644 --- a/RobotApp/Services/WatchTimer.cs +++ b/RobotApp/Services/WatchTimer.cs @@ -19,6 +19,7 @@ public class WatchTimer(int Interval, Action Callback, Logger? 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