From 0d97684f70cc6ce77a178aa26c8beda382424c22 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C4=90=C4=83ng=20Nguy=E1=BB=85n?= Date: Mon, 15 Sep 2025 17:39:02 +0700 Subject: [PATCH] update --- .../Enums/NavigationType.cs | 7 + RobotApp.Common.Shares/JsonOptionExtends.cs | 18 ++ RobotApp.Common.Shares/MessageResult.cs | 8 + .../RobotApp.Common.Shares.csproj | 9 + .../Factsheet/ActionParameters.cs | 8 +- RobotApp.VDA5050/Factsheet/AgvActions.cs | 13 +- RobotApp.VDA5050/Factsheet/AgvGeometry.cs | 7 +- RobotApp.VDA5050/Factsheet/Envelopes2d.cs | 8 +- RobotApp.VDA5050/Factsheet/Envelopes3d.cs | 12 +- RobotApp.VDA5050/Factsheet/FactSheetMsg.cs | 24 +- RobotApp.VDA5050/Factsheet/LoadSets.cs | 14 +- .../Factsheet/LoadSpecification.cs | 5 +- .../Factsheet/OptionalParameters.cs | 7 +- .../Factsheet/ProtocolFeatures.cs | 5 +- RobotApp.VDA5050/Factsheet/ProtocolLimits.cs | 7 +- .../Factsheet/TypeSpecification.cs | 10 +- .../Factsheet/WheelDefinitions.cs | 9 +- RobotApp.VDA5050/FactsheetExtend/Battery.cs | 9 - .../FactsheetExtend/BatteryThreshold.cs | 11 - .../FactsheetExtend/CameraSafety.cs | 31 --- .../FactsheetExtend/ChargerParam.cs | 9 - .../FactsheetExtend/FactsheetExtendMsg.cs | 19 -- .../FactsheetExtend/ForkSafety.cs | 9 - RobotApp.VDA5050/FactsheetExtend/Initpose.cs | 9 - .../FactsheetExtend/LineSegment.cs | 10 - .../FactsheetExtend/Localization.cs | 14 - RobotApp.VDA5050/FactsheetExtend/Motor.cs | 9 - .../FactsheetExtend/Navigation.cs | 11 - RobotApp.VDA5050/FactsheetExtend/PPA.cs | 7 - RobotApp.VDA5050/FactsheetExtend/PTA.cs | 15 -- .../FactsheetExtend/RobotParam.cs | 12 - RobotApp.VDA5050/FactsheetExtend/Rotate.cs | 9 - RobotApp.VDA5050/FactsheetExtend/Safety.cs | 8 - .../FactsheetExtend/ServerParam.cs | 14 - RobotApp.VDA5050/FactsheetExtend/VlMarker.cs | 21 -- RobotApp.VDA5050/FactsheetExtend/Xloc.cs | 16 -- .../InstantAction/{Actions.cs => Action.cs} | 3 +- .../InstantAction/ActionParameter.cs | 1 - .../InstantAction/InstantActionsMsg.cs | 2 - RobotApp.VDA5050/Type/ActionType.cs | 42 +-- RobotApp.VDA5050/VDA5050Helper.cs | 29 -- RobotApp.VDA5050/VDA5050Setting.cs | 21 +- RobotApp.VDA5050/VDA5050Topic.cs | 50 +++- RobotApp.sln | 6 + RobotApp/Interfaces/IDriver.cs | 37 +++ RobotApp/Interfaces/IInstanceActions.cs | 5 + RobotApp/Interfaces/ILocalization.cs | 28 ++ RobotApp/Interfaces/INavigation.cs | 33 +++ RobotApp/Interfaces/IOrder.cs | 15 ++ RobotApp/Interfaces/ISensorIMU.cs | 12 + RobotApp/Program.cs | 11 +- RobotApp/RobotApp.csproj | 17 +- RobotApp/Services/Logger.cs | 108 ++++++++ RobotApp/Services/MQTTClient.cs | 161 +++++++++++ .../Navigation/Algorithm/FuzzyLogic.cs | 252 ++++++++++++++++++ RobotApp/Services/Navigation/Algorithm/PID.cs | 47 ++++ .../Navigation/Algorithm/PurePursuit.cs | 156 +++++++++++ .../Navigation/NavigationController.cs | 5 + .../Services/Navigation/NavigationManager.cs | 6 + RobotApp/Services/Robot/RobotAction.cs | 6 + RobotApp/Services/Robot/RobotConfiguration.cs | 9 + RobotApp/Services/Robot/RobotConnection.cs | 69 +++++ RobotApp/Services/Robot/RobotController.cs | 6 + RobotApp/Services/Robot/RobotFactsheet.cs | 34 +++ .../Services/Robot/RobotOrderController.cs | 30 +++ RobotApp/Services/Robot/RobotVisualization.cs | 6 + RobotApp/Services/WatchTimer.cs | 76 ++++++ RobotApp/Services/WatchTimerAsync.cs | 76 ++++++ RobotApp/appsettings.json | 2 +- 69 files changed, 1363 insertions(+), 412 deletions(-) create mode 100644 RobotApp.Common.Shares/Enums/NavigationType.cs create mode 100644 RobotApp.Common.Shares/JsonOptionExtends.cs create mode 100644 RobotApp.Common.Shares/MessageResult.cs create mode 100644 RobotApp.Common.Shares/RobotApp.Common.Shares.csproj delete mode 100644 RobotApp.VDA5050/FactsheetExtend/Battery.cs delete mode 100644 RobotApp.VDA5050/FactsheetExtend/BatteryThreshold.cs delete mode 100644 RobotApp.VDA5050/FactsheetExtend/CameraSafety.cs delete mode 100644 RobotApp.VDA5050/FactsheetExtend/ChargerParam.cs delete mode 100644 RobotApp.VDA5050/FactsheetExtend/FactsheetExtendMsg.cs delete mode 100644 RobotApp.VDA5050/FactsheetExtend/ForkSafety.cs delete mode 100644 RobotApp.VDA5050/FactsheetExtend/Initpose.cs delete mode 100644 RobotApp.VDA5050/FactsheetExtend/LineSegment.cs delete mode 100644 RobotApp.VDA5050/FactsheetExtend/Localization.cs delete mode 100644 RobotApp.VDA5050/FactsheetExtend/Motor.cs delete mode 100644 RobotApp.VDA5050/FactsheetExtend/Navigation.cs delete mode 100644 RobotApp.VDA5050/FactsheetExtend/PPA.cs delete mode 100644 RobotApp.VDA5050/FactsheetExtend/PTA.cs delete mode 100644 RobotApp.VDA5050/FactsheetExtend/RobotParam.cs delete mode 100644 RobotApp.VDA5050/FactsheetExtend/Rotate.cs delete mode 100644 RobotApp.VDA5050/FactsheetExtend/Safety.cs delete mode 100644 RobotApp.VDA5050/FactsheetExtend/ServerParam.cs delete mode 100644 RobotApp.VDA5050/FactsheetExtend/VlMarker.cs delete mode 100644 RobotApp.VDA5050/FactsheetExtend/Xloc.cs rename RobotApp.VDA5050/InstantAction/{Actions.cs => Action.cs} (96%) delete mode 100644 RobotApp.VDA5050/VDA5050Helper.cs create mode 100644 RobotApp/Interfaces/IDriver.cs create mode 100644 RobotApp/Interfaces/IInstanceActions.cs create mode 100644 RobotApp/Interfaces/ILocalization.cs create mode 100644 RobotApp/Interfaces/INavigation.cs create mode 100644 RobotApp/Interfaces/IOrder.cs create mode 100644 RobotApp/Interfaces/ISensorIMU.cs create mode 100644 RobotApp/Services/Logger.cs create mode 100644 RobotApp/Services/MQTTClient.cs create mode 100644 RobotApp/Services/Navigation/Algorithm/FuzzyLogic.cs create mode 100644 RobotApp/Services/Navigation/Algorithm/PID.cs create mode 100644 RobotApp/Services/Navigation/Algorithm/PurePursuit.cs create mode 100644 RobotApp/Services/Navigation/NavigationController.cs create mode 100644 RobotApp/Services/Navigation/NavigationManager.cs create mode 100644 RobotApp/Services/Robot/RobotAction.cs create mode 100644 RobotApp/Services/Robot/RobotConfiguration.cs create mode 100644 RobotApp/Services/Robot/RobotConnection.cs create mode 100644 RobotApp/Services/Robot/RobotController.cs create mode 100644 RobotApp/Services/Robot/RobotFactsheet.cs create mode 100644 RobotApp/Services/Robot/RobotOrderController.cs create mode 100644 RobotApp/Services/Robot/RobotVisualization.cs create mode 100644 RobotApp/Services/WatchTimer.cs create mode 100644 RobotApp/Services/WatchTimerAsync.cs diff --git a/RobotApp.Common.Shares/Enums/NavigationType.cs b/RobotApp.Common.Shares/Enums/NavigationType.cs new file mode 100644 index 0000000..d018861 --- /dev/null +++ b/RobotApp.Common.Shares/Enums/NavigationType.cs @@ -0,0 +1,7 @@ +namespace RobotApp.Common.Shares.Enums; + +public enum NavigationType +{ + Differential, + Forklift, +} diff --git a/RobotApp.Common.Shares/JsonOptionExtends.cs b/RobotApp.Common.Shares/JsonOptionExtends.cs new file mode 100644 index 0000000..237c314 --- /dev/null +++ b/RobotApp.Common.Shares/JsonOptionExtends.cs @@ -0,0 +1,18 @@ +using System.Text.Json; + +namespace RobotApp.Common.Shares; + +public class JsonOptionExtends +{ + public static readonly JsonSerializerOptions Read = new() + { + PropertyNamingPolicy = JsonNamingPolicy.CamelCase, + WriteIndented = true, + }; + + public static readonly JsonSerializerOptions Write = new() + { + PropertyNamingPolicy = JsonNamingPolicy.CamelCase, + WriteIndented = true, + }; +} diff --git a/RobotApp.Common.Shares/MessageResult.cs b/RobotApp.Common.Shares/MessageResult.cs new file mode 100644 index 0000000..a0ee060 --- /dev/null +++ b/RobotApp.Common.Shares/MessageResult.cs @@ -0,0 +1,8 @@ +namespace RobotApp.Common.Shares; + +public record MessageResult(bool IsSuccess, string Message = ""); + +public record MessageResult(bool IsSuccess, string Message = "") +{ + public T? Data { get; set; } +} diff --git a/RobotApp.Common.Shares/RobotApp.Common.Shares.csproj b/RobotApp.Common.Shares/RobotApp.Common.Shares.csproj new file mode 100644 index 0000000..125f4c9 --- /dev/null +++ b/RobotApp.Common.Shares/RobotApp.Common.Shares.csproj @@ -0,0 +1,9 @@ + + + + net9.0 + enable + enable + + + diff --git a/RobotApp.VDA5050/Factsheet/ActionParameters.cs b/RobotApp.VDA5050/Factsheet/ActionParameters.cs index 9202786..f53b695 100644 --- a/RobotApp.VDA5050/Factsheet/ActionParameters.cs +++ b/RobotApp.VDA5050/Factsheet/ActionParameters.cs @@ -2,8 +2,6 @@ namespace RobotApp.VDA5050.Factsheet; -#nullable disable - public enum ValueDataType { BOOL, @@ -17,9 +15,9 @@ public enum ValueDataType public class ActionParameters { [Required] - public string Key { get; set; } + public string Key { get; set; } = string.Empty; [Required] - public string ValueDataType { get; set; } - public string Description { get; set; } + public string ValueDataType { get; set; } = string.Empty; + public string Description { get; set; } = string.Empty; public bool IsOptional { get; set; } } diff --git a/RobotApp.VDA5050/Factsheet/AgvActions.cs b/RobotApp.VDA5050/Factsheet/AgvActions.cs index f0a87a5..22b520a 100644 --- a/RobotApp.VDA5050/Factsheet/AgvActions.cs +++ b/RobotApp.VDA5050/Factsheet/AgvActions.cs @@ -2,7 +2,6 @@ namespace RobotApp.VDA5050.Factsheet; -#nullable disable public enum ActionScopes { @@ -13,11 +12,11 @@ public enum ActionScopes public class AgvActions { [Required] - public string ActionType { get; set; } - public string ActionDescription { get; set; } + public string ActionType { get; set; } = string.Empty; + public string ActionDescription { get; set; } = string.Empty; [Required] - public string[] ActionScopes { get; set; } - public ActionParameters[] ActionParameters { get; set; } - public string ResultDescription { get; set; } - public string[] BlockingTypes { get; set; } + public string[] ActionScopes { get; set; } = []; + public ActionParameters[] ActionParameters { get; set; } = []; + public string ResultDescription { get; set; } = string.Empty; + public string[] BlockingTypes { get; set; } = []; } diff --git a/RobotApp.VDA5050/Factsheet/AgvGeometry.cs b/RobotApp.VDA5050/Factsheet/AgvGeometry.cs index 2852120..940461e 100644 --- a/RobotApp.VDA5050/Factsheet/AgvGeometry.cs +++ b/RobotApp.VDA5050/Factsheet/AgvGeometry.cs @@ -2,11 +2,10 @@ namespace RobotApp.VDA5050.Factsheet; -#nullable disable public class AgvGeometry { - public WheelDefinitions[] WheelDefinitions { get; set; } - public Envelopes2d[] Envelopes2d { get; set; } - public Envelopes3d[] Envelopes3d { get; set; } + public WheelDefinitions[] WheelDefinitions { get; set; } = []; + public Envelopes2d[] Envelopes2d { get; set; } = []; + public Envelopes3d[] Envelopes3d { get; set; } = []; } diff --git a/RobotApp.VDA5050/Factsheet/Envelopes2d.cs b/RobotApp.VDA5050/Factsheet/Envelopes2d.cs index 01fad43..623be42 100644 --- a/RobotApp.VDA5050/Factsheet/Envelopes2d.cs +++ b/RobotApp.VDA5050/Factsheet/Envelopes2d.cs @@ -2,8 +2,6 @@ namespace RobotApp.VDA5050.Factsheet; -#nullable disable - public class PolygonPoints { [Required] @@ -14,8 +12,8 @@ public class PolygonPoints public class Envelopes2d { [Required] - public string Set { get; set; } + public string Set { get; set; } = string.Empty; [Required] - public PolygonPoints[] PolygonPoints { get; set; } - public string Description { get; set; } + public PolygonPoints[] PolygonPoints { get; set; } = []; + public string Description { get; set; } = string.Empty; } diff --git a/RobotApp.VDA5050/Factsheet/Envelopes3d.cs b/RobotApp.VDA5050/Factsheet/Envelopes3d.cs index 276d8bf..4e31f48 100644 --- a/RobotApp.VDA5050/Factsheet/Envelopes3d.cs +++ b/RobotApp.VDA5050/Factsheet/Envelopes3d.cs @@ -2,15 +2,13 @@ namespace RobotApp.VDA5050.Factsheet; -#nullable disable - public class Envelopes3d { [Required] - public string Set { get; set; } + public string Set { get; set; } = string.Empty; [Required] - public string Format { get; set; } - public object Data { get; set; } - public string Url { get; set; } - public string Description { get; set; } + public string Format { get; set; } = string.Empty; + public object Data { get; set; } = string.Empty; + public string Url { get; set; } = string.Empty; + public string Description { get; set; } = string.Empty; } diff --git a/RobotApp.VDA5050/Factsheet/FactSheetMsg.cs b/RobotApp.VDA5050/Factsheet/FactSheetMsg.cs index 41cef4d..20d88a9 100644 --- a/RobotApp.VDA5050/Factsheet/FactSheetMsg.cs +++ b/RobotApp.VDA5050/Factsheet/FactSheetMsg.cs @@ -2,29 +2,27 @@ namespace RobotApp.VDA5050.Factsheet; -#nullable disable public class FactSheetMsg { - public uint HeaderId { get; set; } - public string Timestamp { get; set; } + public uint HeaderId { get; set; } = 1; + public string Timestamp { get; set; } = DateTime.Now.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"); [Required] - public string Version { get; set; } + public string Version { get; set; } = "1.0.0"; [Required] - public string Manufacturer { get; set; } + public string Manufacturer { get; set; } = "PhenikaaX"; [Required] - public string SerialNumber { get; set; } + public string SerialNumber { get; set; } = string.Empty; [Required] - public TypeSpecification TypeSpecification { get; set; } + public TypeSpecification TypeSpecification { get; set; } = new(); [Required] - public PhysicalParameters PhysicalParameters { get; set; } + public PhysicalParameters PhysicalParameters { get; set; } = new(); [Required] - public ProtocolLimits ProtocolLimits { get; set; } + public ProtocolLimits ProtocolLimits { get; set; } = new(); [Required] - public ProtocolFeatures ProtocolFeatures { get; set; } + public ProtocolFeatures ProtocolFeatures { get; set; } = new(); [Required] - public AgvGeometry AgvGeometry { get; set; } + public AgvGeometry AgvGeometry { get; set; } = new(); [Required] - public LoadSpecification LoadSpecification { get; set; } - //public LocalizationParameter LocalizationParameters { get; set; } + public LoadSpecification LoadSpecification { get; set; } = new(); } diff --git a/RobotApp.VDA5050/Factsheet/LoadSets.cs b/RobotApp.VDA5050/Factsheet/LoadSets.cs index ce9fb7f..0af120a 100644 --- a/RobotApp.VDA5050/Factsheet/LoadSets.cs +++ b/RobotApp.VDA5050/Factsheet/LoadSets.cs @@ -1,14 +1,12 @@ namespace RobotApp.VDA5050.Factsheet; -#nullable disable - public class LoadSets { - public string SetName { get; set; } - public string LoadType { get; set; } - public string[] LoadPositions { get; set; } - public BoundingBoxReference BoundingBoxReference { get; set; } - public LoadDimensions LoadDimensions { get; set; } + public string SetName { get; set; } = string.Empty; + public string LoadType { get; set; } = string.Empty; + public string[] LoadPositions { get; set; } = []; + public BoundingBoxReference BoundingBoxReference { get; set; } = new(); + public LoadDimensions LoadDimensions { get; set; } = new(); public double MaxWeigth { get; set; } public double MinLoadhandlingHeight { get; set; } public double MaxLoadhandlingHeight { get; set; } @@ -21,6 +19,6 @@ public class LoadSets public double AgvDecelerationLimit { get; set; } public double PickTime { get; set; } public double DropTime { get; set; } - public string Description { get; set; } + public string Description { get; set; } = string.Empty; } diff --git a/RobotApp.VDA5050/Factsheet/LoadSpecification.cs b/RobotApp.VDA5050/Factsheet/LoadSpecification.cs index 7102e9c..0eae235 100644 --- a/RobotApp.VDA5050/Factsheet/LoadSpecification.cs +++ b/RobotApp.VDA5050/Factsheet/LoadSpecification.cs @@ -1,9 +1,8 @@ namespace RobotApp.VDA5050.Factsheet; -#nullable disable public class LoadSpecification { - public string[] LoadPositions { get; set; } - public LoadSets[] LoadSets { get; set; } + public string[] LoadPositions { get; set; } = []; + public LoadSets[] LoadSets { get; set; } = []; } diff --git a/RobotApp.VDA5050/Factsheet/OptionalParameters.cs b/RobotApp.VDA5050/Factsheet/OptionalParameters.cs index ed7ecaa..c06dc16 100644 --- a/RobotApp.VDA5050/Factsheet/OptionalParameters.cs +++ b/RobotApp.VDA5050/Factsheet/OptionalParameters.cs @@ -2,7 +2,6 @@ namespace RobotApp.VDA5050.Factsheet; -#nullable disable public enum Support { @@ -12,8 +11,8 @@ public enum Support public class OptionalParameters { [Required] - public string Parameter { get; set; } + public string Parameter { get; set; } = string.Empty; [Required] - public string Support { get; set; } - public string Description { get; set; } + public string Support { get; set; } = string.Empty; + public string Description { get; set; } = string.Empty; } diff --git a/RobotApp.VDA5050/Factsheet/ProtocolFeatures.cs b/RobotApp.VDA5050/Factsheet/ProtocolFeatures.cs index d876148..ac67457 100644 --- a/RobotApp.VDA5050/Factsheet/ProtocolFeatures.cs +++ b/RobotApp.VDA5050/Factsheet/ProtocolFeatures.cs @@ -2,12 +2,11 @@ namespace RobotApp.VDA5050.Factsheet; -#nullable disable public class ProtocolFeatures { [Required] - public OptionalParameters[] OptionalParameters { get; set; } + public OptionalParameters[] OptionalParameters { get; set; } = []; [Required] - public AgvActions[] AgvActions { get; set; } + public AgvActions[] AgvActions { get; set; } = []; } diff --git a/RobotApp.VDA5050/Factsheet/ProtocolLimits.cs b/RobotApp.VDA5050/Factsheet/ProtocolLimits.cs index 4f58e8d..8d22e6c 100644 --- a/RobotApp.VDA5050/Factsheet/ProtocolLimits.cs +++ b/RobotApp.VDA5050/Factsheet/ProtocolLimits.cs @@ -2,13 +2,12 @@ namespace RobotApp.VDA5050.Factsheet; -#nullable disable public class ProtocolLimits { [Required] - public MaxStringLens MaxStringLens { get; set; } + public MaxStringLens MaxStringLens { get; set; } = new(); [Required] - public MaxArrayLens MaxArrayLens { get; set; } + public MaxArrayLens MaxArrayLens { get; set; } = new(); [Required] - public Timing Timing { get; set; } + public Timing Timing { get; set; } = new(); } diff --git a/RobotApp.VDA5050/Factsheet/TypeSpecification.cs b/RobotApp.VDA5050/Factsheet/TypeSpecification.cs index c3d2e9a..fdc8766 100644 --- a/RobotApp.VDA5050/Factsheet/TypeSpecification.cs +++ b/RobotApp.VDA5050/Factsheet/TypeSpecification.cs @@ -2,8 +2,6 @@ namespace RobotApp.VDA5050.Factsheet; -#nullable disable - public enum AgvKinematic { DIFF, @@ -35,12 +33,12 @@ public enum NavigationTypes public class TypeSpecification { [Required] - public string SeriesName { get; set; } - public string SeriesDescription { get; set; } + public string SeriesName { get; set; } = string.Empty; + public string SeriesDescription { get; set; } = string.Empty; [Required] - public string AgvKinematic { get; set; } + public string AgvKinematic { get; set; } = string.Empty; [Required] - public string AgvClass { get; set; } + public string AgvClass { get; set; } = string.Empty; [Required] public double MaxLoadMass { get; set; } [Required] diff --git a/RobotApp.VDA5050/Factsheet/WheelDefinitions.cs b/RobotApp.VDA5050/Factsheet/WheelDefinitions.cs index 1b86945..5291128 100644 --- a/RobotApp.VDA5050/Factsheet/WheelDefinitions.cs +++ b/RobotApp.VDA5050/Factsheet/WheelDefinitions.cs @@ -2,8 +2,6 @@ namespace RobotApp.VDA5050.Factsheet; -#nullable disable - public enum WheelDefinitionsType { DRIVE, @@ -11,6 +9,7 @@ public enum WheelDefinitionsType FIXED, MECANUM, } + public class WheelDefinitionsPosition { [Required] @@ -23,17 +22,17 @@ public class WheelDefinitionsPosition public class WheelDefinitions { [Required] - public string Type { get; set; } + public string Type { get; set; } = string.Empty; [Required] public bool IsActiveDriven { get; set; } [Required] public bool IsActiveSteered { get; set; } [Required] - public WheelDefinitionsPosition Position { get; set; } + public WheelDefinitionsPosition Position { get; set; } = new(); [Required] public double Diameter { get; set; } [Required] public double Width { get; set; } public double CenterDisplacement { get; set; } - public string Constraints { get; set; } + public string Constraints { get; set; } = string.Empty; } diff --git a/RobotApp.VDA5050/FactsheetExtend/Battery.cs b/RobotApp.VDA5050/FactsheetExtend/Battery.cs deleted file mode 100644 index b38594d..0000000 --- a/RobotApp.VDA5050/FactsheetExtend/Battery.cs +++ /dev/null @@ -1,9 +0,0 @@ -namespace RobotApp.VDA5050.FactsheetExtend; - -public class Battery -{ - public uint Battery_low { get; set; } - public uint Battery_normal { get; set; } - public uint Battery_good { get; set; } - public uint Battery_full { get; set; } -} diff --git a/RobotApp.VDA5050/FactsheetExtend/BatteryThreshold.cs b/RobotApp.VDA5050/FactsheetExtend/BatteryThreshold.cs deleted file mode 100644 index 970ba3e..0000000 --- a/RobotApp.VDA5050/FactsheetExtend/BatteryThreshold.cs +++ /dev/null @@ -1,11 +0,0 @@ -namespace RobotApp.VDA5050.FactsheetExtend; - -public enum BatteryThreshold -{ - LOW, - NORMAL, - MIDDLE, - GOOD, - FULL, - NONE -} diff --git a/RobotApp.VDA5050/FactsheetExtend/CameraSafety.cs b/RobotApp.VDA5050/FactsheetExtend/CameraSafety.cs deleted file mode 100644 index 5a503b6..0000000 --- a/RobotApp.VDA5050/FactsheetExtend/CameraSafety.cs +++ /dev/null @@ -1,31 +0,0 @@ -namespace RobotApp.VDA5050.FactsheetExtend; - -public class CameraSafety -{ - public double Pass_through_x_min { get; set; } - public double Pass_through_x_max { get; set; } - public double Pass_through_y_min { get; set; } - public double Pass_through_y_max { get; set; } - public double Pass_through_z_min { get; set; } - public double Pass_through_z_max { get; set; } - public uint Ground_seg_max_iterations { get; set; } - public double Ground_seg_distance_threshold { get; set; } - public double Warn_z1 { get; set; } - public double Protect_z1 { get; set; } - public double Warn_z2 { get; set; } - public double Protect_z2 { get; set; } - public double Warn_z3 { get; set; } - public double Protect_z3 { get; set; } - public double Warn_z4 { get; set; } - public double Protect_z4 { get; set; } - public double Warn_z5 { get; set; } - public double Protect_z5 { get; set; } - public double Warn_z6 { get; set; } - public double Protect_z6 { get; set; } - public uint Min_cluster_warn_size { get; set; } - public uint Min_cluster_protect_size { get; set; } - public uint Min_cluster_detect_size { get; set; } - public uint Min_consecutive_warn_count { get; set; } - public uint Min_consecutive_protect_count { get; set; } - public uint Min_consecutive_detect_count { get; set; } -} diff --git a/RobotApp.VDA5050/FactsheetExtend/ChargerParam.cs b/RobotApp.VDA5050/FactsheetExtend/ChargerParam.cs deleted file mode 100644 index 0f38c76..0000000 --- a/RobotApp.VDA5050/FactsheetExtend/ChargerParam.cs +++ /dev/null @@ -1,9 +0,0 @@ -namespace RobotApp.VDA5050.FactsheetExtend; - -#nullable disable - -public class ChargerParam -{ - public string Charger_ip { get; set; } - public uint Charger_port { get; set; } -} diff --git a/RobotApp.VDA5050/FactsheetExtend/FactsheetExtendMsg.cs b/RobotApp.VDA5050/FactsheetExtend/FactsheetExtendMsg.cs deleted file mode 100644 index c794d8d..0000000 --- a/RobotApp.VDA5050/FactsheetExtend/FactsheetExtendMsg.cs +++ /dev/null @@ -1,19 +0,0 @@ -namespace RobotApp.VDA5050.FactsheetExtend; - -#nullable disable - -public class FactsheetExtendMsg -{ - public uint HeaderId { get; set; } - public DateTime Timestamp { get; set; } - public string Version { get; set; } - public string Manufacturer { get; set; } - public string SerialNumber { get; set; } - - public ServerParam Server_param { get; set; } - public RobotParam Robot_param { get; set; } - public Localization Localization { get; set; } - public Navigation Navigation { get; set; } - public Safety Safety { get; set; } - public ChargerParam Charger_param { get; set; } -} diff --git a/RobotApp.VDA5050/FactsheetExtend/ForkSafety.cs b/RobotApp.VDA5050/FactsheetExtend/ForkSafety.cs deleted file mode 100644 index eaa0a99..0000000 --- a/RobotApp.VDA5050/FactsheetExtend/ForkSafety.cs +++ /dev/null @@ -1,9 +0,0 @@ -namespace RobotApp.VDA5050.FactsheetExtend; - -public class ForkSafety -{ - public double Muted_field_size { get; set; } - public double Protected_field_size { get; set; } - public double Warning_field_size { get; set; } - public double Detect_field_size { get; set; } -} diff --git a/RobotApp.VDA5050/FactsheetExtend/Initpose.cs b/RobotApp.VDA5050/FactsheetExtend/Initpose.cs deleted file mode 100644 index 45917dd..0000000 --- a/RobotApp.VDA5050/FactsheetExtend/Initpose.cs +++ /dev/null @@ -1,9 +0,0 @@ -namespace RobotApp.VDA5050.FactsheetExtend; - -public class Initpose -{ - public bool Use_manual_initpose { get; set; } - public double Initpose_x { get; set; } - public double Initpose_y { get; set; } - public double Initpose_yaw { get; set; } -} diff --git a/RobotApp.VDA5050/FactsheetExtend/LineSegment.cs b/RobotApp.VDA5050/FactsheetExtend/LineSegment.cs deleted file mode 100644 index cbb7d06..0000000 --- a/RobotApp.VDA5050/FactsheetExtend/LineSegment.cs +++ /dev/null @@ -1,10 +0,0 @@ -namespace RobotApp.VDA5050.FactsheetExtend; - -public class LineSegment -{ - public double Least_thresh { get; set; } - public double Min_line_length { get; set; } - public double Predict_distance { get; set; } - public uint Seed_line_points { get; set; } - public uint Min_line_points { get; set; } -} diff --git a/RobotApp.VDA5050/FactsheetExtend/Localization.cs b/RobotApp.VDA5050/FactsheetExtend/Localization.cs deleted file mode 100644 index 51fdfec..0000000 --- a/RobotApp.VDA5050/FactsheetExtend/Localization.cs +++ /dev/null @@ -1,14 +0,0 @@ -namespace RobotApp.VDA5050.FactsheetExtend; - -#nullable disable - -public class Localization -{ - public uint Threshold_quality_loc { get; set; } - public bool Use_localization_marker { get; set; } - public bool Use_pallet_detection { get; set; } - public Initpose Initpose { get; set; } - public Xloc Xloc { get; set; } - public VlMarker Vl_marker { get; set; } - -} diff --git a/RobotApp.VDA5050/FactsheetExtend/Motor.cs b/RobotApp.VDA5050/FactsheetExtend/Motor.cs deleted file mode 100644 index 0302320..0000000 --- a/RobotApp.VDA5050/FactsheetExtend/Motor.cs +++ /dev/null @@ -1,9 +0,0 @@ -namespace RobotApp.VDA5050.FactsheetExtend; - -public class Motor -{ - public double OdomEncSteeringAngleOffset { get; set; } - public double Steering_fix_wheel_distance_x { get; set; } - public double Steering_fix_wheel_distance_y { get; set; } - public double WheelAcceleration { get; set; } -} diff --git a/RobotApp.VDA5050/FactsheetExtend/Navigation.cs b/RobotApp.VDA5050/FactsheetExtend/Navigation.cs deleted file mode 100644 index 8ece0e9..0000000 --- a/RobotApp.VDA5050/FactsheetExtend/Navigation.cs +++ /dev/null @@ -1,11 +0,0 @@ -namespace RobotApp.VDA5050.FactsheetExtend; - -#nullable disable -public class Navigation -{ - public bool Using_control_safety { get; set; } - public uint Control_rate { get; set; } - public Rotate Rotate { get; set; } - public PTA Pta { get; set; } - public PPA Ppa { get; set; } -} diff --git a/RobotApp.VDA5050/FactsheetExtend/PPA.cs b/RobotApp.VDA5050/FactsheetExtend/PPA.cs deleted file mode 100644 index 83f045f..0000000 --- a/RobotApp.VDA5050/FactsheetExtend/PPA.cs +++ /dev/null @@ -1,7 +0,0 @@ -namespace RobotApp.VDA5050.FactsheetExtend; - -public class PPA -{ - public double Ppa_accuracy_goal { get; set; } - public double Ppa_distance_reduce { get; set; } -} diff --git a/RobotApp.VDA5050/FactsheetExtend/PTA.cs b/RobotApp.VDA5050/FactsheetExtend/PTA.cs deleted file mode 100644 index b97e588..0000000 --- a/RobotApp.VDA5050/FactsheetExtend/PTA.cs +++ /dev/null @@ -1,15 +0,0 @@ -namespace RobotApp.VDA5050.FactsheetExtend; - -public class PTA -{ - public double Pta_linear_vel_max { get; set; } - public double Pta_linear_vel_min { get; set; } - public double Pta_accuracy_goal { get; set; } - public double Pta_distance_reduce { get; set; } - public double Pta_acceleration { get; set; } - public double Pta_lm_front { get; set; } - public double Pta_lm_back { get; set; } - public double Pta_phi_max { get; set; } - public double Pta_amplitude_max { get; set; } - public uint Pta_w_option { get; set; } -} diff --git a/RobotApp.VDA5050/FactsheetExtend/RobotParam.cs b/RobotApp.VDA5050/FactsheetExtend/RobotParam.cs deleted file mode 100644 index 73c4d59..0000000 --- a/RobotApp.VDA5050/FactsheetExtend/RobotParam.cs +++ /dev/null @@ -1,12 +0,0 @@ -namespace RobotApp.VDA5050.FactsheetExtend; - -public class RobotParam -{ - public bool Use_dynamic_parameter { get; set; } // (Default: True) Declare whether to use dynamic parameters or not - public string? Ethernet_name { get; set; } // The name of Ethernet port which connects to wifi client (e.g eno1, lo, enp3s0) - public double Speed_max_backward { get; set; } // (Default: True) Declare whether to use dynamic parameters or not - public uint Num_day_logger { get; set; } // The name of Ethernet port which connects to wifi client (e.g eno1, lo, enp3s0) - - public Motor Motor { get; set; } = new(); - public Battery Battery { get; set; } = new(); -} diff --git a/RobotApp.VDA5050/FactsheetExtend/Rotate.cs b/RobotApp.VDA5050/FactsheetExtend/Rotate.cs deleted file mode 100644 index ea85c7f..0000000 --- a/RobotApp.VDA5050/FactsheetExtend/Rotate.cs +++ /dev/null @@ -1,9 +0,0 @@ -namespace RobotApp.VDA5050.FactsheetExtend; - -public class Rotate -{ - public double Angular_vel_max { get; set; } - public double Angular_vel_min { get; set; } - public double Acceleration_rotate { get; set; } - public double Tolerances_rotate { get; set; } -} diff --git a/RobotApp.VDA5050/FactsheetExtend/Safety.cs b/RobotApp.VDA5050/FactsheetExtend/Safety.cs deleted file mode 100644 index 14d89b7..0000000 --- a/RobotApp.VDA5050/FactsheetExtend/Safety.cs +++ /dev/null @@ -1,8 +0,0 @@ -namespace RobotApp.VDA5050.FactsheetExtend; - -public class Safety -{ - public bool Use_camera_safety { get; set; } - public CameraSafety Camera_safety { get; set; } = new(); - public ForkSafety Fork_safety { get; set; } = new(); -} diff --git a/RobotApp.VDA5050/FactsheetExtend/ServerParam.cs b/RobotApp.VDA5050/FactsheetExtend/ServerParam.cs deleted file mode 100644 index 8a45ccc..0000000 --- a/RobotApp.VDA5050/FactsheetExtend/ServerParam.cs +++ /dev/null @@ -1,14 +0,0 @@ -namespace RobotApp.VDA5050.FactsheetExtend; - -#nullable disable - -public class ServerParam -{ - public string Server_ip { get; set; } - public string Server_port { get; set; } - public string Keepalive { get; set; } - public string Username { get; set; } - public string Password { get; set; } - public string Client_protocol { get; set; } - public string Client_id { get; set; } -} diff --git a/RobotApp.VDA5050/FactsheetExtend/VlMarker.cs b/RobotApp.VDA5050/FactsheetExtend/VlMarker.cs deleted file mode 100644 index cbfd3c1..0000000 --- a/RobotApp.VDA5050/FactsheetExtend/VlMarker.cs +++ /dev/null @@ -1,21 +0,0 @@ -namespace RobotApp.VDA5050.FactsheetExtend; - -public class VlMarker -{ - public bool Use_odometry { get; set; } - public uint V_angle { get; set; } - public double Length_v { get; set; } - public double Length_l { get; set; } - public double X_laser { get; set; } - public double Y_laser { get; set; } - public bool Flip_laser { get; set; } - public double Rotate_laser { get; set; } - public uint Frequence_control { get; set; } - public double Angle_min { get; set; } - public double Angle_max { get; set; } - public double Max_init_x { get; set; } - public double Max_init_y { get; set; } - public double Max_init_yaw { get; set; } - - public LineSegment Line_segment { get; set; } = new(); -} diff --git a/RobotApp.VDA5050/FactsheetExtend/Xloc.cs b/RobotApp.VDA5050/FactsheetExtend/Xloc.cs deleted file mode 100644 index f87da47..0000000 --- a/RobotApp.VDA5050/FactsheetExtend/Xloc.cs +++ /dev/null @@ -1,16 +0,0 @@ -namespace RobotApp.VDA5050.FactsheetExtend; - -public class Xloc -{ - public double Front_vls_width { get; set; } - public double Front_vls_pose_x { get; set; } - public double Front_vls_pose_y { get; set; } - public double Front_vls_pose_yaw { get; set; } - public uint Front_vls_source_id { get; set; } - - public double Rear_vls_width { get; set; } - public double Rear_vls_pose_x { get; set; } - public double Rear_vls_pose_y { get; set; } - public double Rear_vls_pose_yaw { get; set; } - public uint Rear_vls_source_id { get; set; } -} diff --git a/RobotApp.VDA5050/InstantAction/Actions.cs b/RobotApp.VDA5050/InstantAction/Action.cs similarity index 96% rename from RobotApp.VDA5050/InstantAction/Actions.cs rename to RobotApp.VDA5050/InstantAction/Action.cs index 55d6dd5..fe09c7f 100644 --- a/RobotApp.VDA5050/InstantAction/Actions.cs +++ b/RobotApp.VDA5050/InstantAction/Action.cs @@ -2,14 +2,13 @@ namespace RobotApp.VDA5050.InstantAction; -#nullable disable - public enum BlockingType { NONE, SOFT, HARD } + public class Action { [Required] diff --git a/RobotApp.VDA5050/InstantAction/ActionParameter.cs b/RobotApp.VDA5050/InstantAction/ActionParameter.cs index b701b60..afab9bf 100644 --- a/RobotApp.VDA5050/InstantAction/ActionParameter.cs +++ b/RobotApp.VDA5050/InstantAction/ActionParameter.cs @@ -2,7 +2,6 @@ namespace RobotApp.VDA5050.InstantAction; -#nullable disable public class ActionParameter { [Required] diff --git a/RobotApp.VDA5050/InstantAction/InstantActionsMsg.cs b/RobotApp.VDA5050/InstantAction/InstantActionsMsg.cs index 17355e3..4711ef1 100644 --- a/RobotApp.VDA5050/InstantAction/InstantActionsMsg.cs +++ b/RobotApp.VDA5050/InstantAction/InstantActionsMsg.cs @@ -1,7 +1,5 @@ namespace RobotApp.VDA5050.InstantAction; -#nullable disable - public class InstantActionsMsg { public uint HeaderId { get; set; } diff --git a/RobotApp.VDA5050/Type/ActionType.cs b/RobotApp.VDA5050/Type/ActionType.cs index 58c473d..4b56503 100644 --- a/RobotApp.VDA5050/Type/ActionType.cs +++ b/RobotApp.VDA5050/Type/ActionType.cs @@ -2,34 +2,16 @@ public enum ActionType { - startPause, // No actionParameters - stopPause, // No actionParameters - startCharging, // ActionParameters {CHARGER_IP, CHARGER_PORT, X, Y, THETA} - stopCharging, // ActionParameters {X, Y, THETA} - initPosition, // ActionParameters {X, Y, THETA, MAP_ID, LAST_NODE_ID} - stateRequest, // No actionParameters - logReport, // No actionParameters - pick, // No actionParameters - drop, // No actionParameters - detectObject, // No actionParameters - finePositioning, // ActionParameters {X, Y, THETA, MAP_ID, LAST_NODE_ID} - waitForTrigger, // No actionParameters - cancelOrder, // No actionParameters - factsheetRequest, // No actionParameters - - setDynparam, // ActionParameters {PARAM_NAME, PARAM_TYPE, PARAM_VALUE} - saveDynparamRuntime, // No actionParameters - loadDynparamRuntime, // No actionParameters - loadDynparamDefault, // No actionParameters - getDynparamRuntime, // No actionParameters - - controlLight, // ActionParameters {LIGHT_TYPE, CONTROL_TYPE} - controlFan, // ActionParameters {CONTROL_TYPE} - controlSpeaker, // ActionParameters {SONG_NUMBER, CONTROL_TYPE} - controlSafetyField, // ActionParameters {FIELD_TYPE, CONTROL_TYPE} - startInPallet, // ActionParameters {X, Y, THETA} - startOutPallet, // ActionParameters {X, Y, THETA} - - rotate, // ActionParameters {THETA} - setMap, // ActionParameter {MAP_ID} + START_PAUSE, + STOP_PAUSE, + START_CHARGING, + STOP_CHARGING, + INITIALIZATION_POSITION, + PICK, + DROP, + CANCEL_ORDER, + ROTATE, + REQUEST_FACTSHEET, + REQUEST_VISUALIZATION, + REQUEST_STATE, } diff --git a/RobotApp.VDA5050/VDA5050Helper.cs b/RobotApp.VDA5050/VDA5050Helper.cs deleted file mode 100644 index 6fc8be0..0000000 --- a/RobotApp.VDA5050/VDA5050Helper.cs +++ /dev/null @@ -1,29 +0,0 @@ -using RobotApp.VDA5050.State; -using System.Text; - -namespace RobotApp.VDA5050; - -public class VDA5050Helper -{ - public static string ConvertErrorDetail(IEnumerable errors) - { - string errorsType = ""; - foreach (var error in errors) - { - if (error == errors.Last()) errorsType += $"{error.ErrorType}"; - else errorsType += $"{error.ErrorType}, "; - } - StringBuilder errorStr = new($"Robot có lỗi: [{errorsType}]\n"); - foreach (var error in errors) - { - string errorDes = $"- {error.ErrorType}: {error.ErrorDescription}\n"; - errorStr.Append(errorDes.PadLeft(errorDes.Length + 3)); - foreach (var refer in error.ErrorReferences) - { - string errorRefer = $"+ {refer.ReferenceKey}: {refer.ReferenceValue}\n"; - errorStr.Append(errorRefer.PadLeft(errorRefer.Length + 9)); - } - } - return !errors.Any() ? "" : errorStr.ToString(); - } -} diff --git a/RobotApp.VDA5050/VDA5050Setting.cs b/RobotApp.VDA5050/VDA5050Setting.cs index 58f59b0..95cac01 100644 --- a/RobotApp.VDA5050/VDA5050Setting.cs +++ b/RobotApp.VDA5050/VDA5050Setting.cs @@ -1,19 +1,12 @@ namespace RobotApp.VDA5050; -#nullable disable - public class VDA5050Setting { - public bool ServerEnable { get; set; } - public string HostServer { get; set; } - public int Port { get; set; } - public string UserName { get; set; } - public string Password { get; set; } - public string Manufacturer { get; set; } - public string Version { get; set; } - public int Repeat { get; set; } - public int ConnectionTimeoutSeconds { get; set; } - public int ConnectionBacklog { set; get; } - public int KeepAliveInterval { get; set; } - public int CheckingRobotMsgTimout { get; set; } + public string HostServer { get; set; } = string.Empty; + public int Port { get; set; } = 1883; + public string UserName { get; set; } = "robotics"; + public string Password { get; set; } = "robotics"; + public string Manufacturer { get; set; } = "PhenikaaX"; + public string Version { get; set; } = "0.0.1"; + public int PublishRepeat { get; set; } = 2; } diff --git a/RobotApp.VDA5050/VDA5050Topic.cs b/RobotApp.VDA5050/VDA5050Topic.cs index af26ef4..89cb92a 100644 --- a/RobotApp.VDA5050/VDA5050Topic.cs +++ b/RobotApp.VDA5050/VDA5050Topic.cs @@ -1,12 +1,46 @@ namespace RobotApp.VDA5050; -public static class VDA5050Topic +public enum VDA5050Topic { - public const string Connection = "connection"; - public const string Order = "order"; - public const string InstantActions = "instantActions"; - public const string State = "state"; - public const string Visualization = "visualization"; - public const string Factsheet = "factsheet"; - public const string FactsheetExtend = "factsheetExtend"; // custom by TungNV + CONNECTION, + ORDER, + INSTANTACTIONS, + STATE, + VISUALIZATION, + FACTSHEET } + +public static class EnumExtensions +{ + private static readonly Dictionary TopicToStringMap = new() + { + { VDA5050Topic.CONNECTION, "connection" }, + { VDA5050Topic.ORDER, "order" }, + { VDA5050Topic.INSTANTACTIONS, "instantActions" }, + { VDA5050Topic.STATE, "state" }, + { VDA5050Topic.VISUALIZATION, "visualization" }, + { VDA5050Topic.FACTSHEET, "factsheet" } + }; + + private static readonly Dictionary StringToTopicMap = new(StringComparer.OrdinalIgnoreCase) + { + { "connection", VDA5050Topic.CONNECTION }, + { "order", VDA5050Topic.ORDER }, + { "instantActions", VDA5050Topic.INSTANTACTIONS }, + { "state", VDA5050Topic.STATE }, + { "visualization", VDA5050Topic.VISUALIZATION }, + { "factsheet", VDA5050Topic.FACTSHEET } + }; + + public static string ToTopicString(this VDA5050Topic type) + { + if (TopicToStringMap.TryGetValue(type, out var value)) return value; + throw new ArgumentException($"Invalid VDA5050Topic: {type}"); + } + + public static VDA5050Topic ToTopic(string value) + { + if (StringToTopicMap.TryGetValue(value, out var result)) return result; + throw new ArgumentException($"No VDA5050Topic with string value '{value}' found."); + } +} \ No newline at end of file diff --git a/RobotApp.sln b/RobotApp.sln index 0a573f0..c17bc5b 100644 --- a/RobotApp.sln +++ b/RobotApp.sln @@ -9,6 +9,8 @@ Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "RobotApp.Client", "RobotApp EndProject Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "RobotApp.VDA5050", "RobotApp.VDA5050\RobotApp.VDA5050.csproj", "{617FD155-904A-44E6-AD1A-6BC878421F9F}" EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "RobotApp.Common.Shares", "RobotApp.Common.Shares\RobotApp.Common.Shares.csproj", "{480F459B-F07C-44D9-A738-E8DF6C438A80}" +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|Any CPU = Debug|Any CPU @@ -27,6 +29,10 @@ Global {617FD155-904A-44E6-AD1A-6BC878421F9F}.Debug|Any CPU.Build.0 = Debug|Any CPU {617FD155-904A-44E6-AD1A-6BC878421F9F}.Release|Any CPU.ActiveCfg = Release|Any CPU {617FD155-904A-44E6-AD1A-6BC878421F9F}.Release|Any CPU.Build.0 = Release|Any CPU + {480F459B-F07C-44D9-A738-E8DF6C438A80}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {480F459B-F07C-44D9-A738-E8DF6C438A80}.Debug|Any CPU.Build.0 = Debug|Any CPU + {480F459B-F07C-44D9-A738-E8DF6C438A80}.Release|Any CPU.ActiveCfg = Release|Any CPU + {480F459B-F07C-44D9-A738-E8DF6C438A80}.Release|Any CPU.Build.0 = Release|Any CPU EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/RobotApp/Interfaces/IDriver.cs b/RobotApp/Interfaces/IDriver.cs new file mode 100644 index 0000000..1224c57 --- /dev/null +++ b/RobotApp/Interfaces/IDriver.cs @@ -0,0 +1,37 @@ +namespace RobotApp.Interfaces; + +/// +/// Interface điều khiển động cơ robot +/// +public interface IDriver +{ + /// + /// Trạng thái sẵn sàng nhận lệnh điều khiển + /// + bool IsReady { get; } + + /// + /// Tốc độ động cơ bên trái + /// + double LeftVelocity { get; } + + /// + /// Tốc độ động cơ bên phải + /// + double RightVelocity { get; } + + /// + /// Điều khiển tốc độ động cơ trái và phải + /// + /// Tốc độ động cơ bên trái + /// Tốc độ động cơ bên phải + /// + bool ControlVelocity(double left, double right); + + /// + /// Dừng robot + /// + /// true nếu dừng thả trôi, false nếu dừng khóa cứng + /// + bool Stop(bool isFree); +} diff --git a/RobotApp/Interfaces/IInstanceActions.cs b/RobotApp/Interfaces/IInstanceActions.cs new file mode 100644 index 0000000..3fb168f --- /dev/null +++ b/RobotApp/Interfaces/IInstanceActions.cs @@ -0,0 +1,5 @@ +namespace RobotApp.Interfaces; + +public interface IInstanceActions +{ +} diff --git a/RobotApp/Interfaces/ILocalization.cs b/RobotApp/Interfaces/ILocalization.cs new file mode 100644 index 0000000..847ee4a --- /dev/null +++ b/RobotApp/Interfaces/ILocalization.cs @@ -0,0 +1,28 @@ +namespace RobotApp.Interfaces; + +public interface ILocalization +{ + /// + /// Vị trí hiện tại của robot trong hệ tọa độ toàn cục theo trục X (đơn vị: mét). + /// + double X { get; } + + /// + /// Vị trí hiện tại của robot trong hệ tọa độ toàn cục theo trục Y (đơn vị: mét). + /// + double Y { get; } + + /// + /// Hướng hiện tại của robot trong hệ tọa độ toàn cục (đơn vị: độ). + /// + double Theta { get; } + + /// + /// Khởi tạo vị trí của robot trong hệ tọa độ toàn cục. + /// + /// đơn vị: mét + /// đơn vị: mét + /// đơn vị: độ + /// + bool InitializePosition(double x, double y, double theta); +} diff --git a/RobotApp/Interfaces/INavigation.cs b/RobotApp/Interfaces/INavigation.cs new file mode 100644 index 0000000..76a73c9 --- /dev/null +++ b/RobotApp/Interfaces/INavigation.cs @@ -0,0 +1,33 @@ +using RobotApp.VDA5050.Order; + +namespace RobotApp.Interfaces; + +public enum NavigationState +{ + Idle, + Moving, + Rotating, + Paused, + Error +} + +public enum NavigationProccess +{ + Started, + InProgress, + Completed, + Failed, + Cancelled +} + +public interface INavigation +{ + 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(); +} diff --git a/RobotApp/Interfaces/IOrder.cs b/RobotApp/Interfaces/IOrder.cs new file mode 100644 index 0000000..c2f27f4 --- /dev/null +++ b/RobotApp/Interfaces/IOrder.cs @@ -0,0 +1,15 @@ +using RobotApp.VDA5050.State; + +namespace RobotApp.Interfaces; + +public interface IOrder +{ + string OrderId { get; } + VDA5050.InstantAction.Action[] Actions { get; } + NodeState[] NodeStates { get; } + EdgeState[] EdgeStates { get; } + + bool StartOrder(); + bool UpdateOrder(); + bool StopOrder(); +} diff --git a/RobotApp/Interfaces/ISensorIMU.cs b/RobotApp/Interfaces/ISensorIMU.cs new file mode 100644 index 0000000..d070a4f --- /dev/null +++ b/RobotApp/Interfaces/ISensorIMU.cs @@ -0,0 +1,12 @@ +namespace RobotApp.Interfaces; + +/// +/// Interface cảm biến IMU +/// +public interface ISensorIMU +{ + /// + /// Góc xoay của robot (đơn vị độ) + /// + double Angle { get; } +} diff --git a/RobotApp/Program.cs b/RobotApp/Program.cs index 5bf0725..679289a 100644 --- a/RobotApp/Program.cs +++ b/RobotApp/Program.cs @@ -1,10 +1,10 @@ using Microsoft.AspNetCore.Components.Authorization; using Microsoft.AspNetCore.Identity; using Microsoft.EntityFrameworkCore; -using RobotApp.Client.Pages; using RobotApp.Components; using RobotApp.Components.Account; using RobotApp.Data; +using RobotApp.Services.Robot; var builder = WebApplication.CreateBuilder(args); @@ -27,8 +27,9 @@ builder.Services.AddAuthentication(options => .AddIdentityCookies(); var connectionString = builder.Configuration.GetConnectionString("DefaultConnection") ?? throw new InvalidOperationException("Connection string 'DefaultConnection' not found."); -builder.Services.AddDbContext(options => - options.UseSqlServer(connectionString)); +Action appDbOptions = options => options.UseSqlite(connectionString, b => b.MigrationsAssembly("RobotApp")); + +builder.Services.AddDbContext(appDbOptions); builder.Services.AddDatabaseDeveloperPageExceptionFilter(); builder.Services.AddIdentityCore(options => options.SignIn.RequireConfirmedAccount = true) @@ -38,6 +39,10 @@ builder.Services.AddIdentityCore(options => options.SignIn.Requ builder.Services.AddSingleton, IdentityNoOpEmailSender>(); +builder.Services.AddSingleton(typeof(RobotApp.Services.Logger<>)); +builder.Services.AddSingleton(); +builder.Services.AddHostedService(sp => sp.GetRequiredService()); + var app = builder.Build(); // Configure the HTTP request pipeline. diff --git a/RobotApp/RobotApp.csproj b/RobotApp/RobotApp.csproj index 3c36dac..ebfb1b0 100644 --- a/RobotApp/RobotApp.csproj +++ b/RobotApp/RobotApp.csproj @@ -9,11 +9,18 @@ - - - - - + + + + + + + + + all + runtime; build; native; contentfiles; analyzers; buildtransitive + + diff --git a/RobotApp/Services/Logger.cs b/RobotApp/Services/Logger.cs new file mode 100644 index 0000000..f5ed85f --- /dev/null +++ b/RobotApp/Services/Logger.cs @@ -0,0 +1,108 @@ +namespace RobotApp.Services; + +public class Logger(ILogger Logger) where T : class +{ + public event Action? LoggerUpdate; + public void Write(string message, LogLevel level) + { + switch (level) + { + case LogLevel.Trace: + Logger.LogTrace("{mes}", message); + break; + case LogLevel.Debug: + Logger.LogDebug("{mes}", message); + break; + case LogLevel.Information: + Logger.LogInformation("{mes}", message); + break; + case LogLevel.Warning: + Logger.LogWarning("{mes}", message); + break; + case LogLevel.Error: + Logger.LogError("{mes}", message); + break; + case LogLevel.Critical: + Logger.LogCritical("{mes}", message); + break; + } + LoggerUpdate?.Invoke(); + } + + public void Write(string message) + { + Write(message, LogLevel.Information); + } + + public async Task WriteAsync(string message) + { + var write = Task.Run(() => Write(message)); + await write.WaitAsync(CancellationToken.None); + } + + public async Task TraceAsync(string message) + { + var write = Task.Run(() => Write(message, LogLevel.Trace)); + await write.WaitAsync(CancellationToken.None); + } + + public async Task DebugAsync(string message) + { + var write = Task.Run(() => Write(message, LogLevel.Debug)); + await write.WaitAsync(CancellationToken.None); + } + + public async Task InfoAsync(string message) + { + var write = Task.Run(() => Write(message, LogLevel.Information)); + await write.WaitAsync(CancellationToken.None); + } + + public async Task WarningAsync(string message) + { + var write = Task.Run(() => Write(message, LogLevel.Warning)); + await write.WaitAsync(CancellationToken.None); + } + + public async Task ErrorAsync(string message) + { + var write = Task.Run(() => Write(message, LogLevel.Error)); + await write.WaitAsync(CancellationToken.None); + } + + public async Task CriticalAsync(string message) + { + var write = Task.Run(() => Write(message, LogLevel.Critical)); + await write.WaitAsync(CancellationToken.None); + } + + public void Trace(string message) + { + Write(message, LogLevel.Trace); + } + + public void Debug(string message) + { + Write(message, LogLevel.Debug); + } + + public void Info(string message) + { + Write(message, LogLevel.Information); + } + + public void Warning(string message) + { + Write(message, LogLevel.Warning); + } + + public void Error(string message) + { + Write(message, LogLevel.Error); + } + + public void Critical(string message) + { + Write(message, LogLevel.Critical); + } +} diff --git a/RobotApp/Services/MQTTClient.cs b/RobotApp/Services/MQTTClient.cs new file mode 100644 index 0000000..6415deb --- /dev/null +++ b/RobotApp/Services/MQTTClient.cs @@ -0,0 +1,161 @@ +using MQTTnet; +using MQTTnet.Protocol; +using RobotApp.Common.Shares; +using RobotApp.VDA5050; +using System.Text; + +namespace RobotApp.Services; + +public class MQTTClient : IAsyncDisposable +{ + private readonly MqttClientFactory MqttClientFactory; + private readonly MqttClientOptions MqttClientOptions; + private readonly MqttClientSubscribeOptions MqttClientSubscribeOptions; + private IMqttClient? MqttClient; + + private readonly Logger Logger; + private readonly VDA5050Setting VDA5050Setting; + private bool IsReconnecing; + + public event Action? OrderChanged; + public event Action? InstanceActionsChanged; + public bool IsConnected => !IsReconnecing && MqttClient is not null && MqttClient.IsConnected; + + public MQTTClient(string clientId, VDA5050Setting setting, Logger logger) + { + VDA5050Setting = setting; + Logger = logger; + + MqttClientFactory = new MqttClientFactory(); + MqttClient = MqttClientFactory.CreateMqttClient(); + MqttClientOptions = MqttClientFactory.CreateClientOptionsBuilder() + .WithTcpServer(setting.HostServer, setting.Port) + .WithCredentials(setting.UserName, setting.Password) + .WithClientId(clientId) + .WithCleanSession(true) + .Build(); + MqttClientSubscribeOptions = MqttClientFactory.CreateSubscribeOptionsBuilder() + .WithTopicFilter(f => f.WithTopic(VDA5050Topic.ORDER.ToTopicString())) + .WithTopicFilter(f => f.WithTopic(VDA5050Topic.INSTANTACTIONS.ToTopicString())) + .Build(); + 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(); + MqttClient.Dispose(); + + await ConnectAsync(); + await SubscribeAsync(); + 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}"); + else + { + Logger.Info("Kết nối tới broker thành công"); + break; + } + } + catch (Exception ex) + { + Logger.Error($"Lỗi khi tạo MQTT client: {ex.Message}"); + } + await Task.Delay(3000, cancellationToken); + } + } + + public async Task SubscribeAsync(CancellationToken cancellationToken = default) + { + if (MqttClient is null) throw new Exception("Kết nối tới broker chưa được khởi tạo nhưng đã yêu cầu subscribe"); + if(!MqttClient.IsConnected) throw new Exception("Kết nối tới broker chưa thành công nhưng đã yêu cầu subscribe"); + + MqttClient.ApplicationMessageReceivedAsync += delegate (MqttApplicationMessageReceivedEventArgs args) + { + var stringData = Encoding.UTF8.GetString(args.ApplicationMessage.Payload); + VDA5050Topic topic = EnumExtensions.ToTopic(args.ApplicationMessage.Topic); + if (topic == VDA5050Topic.ORDER) OrderChanged?.Invoke(stringData); + else if (topic == VDA5050Topic.INSTANTACTIONS) InstanceActionsChanged?.Invoke(stringData); + return Task.CompletedTask; + }; + + while (!cancellationToken.IsCancellationRequested) + { + try + { + var response = await MqttClient.SubscribeAsync(MqttClientSubscribeOptions, cancellationToken); + bool isSuccess = true; + foreach (var item in response.Items) + { + if (item.ResultCode == MqttClientSubscribeResultCode.GrantedQoS0 || + item.ResultCode == MqttClientSubscribeResultCode.GrantedQoS1 || + item.ResultCode == MqttClientSubscribeResultCode.GrantedQoS2) + { + Logger.Info($"Subscribe thành công cho topic: {item.TopicFilter.Topic} với QoS: {item.ResultCode}"); + } + else + { + Logger.Warning($"Subscribe thất bại cho topic: {item.TopicFilter.Topic}. Lý do: {response.ReasonString}"); + isSuccess = false; + break; + } + } + if (isSuccess) break; + } + catch (Exception ex) + { + Logger.Error($"Lỗi khi subscribe: {ex.Message}"); + } + await Task.Delay(3000, cancellationToken); + } + } + + public async Task PublishAsync(string topic, string data) + { + var repeat = VDA5050Setting.PublishRepeat; + while (repeat-- > 0) + { + try + { + var applicationMessage = new MqttApplicationMessageBuilder() + .WithTopic(topic) + .WithPayload(data) + .WithQualityOfServiceLevel(MqttQualityOfServiceLevel.AtLeastOnce) + .Build(); + if (MqttClient is null || !IsConnected) return new(false, "Chưa có kết nối tới broker"); + var publish = await MqttClient.PublishAsync(applicationMessage); + if (!publish.IsSuccess) continue; + return new(true); + } + catch (Exception ex) + { + Logger.Error($"Lỗi khi publish MQTT: {ex.Message}"); + } + } + return new(false, "Không thể publish tới broker"); + } + + public async ValueTask DisposeAsync() + { + if (MqttClient is not null) + { + if (MqttClient.IsConnected) await MqttClient.DisconnectAsync(); + MqttClient.Dispose(); + MqttClient = null; + } + GC.SuppressFinalize(this); + } +} diff --git a/RobotApp/Services/Navigation/Algorithm/FuzzyLogic.cs b/RobotApp/Services/Navigation/Algorithm/FuzzyLogic.cs new file mode 100644 index 0000000..b6bb273 --- /dev/null +++ b/RobotApp/Services/Navigation/Algorithm/FuzzyLogic.cs @@ -0,0 +1,252 @@ +namespace RobotApp.Services.Navigation.Algorithm; + +public class FuzzyLogic +{ + private double Gain_P = 0.5; + private double Gain_I = 0.01; + private double DiscreteTimeIntegrator_DSTATE; + + public FuzzyLogic WithGainP(double gainP) + { + Gain_P = gainP; + return this; + } + + public FuzzyLogic WithGainI(double gainI) + { + Gain_I = gainI; + return this; + } + + private static double Fuzzy_trapmf(double x, double[] parame) + { + double b_y1; + double y2; + b_y1 = 0.0; + y2 = 0.0; + if (x >= parame[1]) + { + b_y1 = 1.0; + } + if (x < parame[0]) + { + b_y1 = 0.0; + } + if (parame[0] <= x && x < parame[1] && parame[0] != parame[1]) + { + b_y1 = 1.0 / (parame[1] - parame[0]) * (x - parame[0]); + } + if (x <= parame[2]) + { + y2 = 1.0; + } + if (x > parame[3]) + { + y2 = 0.0; + } + if (parame[2] < x && x <= parame[3] && parame[2] != parame[3]) + { + y2 = 1.0 / (parame[3] - parame[2]) * (parame[3] - x); + } + return b_y1 < y2 ? b_y1 : y2; + } + + private static double Fuzzy_trimf(double x, double[] parame) + { + double y; + y = 0.0; + if (parame[0] != parame[1] && parame[0] < x && x < parame[1]) + { + y = 1.0 / (parame[1] - parame[0]) * (x - parame[0]); + } + if (parame[1] != parame[2] && parame[1] < x && x < parame[2]) + { + y = 1.0 / (parame[2] - parame[1]) * (parame[2] - x); + } + if (x == parame[1]) + { + y = 1.0; + } + return y; + } + + public (double wl, double wr) Fuzzy_step(double v, double w, double TimeSample) + { + (double wl, double wr) result = new(); + double[] inputMFCache = new double[10]; + double[] outputMFCache = new double[5]; + double[] outputMFCache_0 = new double[5]; + double[] tmp = new double[3]; + double aggregatedOutputs; + double rtb_TmpSignalConversionAtSFun_0; + double rtb_antecedentOutputs_e; + double sumAntecedentOutputs; + int ruleID; + double[] f = [-1.0E+10, -1.0E+10, -1.0, -0.5]; + double[] e = [0.5, 1.0, 1.0E+10, 1.0E+10]; + double[] d = [0.75, 1.0, 1.0E+9, 1.0E+9]; + double[] c = [-1.0E+9, -1.0E+9, 0.0, 0.25]; + byte[] b = [ 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 4, + 4, 4, 4, 4, 5, 5, 5, 5, 5, 1, 2, 3, 4, 5, 1, 2, 3, + 4, 5, 1, 2, 3, 4, 5, 3, 4, 5, 1, 2, 1, 2, 3, 4, 5 ]; + byte[] b_0 = [1, 1, 2, 1, 1, 2, 3, 5, 1, 4, 5, 5, 5, 5, 5, 2, 1, 1, 1, 1, 5, 5, 5, 5, 5]; + byte[] b_1 = [ 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 4, + 4, 4, 4, 4, 5, 5, 5, 5, 5, 1, 2, 3, 4, 5, 4, 1, + 2, 3, 5, 3, 1, 2, 4, 5, 1, 2, 3, 4, 5, 1, 2, 4, 5, 3 ]; + byte[] b_2 = [5, 5, 5, 5, 5, 1, 2, 3, 5, 4, 2, 1, 1, 1, 1, 5, 5, 5, 5, 5, 1, 1, 1, 1, 2]; + double inputMFCache_tmp; + double inputMFCache_tmp_0; + double inputMFCache_tmp_1; + double inputMFCache_tmp_2; + /* Outputs for Atomic SubSystem: '/Fuzzy Logic Controller1' */ + /* Outputs for Atomic SubSystem: '/Fuzzy Logic Controller' */ + /* SignalConversion generated from: '/ SFunction ' incorporates: + * Constant: '/w' + * DiscreteIntegrator: '/Discrete-Time Integrator' + * Gain: '/Gain1' + * MATLAB Function: '/Evaluate Rule Antecedents' + * MATLAB Function: '/Evaluate Rule Antecedents' + * SignalConversion generated from: '/ SFunction ' + * Sum: '/Sum' + */ + DiscreteTimeIntegrator_DSTATE += Gain_I * w * TimeSample; + rtb_TmpSignalConversionAtSFun_0 = Gain_P * w + DiscreteTimeIntegrator_DSTATE; + /* End of Outputs for SubSystem: '/Fuzzy Logic Controller1' */ + /* MATLAB Function: '/Evaluate Rule Antecedents' incorporates: + * Constant: '/v' + * MATLAB Function: '/Evaluate Rule Antecedents' + * SignalConversion generated from: '/ SFunction ' + */ + sumAntecedentOutputs = 0.0; + /* Outputs for Atomic SubSystem: '/Fuzzy Logic Controller1' */ + inputMFCache_tmp = Fuzzy_trapmf(rtb_TmpSignalConversionAtSFun_0, f); + /* End of Outputs for SubSystem: '/Fuzzy Logic Controller1' */ + inputMFCache[0] = inputMFCache_tmp; + tmp[0] = -0.5; + tmp[1] = 0.0; + tmp[2] = 0.5; + inputMFCache[1] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp); + /* Outputs for Atomic SubSystem: '/Fuzzy Logic Controller1' */ + inputMFCache_tmp_0 = Fuzzy_trapmf(rtb_TmpSignalConversionAtSFun_0, e); + /* End of Outputs for SubSystem: '/Fuzzy Logic Controller1' */ + inputMFCache[2] = inputMFCache_tmp_0; + tmp[0] = -1.0; + tmp[1] = -0.5; + tmp[2] = 0.0; + inputMFCache[3] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp); + tmp[0] = 0.0; + tmp[1] = 0.5; + tmp[2] = 1.0; + inputMFCache[4] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp); + tmp[0] = 0.0; + tmp[1] = 0.25; + tmp[2] = 0.5; + inputMFCache[5] = Fuzzy_trimf(v, tmp); + tmp[0] = 0.25; + tmp[1] = 0.5; + tmp[2] = 0.75; + inputMFCache[6] = Fuzzy_trimf(v, tmp); + /* Outputs for Atomic SubSystem: '/Fuzzy Logic Controller1' */ + inputMFCache_tmp_1 = Fuzzy_trapmf(v, d); + /* End of Outputs for SubSystem: '/Fuzzy Logic Controller1' */ + inputMFCache[7] = inputMFCache_tmp_1; + /* Outputs for Atomic SubSystem: '/Fuzzy Logic Controller1' */ + inputMFCache_tmp_2 = Fuzzy_trapmf(v, c); + /* End of Outputs for SubSystem: '/Fuzzy Logic Controller1' */ + inputMFCache[8] = inputMFCache_tmp_2; + tmp[0] = 0.5; + tmp[1] = 0.75; + tmp[2] = 1.0; + inputMFCache[9] = Fuzzy_trimf(v, tmp); + /* MATLAB Function: '/Evaluate Rule Consequents' */ + aggregatedOutputs = 0.0; + outputMFCache[0] = 0.0; + outputMFCache[1] = 0.25; + outputMFCache[2] = 0.5; + outputMFCache[3] = 0.75; + outputMFCache[4] = 1.0; + for (ruleID = 0; ruleID < 25; ruleID++) + { + /* MATLAB Function: '/Evaluate Rule Antecedents' */ + rtb_antecedentOutputs_e = inputMFCache[b[ruleID + 25] + 4] * inputMFCache[b[ruleID] - 1]; + sumAntecedentOutputs += rtb_antecedentOutputs_e; + /* MATLAB Function: '/Evaluate Rule Consequents' */ + aggregatedOutputs += outputMFCache[b_0[ruleID] - 1] * rtb_antecedentOutputs_e; + } + /* MATLAB Function: '/Defuzzify Outputs' incorporates: + * MATLAB Function: '/Evaluate Rule Antecedents' + * MATLAB Function: '/Evaluate Rule Consequents' + */ + if (sumAntecedentOutputs == 0.0) + { + result.wr = 0.5; + } + else + { + result.wr = 1.0 / sumAntecedentOutputs * aggregatedOutputs; + } + /* Outputs for Atomic SubSystem: '/Fuzzy Logic Controller1' */ + /* MATLAB Function: '/Evaluate Rule Antecedents' incorporates: + * Constant: '/v' + * SignalConversion generated from: '/ SFunction ' + */ + sumAntecedentOutputs = 0.0; + inputMFCache[0] = inputMFCache_tmp; + tmp[0] = -0.5; + tmp[1] = 0.0; + tmp[2] = 0.5; + inputMFCache[1] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp); + inputMFCache[2] = inputMFCache_tmp_0; + tmp[0] = -1.0; + tmp[1] = -0.5; + tmp[2] = 0.0; + inputMFCache[3] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp); + tmp[0] = 0.0; + tmp[1] = 0.5; + tmp[2] = 1.0; + inputMFCache[4] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp); + tmp[0] = 0.0; + tmp[1] = 0.25; + tmp[2] = 0.5; + inputMFCache[5] = Fuzzy_trimf(v, tmp); + tmp[0] = 0.25; + tmp[1] = 0.5; + tmp[2] = 0.75; + inputMFCache[6] = Fuzzy_trimf(v, tmp); + inputMFCache[7] = inputMFCache_tmp_1; + inputMFCache[8] = inputMFCache_tmp_2; + tmp[0] = 0.5; + tmp[1] = 0.75; + tmp[2] = 1.0; + inputMFCache[9] = Fuzzy_trimf(v, tmp); + /* MATLAB Function: '/Evaluate Rule Consequents' */ + aggregatedOutputs = 0.0; + outputMFCache_0[0] = 0.0; + outputMFCache_0[1] = 0.25; + outputMFCache_0[2] = 0.5; + outputMFCache_0[3] = 0.75; + outputMFCache_0[4] = 1.0; + for (ruleID = 0; ruleID < 25; ruleID++) + { + /* MATLAB Function: '/Evaluate Rule Antecedents' */ + rtb_antecedentOutputs_e = inputMFCache[b_1[ruleID + 25] + 4] * inputMFCache[b_1[ruleID] - 1]; + sumAntecedentOutputs += rtb_antecedentOutputs_e; + /* MATLAB Function: '/Evaluate Rule Consequents' */ + aggregatedOutputs += outputMFCache_0[b_2[ruleID] - 1] * + rtb_antecedentOutputs_e; + } + /* MATLAB Function: '/Defuzzify Outputs' incorporates: + * MATLAB Function: '/Evaluate Rule Antecedents' + * MATLAB Function: '/Evaluate Rule Consequents' + */ + if (sumAntecedentOutputs == 0.0) + { + result.wl = 0.5; + } + else + { + result.wl = 1.0 / sumAntecedentOutputs * aggregatedOutputs; + } + return result; + } +} diff --git a/RobotApp/Services/Navigation/Algorithm/PID.cs b/RobotApp/Services/Navigation/Algorithm/PID.cs new file mode 100644 index 0000000..e74333c --- /dev/null +++ b/RobotApp/Services/Navigation/Algorithm/PID.cs @@ -0,0 +1,47 @@ +namespace RobotApp.Services.Navigation.Algorithm; + +public class PID +{ + private double Kp = 0.3; + private double Ki = 0.0001; + private double Kd = 0.01; + private double Pre_Error; + private double Pre_Pre_Error; + private double Pre_Out; + + private DateTime PreTime; + + public PID WithKp(double kp) + { + Kp = kp; + return this; + } + + public PID WithKi(double ki) + { + Ki = ki; + return this; + } + + public PID WithKd(double kd) + { + Kd = kd; + return this; + } + + public double PID_step(double error, double min, double max) + { + DateTime CurrentTime = DateTime.Now; + double TimeSample = (CurrentTime - PreTime).TotalSeconds; + double P_part = Kp * (error - Pre_Error); + double I_part = 0.5 * Ki * TimeSample * (error + Pre_Error); + double D_part = Kd / TimeSample * (error - 2 * Pre_Error + Pre_Pre_Error); + double Out = Pre_Out + P_part + I_part + D_part; + Pre_Pre_Error = Pre_Error; + Pre_Error = error; + Pre_Out = Out; + Out = Math.Clamp(Out, min, max); + PreTime = CurrentTime; + return Out; + } +} diff --git a/RobotApp/Services/Navigation/Algorithm/PurePursuit.cs b/RobotApp/Services/Navigation/Algorithm/PurePursuit.cs new file mode 100644 index 0000000..5901346 --- /dev/null +++ b/RobotApp/Services/Navigation/Algorithm/PurePursuit.cs @@ -0,0 +1,156 @@ +//namespace RobotApp.Services.Navigation.Algorithm; +//public class PurePursuit +//{ +// private double MaxAngularVelocity = 1.5; +// private double LookaheadDistance = 0.5; +// private List Waypoints_Value = []; +// public int OnNodeIndex = 0; +// public int GoalIndex = 0; +// public NavigationNode? Goal; + +// public PurePursuit WithLookheadDistance(double distance) +// { +// LookaheadDistance = distance; +// return this; +// } + +// public PurePursuit WithMaxAngularVelocity(double vel) +// { +// MaxAngularVelocity = vel; +// return this; +// } + +// public PurePursuit WithPath(NavigationNode[] path) +// { +// Waypoints_Value = [.. path]; +// return this; +// } + +// public void UpdatePath(NavigationNode[] path) +// { +// Waypoints_Value = [.. path]; +// } + +// public void UpdateGoal(NavigationNode goal) +// { +// Goal = goal; +// GoalIndex = Waypoints_Value.IndexOf(Goal); +// } + +// public (NavigationNode node, int index) GetOnNode(double x, double y) +// { +// double minDistance = double.MaxValue; +// NavigationNode onNode = Waypoints_Value[0]; +// int index = 0; +// for (int i = 1; i < Waypoints_Value.Count; i++) +// { +// var distance = Math.Sqrt(Math.Pow(x - Waypoints_Value[i].X, 2) + Math.Pow(y - Waypoints_Value[i].Y, 2)); +// if (distance < minDistance) +// { +// onNode = Waypoints_Value[i]; +// minDistance = distance; +// index = i; +// } +// } +// return (onNode, index); +// } + +// private (NavigationNode? node, int index) OnNode(double x, double y) +// { +// if (Waypoints_Value is null || Waypoints_Value.Count == 0) return (null, 0); +// double minDistance = double.MaxValue; +// int index = 0; +// NavigationNode? onNode = null; +// if (Goal is null) return (onNode, index); +// for (int i = OnNodeIndex; i < Waypoints_Value.IndexOf(Goal); i++) +// { +// var distance = Math.Sqrt(Math.Pow(x - Waypoints_Value[i].X, 2) + Math.Pow(y - Waypoints_Value[i].Y, 2)); +// if (distance < minDistance) +// { +// onNode = Waypoints_Value[i]; +// minDistance = distance; +// index = i; +// } +// } +// return (onNode, index); +// } + +// public double PurePursuit_step(double X_Ref, double Y_Ref, double Angle_Ref) +// { +// if (Waypoints_Value is null || Waypoints_Value.Count < 2) return 0; +// NavigationNode? lookaheadStartPt = null; +// var (onNode, index) = OnNode(X_Ref, Y_Ref); +// if (onNode is null || Goal is null) return 0; +// OnNodeIndex = index; +// double lookDistance = 0; +// for (int i = OnNodeIndex + 1; i < Waypoints_Value.IndexOf(Goal); i++) +// { +// lookDistance += Math.Sqrt(Math.Pow(Waypoints_Value[i - 1].X - Waypoints_Value[i].X, 2) + Math.Pow(Waypoints_Value[i - 1].Y - Waypoints_Value[i].Y, 2)); +// if (lookDistance >= LookaheadDistance || Waypoints_Value[i].Direction != onNode.Direction) +// { +// lookaheadStartPt = Waypoints_Value[i]; +// break; +// } +// } +// lookaheadStartPt ??= Goal; +// if (onNode.Direction == RobotDirection.BACKWARD) +// { +// if (Angle_Ref > Math.PI) Angle_Ref -= Math.PI * 2; +// else if (Angle_Ref < -Math.PI) Angle_Ref += Math.PI * 2; +// Angle_Ref += Math.PI; +// if (Angle_Ref > Math.PI) Angle_Ref -= Math.PI * 2; +// } +// var distance = Math.Atan2(lookaheadStartPt.Y - Y_Ref, lookaheadStartPt.X - X_Ref) - Angle_Ref; + +// if (Math.Abs(distance) > Math.PI) +// { +// double minDistance; +// if (distance + Math.PI == 0.0) minDistance = 0.0; +// else +// { +// double data = (distance + Math.PI) / (2 * Math.PI); +// if (data < 0) data = Math.Round(data + 0.5); +// else data = Math.Round(data - 0.5); +// minDistance = distance + Math.PI - data * (2 * Math.PI); +// double checker = 0; +// if (minDistance != 0.0) +// { +// checker = Math.Abs((distance + Math.PI) / (2 * Math.PI)); +// } +// if (!(Math.Abs(checker - Math.Floor(checker + 0.5)) > 2.2204460492503131E-16 * checker)) +// { +// minDistance = 0.0; +// } +// else if (distance + Math.PI < 0.0) +// { +// minDistance += Math.PI * 2; +// } +// } +// if (minDistance == 0.0 && distance + Math.PI > 0.0) +// { +// minDistance = Math.PI * 2; +// } +// distance = minDistance - Math.PI; +// } + +// var AngularVelocity = 2.0 * 0.5 * Math.Sin(distance) / LookaheadDistance; +// if (Math.Abs(AngularVelocity) > MaxAngularVelocity) +// { +// if (AngularVelocity < 0.0) +// { +// AngularVelocity = -1.0; +// } +// else if (AngularVelocity > 0.0) +// { +// AngularVelocity = 1.0; +// } +// else if (AngularVelocity == 0.0) +// { +// AngularVelocity = 0.0; +// } +// AngularVelocity *= MaxAngularVelocity; +// } +// return AngularVelocity; +// } + +//} diff --git a/RobotApp/Services/Navigation/NavigationController.cs b/RobotApp/Services/Navigation/NavigationController.cs new file mode 100644 index 0000000..9da57cf --- /dev/null +++ b/RobotApp/Services/Navigation/NavigationController.cs @@ -0,0 +1,5 @@ +namespace RobotApp.Services.Navigation; + +public class NavigationController +{ +} diff --git a/RobotApp/Services/Navigation/NavigationManager.cs b/RobotApp/Services/Navigation/NavigationManager.cs new file mode 100644 index 0000000..7575190 --- /dev/null +++ b/RobotApp/Services/Navigation/NavigationManager.cs @@ -0,0 +1,6 @@ +namespace RobotApp.Services.Navigation +{ + public class NavigationManager + { + } +} diff --git a/RobotApp/Services/Robot/RobotAction.cs b/RobotApp/Services/Robot/RobotAction.cs new file mode 100644 index 0000000..0c280d3 --- /dev/null +++ b/RobotApp/Services/Robot/RobotAction.cs @@ -0,0 +1,6 @@ +namespace RobotApp.Services.Robot +{ + public class RobotAction + { + } +} diff --git a/RobotApp/Services/Robot/RobotConfiguration.cs b/RobotApp/Services/Robot/RobotConfiguration.cs new file mode 100644 index 0000000..97aa1bb --- /dev/null +++ b/RobotApp/Services/Robot/RobotConfiguration.cs @@ -0,0 +1,9 @@ +using RobotApp.Common.Shares.Enums; + +namespace RobotApp.Services.Robot; + +public class RobotConfiguration +{ + public static string SerialNumber { get; set; } = "Robot-Demo"; + public static NavigationType NavigationType { get; set; } = NavigationType.Differential; +} diff --git a/RobotApp/Services/Robot/RobotConnection.cs b/RobotApp/Services/Robot/RobotConnection.cs new file mode 100644 index 0000000..5befe24 --- /dev/null +++ b/RobotApp/Services/Robot/RobotConnection.cs @@ -0,0 +1,69 @@ +using RobotApp.Common.Shares; +using RobotApp.VDA5050; +using RobotApp.VDA5050.InstantAction; +using RobotApp.VDA5050.Order; +using System.Text.Json; + +namespace RobotApp.Services.Robot; + +public class RobotConnection(Logger Logger, Logger MQTTClientLogger) : BackgroundService +{ + private readonly VDA5050Setting VDA5050Setting = new() + { + HostServer = "172.20.235.170", + Port = 1886, + }; + private MQTTClient? MqttClient; + + public bool IsConnected => MqttClient is not null && MqttClient.IsConnected; + public readonly static string SerialNumber = "RobotDemo"; + + private void OrderChanged(string data) + { + try + { + var msg = JsonSerializer.Deserialize(data); + if (msg is null || string.IsNullOrEmpty(msg.SerialNumber) || msg.SerialNumber != SerialNumber) return; + } + catch (Exception ex) + { + Logger.Warning($"Nhận Order xảy ra lỗi: {ex.Message} - {ex.StackTrace}"); + } + } + + private void InstanceActionsChanged(string data) + { + try + { + var msg = JsonSerializer.Deserialize(data); + if (msg is null || string.IsNullOrEmpty(msg.SerialNumber) || msg.SerialNumber != SerialNumber) return; + } + catch (Exception ex) + { + Logger.Warning($"Nhận InstanceActions xảy ra lỗi: {ex.Message} - {ex.StackTrace}"); + } + } + + public async Task Publish(string topic, string data) + { + if (MqttClient is not null && MqttClient.IsConnected) return await MqttClient.PublishAsync(topic, data); + return new(false, "Chưa có kết nối tới broker"); + } + + protected override async Task ExecuteAsync(CancellationToken stoppingToken) + { + await Task.Yield(); + + MqttClient = new MQTTClient(SerialNumber, VDA5050Setting, MQTTClientLogger); + MqttClient.OrderChanged += OrderChanged; + MqttClient.InstanceActionsChanged += InstanceActionsChanged; + await MqttClient.ConnectAsync(stoppingToken); + await MqttClient.SubscribeAsync(stoppingToken); + } + + public override async Task StopAsync(CancellationToken cancellationToken) + { + if(MqttClient is not null) await MqttClient.DisposeAsync(); + await base.StopAsync(cancellationToken); + } +} diff --git a/RobotApp/Services/Robot/RobotController.cs b/RobotApp/Services/Robot/RobotController.cs new file mode 100644 index 0000000..482457c --- /dev/null +++ b/RobotApp/Services/Robot/RobotController.cs @@ -0,0 +1,6 @@ +namespace RobotApp.Services.Robot +{ + public class RobotController + { + } +} diff --git a/RobotApp/Services/Robot/RobotFactsheet.cs b/RobotApp/Services/Robot/RobotFactsheet.cs new file mode 100644 index 0000000..07ac2c7 --- /dev/null +++ b/RobotApp/Services/Robot/RobotFactsheet.cs @@ -0,0 +1,34 @@ + +using RobotApp.Common.Shares; +using RobotApp.VDA5050; +using RobotApp.VDA5050.Factsheet; +using System.Text.Json; + +namespace RobotApp.Services.Robot; + +public class RobotFactsheet(RobotConnection RobotConnection) : BackgroundService +{ + public async Task PubFactsheet() + { + if (!RobotConnection.IsConnected) return; + FactSheetMsg factSheet = new() + { + SerialNumber = RobotConnection.SerialNumber, + ProtocolFeatures = new() + { + AgvActions = [], + } + }; + string factSheetJson = JsonSerializer.Serialize(factSheet, JsonOptionExtends.Write); + await RobotConnection.Publish(VDA5050Topic.FACTSHEET.ToTopicString(), factSheetJson); + } + protected override async Task ExecuteAsync(CancellationToken stoppingToken) + { + while(!stoppingToken.IsCancellationRequested) + { + await Task.Delay(1000, stoppingToken); + if (RobotConnection.IsConnected) break; + } + + } +} diff --git a/RobotApp/Services/Robot/RobotOrderController.cs b/RobotApp/Services/Robot/RobotOrderController.cs new file mode 100644 index 0000000..12a5dcf --- /dev/null +++ b/RobotApp/Services/Robot/RobotOrderController.cs @@ -0,0 +1,30 @@ +using RobotApp.Interfaces; +using RobotApp.VDA5050.State; + +namespace RobotApp.Services.Robot; + +public class RobotOrderController : IOrder +{ + public string OrderId => throw new NotImplementedException(); + + public VDA5050.InstantAction.Action[] Actions => throw new NotImplementedException(); + + public NodeState[] NodeStates => throw new NotImplementedException(); + + public EdgeState[] EdgeStates => throw new NotImplementedException(); + + public bool StartOrder() + { + throw new NotImplementedException(); + } + + public bool UpdateOrder() + { + throw new NotImplementedException(); + } + + public bool StopOrder() + { + throw new NotImplementedException(); + } +} diff --git a/RobotApp/Services/Robot/RobotVisualization.cs b/RobotApp/Services/Robot/RobotVisualization.cs new file mode 100644 index 0000000..e2de5c6 --- /dev/null +++ b/RobotApp/Services/Robot/RobotVisualization.cs @@ -0,0 +1,6 @@ +namespace RobotApp.Services.Robot +{ + public class RobotVisualization + { + } +} diff --git a/RobotApp/Services/WatchTimer.cs b/RobotApp/Services/WatchTimer.cs new file mode 100644 index 0000000..08ff331 --- /dev/null +++ b/RobotApp/Services/WatchTimer.cs @@ -0,0 +1,76 @@ +using System.Diagnostics; + +namespace RobotApp.Services; + +public class WatchTimer(int Interval, Action Callback, Logger? Logger) : IDisposable where T : class +{ + private Timer? Timer; + private readonly Stopwatch Watch = new(); + public bool Disposed; + + private void Handler(object? state) + { + try + { + Watch.Restart(); + + Callback.Invoke(); + + Watch.Stop(); + if (Watch.ElapsedMilliseconds >= Interval || Interval - Watch.ElapsedMilliseconds <= 50) + { + Timer?.Change(Interval, Timeout.Infinite); + } + else + { + Timer?.Change(Interval - Watch.ElapsedMilliseconds, Timeout.Infinite); + } + } + catch (Exception ex) + { + Logger?.Error($"WatchTimer Error: {ex}"); + Timer?.Change(Interval, Timeout.Infinite); + } + } + + public void Start() + { + if (!Disposed) + { + Timer = new Timer(Handler, null, Timeout.Infinite, Timeout.Infinite); + Timer.Change(Interval, 0); + } + else throw new ObjectDisposedException(nameof(WatchTimer)); + } + + public void Stop() + { + if (Disposed) return; + if (Timer != null) + { + Timer.Change(Timeout.Infinite, Timeout.Infinite); + Timer.Dispose(); + Timer = null; + } + } + + public void Dispose() + { + Dispose(true); + GC.SuppressFinalize(this); + } + + protected virtual void Dispose(bool disposing) + { + if (Disposed) return; + + if (disposing) Stop(); + + Disposed = true; + } + + ~WatchTimer() + { + Dispose(false); + } +} diff --git a/RobotApp/Services/WatchTimerAsync.cs b/RobotApp/Services/WatchTimerAsync.cs new file mode 100644 index 0000000..b5da39e --- /dev/null +++ b/RobotApp/Services/WatchTimerAsync.cs @@ -0,0 +1,76 @@ +using System.Diagnostics; + +namespace RobotApp.Services; + +public class WatchTimerAsync(int Interval, Func Callback, Logger? Logger) : IDisposable where T : class +{ + private Timer? Timer; + private readonly Stopwatch Watch = new(); + public bool Disposed; + + private async void Handler(object? state) + { + try + { + Watch.Restart(); + + await Callback.Invoke(); + + Watch.Stop(); + if (Watch.ElapsedMilliseconds >= Interval || Interval - Watch.ElapsedMilliseconds <= 50) + { + Timer?.Change(Interval, Timeout.Infinite); + } + else + { + Timer?.Change(Interval - Watch.ElapsedMilliseconds, Timeout.Infinite); + } + } + catch (Exception ex) + { + Logger?.Error($"WatchTimerAsync Error: {ex}"); + Timer?.Change(Interval, Timeout.Infinite); + } + } + + public void Start() + { + if (!Disposed) + { + Timer = new Timer(Handler, null, Timeout.Infinite, Timeout.Infinite); + Timer.Change(Interval, 0); + } + else throw new ObjectDisposedException(nameof(WatchTimerAsync)); + } + + public void Stop() + { + if (Disposed) return; + if (Timer != null) + { + Timer.Change(Timeout.Infinite, Timeout.Infinite); + Timer.Dispose(); + Timer = null; + } + } + + public void Dispose() + { + Dispose(true); + GC.SuppressFinalize(this); + } + + protected virtual void Dispose(bool disposing) + { + if (Disposed) return; + + if (disposing) Stop(); + + Disposed = true; + } + + ~WatchTimerAsync() + { + Dispose(false); + } +} diff --git a/RobotApp/appsettings.json b/RobotApp/appsettings.json index 3b09a76..e36d9c8 100644 --- a/RobotApp/appsettings.json +++ b/RobotApp/appsettings.json @@ -1,6 +1,6 @@ { "ConnectionStrings": { - "DefaultConnection": "Server=(localdb)\\mssqllocaldb;Database=aspnet-RobotApp-1f61caa2-bbbb-40cd-88b6-409b408a84ea;Trusted_Connection=True;MultipleActiveResultSets=true" + "DefaultConnection": "Data Source=robot.db" }, "Logging": { "LogLevel": {