diff --git a/RobotApp.Client/Pages/Components/Mapping/MapView.razor.css b/RobotApp.Client/Pages/Components/Mapping/MapView.razor.css index deafb45..974d558 100644 --- a/RobotApp.Client/Pages/Components/Mapping/MapView.razor.css +++ b/RobotApp.Client/Pages/Components/Mapping/MapView.razor.css @@ -5,7 +5,7 @@ display: flex; flex-direction: column; position: relative; - background-color: var(--mud-palette-surface); + background-color: #808080; border-radius: var(--mud-default-borderradius); transition: box-shadow 300ms cubic-bezier(0.4, 0, 0.2, 1) 0ms; box-shadow: var(--mud-elevation-10); diff --git a/RobotApp.Common.Shares/Enums/StateType.cs b/RobotApp.Common.Shares/Enums/StateType.cs index c9b3b75..84b9b98 100644 --- a/RobotApp.Common.Shares/Enums/StateType.cs +++ b/RobotApp.Common.Shares/Enums/StateType.cs @@ -2,22 +2,29 @@ public enum RootStateType { - Booting, - Operational, + System, + Auto, + Manual, + Service, + Stop, + Fault, } -public enum OperationalStateType +public enum SystemStateType { - + Initializing, + Standby, + Shutting_Down, } -public enum AutomationStateType +public enum AutoStateType { Idle, Executing, Paused, - Charging, - Error, + Holding, + Canceling, + Recovering, Remote_Override, } @@ -27,13 +34,60 @@ public enum ManualStateType Active, } -public enum SafetyStateType +public enum ServiceStateType { - Init, - Run_Ok, - SS1, - STO, - PDS, - SLS, - Error, + Idle, + Active, +} + +public enum StopStateType +{ + EMC, + Protective, + Manual, +} + +public enum FaultStateType +{ + Navigation, + Localization, + Shielf, + Battery, + Driver, + Peripherals, + Safety, + Communication, +} + +public enum ExecutingStateType +{ + Planning, + Moving, + ACT, +} + +public enum ACTStateType +{ + Docking, + Docked, + Charging, + Undocking, + Loading, + Unloading, + TechAction, +} + +public enum MoveStateType +{ + Navigation, + Avoidance, + Approach, + Tracking, + Repositioning, +} + +public enum PlanStateType +{ + Task, + Path, } \ No newline at end of file diff --git a/RobotApp.Common.Shares/MathExtensions.cs b/RobotApp.Common.Shares/MathExtensions.cs index 54d57fc..6122d33 100644 --- a/RobotApp.Common.Shares/MathExtensions.cs +++ b/RobotApp.Common.Shares/MathExtensions.cs @@ -4,6 +4,14 @@ namespace RobotApp.Common.Shares; public static class MathExtensions { + public static double NormalizeAngle(double angle) + { + angle = angle % 360; + if (angle > 180) angle -= 360; + else if (angle < -180) angle += 360; + return angle; + } + 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; @@ -77,4 +85,21 @@ public static class MathExtensions } return distance; } + + public static double GetVectorAngle(double originNodeX, double originNodeY, double vector1X, double vector1Y, double vector2X, double vector2Y) + { + double BA_x = vector1X - originNodeX; + double BA_y = vector1Y - originNodeY; + double BC_x = vector2X - originNodeX; + double BC_y = vector2Y - originNodeY; + // Tính độ dài của các vector AB và BC + double lengthAB = Math.Sqrt(BA_x * BA_x + BA_y * BA_y); + double lengthBC = Math.Sqrt(BC_x * BC_x + BC_y * BC_y); + // Tính tích vô hướng của AB và BC + double dotProduct = BA_x * BC_x + BA_y * BC_y; + if (lengthAB * lengthBC == 0) return 0; + if (dotProduct / (lengthAB * lengthBC) > 1) return 0; + if (dotProduct / (lengthAB * lengthBC) < -1) return 180; + return Math.Acos(dotProduct / (lengthAB * lengthBC)) * (180.0 / Math.PI); + } } diff --git a/RobotApp/Interfaces/IBattery.cs b/RobotApp/Interfaces/IBattery.cs index bcb2b29..4c29575 100644 --- a/RobotApp/Interfaces/IBattery.cs +++ b/RobotApp/Interfaces/IBattery.cs @@ -1,5 +1,31 @@ namespace RobotApp.Interfaces; +public enum BatteryStatus +{ + Unknown, + Charging, + Discharging, + Full, + NotCharging, + Fault, + Standby +} + public interface IBattery { + bool IsReady { get; } + double Voltage { get; } + double Current { get; } + double SOC { get; } + double SOH { get; } + BatteryStatus Status { get; } + int ChargeTime { get; } + int DischargeTime { get; } + double Temperature { get; } + double RemainingCapacity { get; } + double RemainingEnergy { get; } + int DischargeCycles { get; } + int ChargeCycles { get; } + bool IsCharging { get; } + string[] ErrorStatus { get; } } diff --git a/RobotApp/Interfaces/IError.cs b/RobotApp/Interfaces/IError.cs index ba09ab1..17db062 100644 --- a/RobotApp/Interfaces/IError.cs +++ b/RobotApp/Interfaces/IError.cs @@ -6,4 +6,6 @@ public interface IError { bool HasFatalError { get; } void AddError(Error error, TimeSpan? clearAfter = null); + void DeleteError(string errorType); + void ClearAllErrors(); } diff --git a/RobotApp/Interfaces/IInfomation.cs b/RobotApp/Interfaces/IInfomation.cs index bd79f4a..8563825 100644 --- a/RobotApp/Interfaces/IInfomation.cs +++ b/RobotApp/Interfaces/IInfomation.cs @@ -2,4 +2,5 @@ public interface IInfomation { + } diff --git a/RobotApp/Interfaces/IInstanceActions.cs b/RobotApp/Interfaces/IInstantActions.cs similarity index 91% rename from RobotApp/Interfaces/IInstanceActions.cs rename to RobotApp/Interfaces/IInstantActions.cs index 87f9825..6e6ab98 100644 --- a/RobotApp/Interfaces/IInstanceActions.cs +++ b/RobotApp/Interfaces/IInstantActions.cs @@ -3,7 +3,7 @@ using Action = RobotApp.VDA5050.InstantAction.Action; namespace RobotApp.Interfaces; -public interface IInstanceActions +public interface IInstantActions { ActionState[] ActionStates { get; } bool HasActionRunning { get; } diff --git a/RobotApp/Interfaces/ILocalization.cs b/RobotApp/Interfaces/ILocalization.cs index 847ee4a..9a22d40 100644 --- a/RobotApp/Interfaces/ILocalization.cs +++ b/RobotApp/Interfaces/ILocalization.cs @@ -1,7 +1,14 @@ -namespace RobotApp.Interfaces; +using RobotApp.Common.Shares; + +namespace RobotApp.Interfaces; public interface ILocalization { + /// + /// Trạng thái sẵn sàng của hệ thống định vị. + /// + bool IsReady { get; } + /// /// 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). /// @@ -13,10 +20,36 @@ public interface ILocalization double Y { get; } /// - /// Hướng hiện tại của robot trong hệ tọa độ toàn cục (đơn vị: độ). + /// Hướng hiện tại của robot trong hệ tọa độ toàn cục (đơn vị: radian). /// double Theta { get; } + /// + /// Trang thái SLAM hiện tại của robot. + /// + string SlamState { get; } + + /// + /// Thông tin chi tiết về trạng thái SLAM hiện tại của robot. + /// + string SlamStateDetail { get; } + + /// + /// Bản đồ đang được sử dụng để định vị hiện tại. + /// + string CurrentActiveMap { get; } + + + /// + /// Độ tin cậy của vị trí định vị hiện tại (0.0 - 100.0). + /// + double Reliability { get; } + + /// + /// Độ phù hợp của vị trí định vị hiện tại so với bản đồ (0.0 - 100.0). + /// + double MatchingScore { get; } + /// /// Khởi tạo vị trí của robot trong hệ tọa độ toàn cục. /// @@ -24,5 +57,57 @@ public interface ILocalization /// đơn vị: mét /// đơn vị: độ /// - bool InitializePosition(double x, double y, double theta); + MessageResult SetInitializePosition(double x, double y, double theta); + + /// + /// Bắt đầu quá trình lập bản đồ với độ phân giải chỉ định. + /// + /// đơn vị mét/px + /// + MessageResult StartMapping(double resolution); + + /// + /// Dừng quá trình lập bản đồ hiện tại và lưu bản đồ với tên chỉ định. + /// + /// + /// + MessageResult StopMapping(string mapName); + + /// + /// Bắt đầu quá trình định vị sử dụng bản đồ đã lưu. + /// + /// + MessageResult StartLocalization(); + + /// + /// Dừng quá trình định vị hiện tại. + /// + /// + MessageResult StopLocalization(); + + /// + /// Kích hoạt bản đồ đã lưu để sử dụng trong quá trình định vị. + /// + /// + /// + MessageResult ActivateMap(string mapName); + + /// + /// Chuyển sang bản đồ đã lưu và khởi tạo vị trí của robot trên bản đồ đó. + /// + /// + /// + /// + /// + /// + MessageResult SwitchMap(string mapName, double x, double y, double theta); + + /// + /// Thay đổi gốc tọa độ của bản đồ hiện tại. + /// + /// + /// + /// + /// + MessageResult ChangeMapOrigin(double x, double y, double theta); } diff --git a/RobotApp/Interfaces/INavigation.cs b/RobotApp/Interfaces/INavigation.cs index 3e09fd1..8e5ff03 100644 --- a/RobotApp/Interfaces/INavigation.cs +++ b/RobotApp/Interfaces/INavigation.cs @@ -5,11 +5,12 @@ namespace RobotApp.Interfaces; public enum NavigationState { None, - Initializing, - Initialized, Idle, + Initializing, + Waiting, Moving, Rotating, + Canceled, Paused, Error } @@ -25,13 +26,19 @@ public enum NavigationProccess public interface INavigation { + bool IsReady { get; } bool Driving { get; } + double VelocityX { get; } + double VelocityY { get; } + double Omega { get; } NavigationState State { get; } 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 UpdateOrder(string lastBaseNodeId); + void RefreshOrder(Node[] nodes, Edge[] edges); + void Refresh(); void CancelMovement(); } diff --git a/RobotApp/Interfaces/IPeripheral.cs b/RobotApp/Interfaces/IPeripheral.cs index d8bb49a..1290792 100644 --- a/RobotApp/Interfaces/IPeripheral.cs +++ b/RobotApp/Interfaces/IPeripheral.cs @@ -1,14 +1,35 @@ namespace RobotApp.Interfaces; -public enum OperatingMode +public enum PeripheralMode { AUTOMATIC, MANUAL, - SEMIAUTOMATIC, - TEACHIN, SERVICE, } public interface IPeripheral { - OperatingMode OperatingMode { get; } + bool IsReady { get; } + PeripheralMode PeripheralMode { get; } + bool Emergency { get; } + bool Bumper { get; } + bool LiftedUp { get; } + bool LiftedDown { get; } + bool LiftHome { get; } + bool LeftMotorReady { get; } + bool RightMotorReady { get; } + bool LiftMotorReady { get; } + bool SwitchLock { get; } + bool SwitchManual { get; } + bool SwitchAuto { get; } + bool ButtonStart { get; } + bool ButtonStop { get; } + bool ButtonReset { get; } + bool HasLoad { get; } + bool MutedBase { get; } + bool MutedLoad { get; } + bool StartedCharging { get; } + + bool SetSytemState(); + bool SetProccessState(); + bool SetOnCharging(); } diff --git a/RobotApp/Interfaces/IRFHandler.cs b/RobotApp/Interfaces/IRFHandler.cs index d5d6d7c..1052920 100644 --- a/RobotApp/Interfaces/IRFHandler.cs +++ b/RobotApp/Interfaces/IRFHandler.cs @@ -1,6 +1,29 @@ -namespace RobotApp.Interfaces +namespace RobotApp.Interfaces; + +public enum RFHandlerMode { - public class IRFHandler - { - } + Default, + Maintenance, + Override, + Unknown +} + +public interface IRFHandler +{ + bool IsConnected { get; } + bool IsEnabled { get; } + bool IsLocked { get; } + bool EStop { get; } + bool LiftUp { get; } + bool LiftDown { get; } + int Speed { get; } + bool Forward { get; } + bool Backward { get; } + bool RotateLeft { get; } + bool RotateRight { get; } + bool Left { get; } + bool Right { get; } + RFHandlerMode Mode { get; } + event Action OnStateChanged; + event Action? OnConnectionChanged; } diff --git a/RobotApp/Interfaces/ISafety.cs b/RobotApp/Interfaces/ISafety.cs index b92ef96..8351a4e 100644 --- a/RobotApp/Interfaces/ISafety.cs +++ b/RobotApp/Interfaces/ISafety.cs @@ -2,4 +2,17 @@ public interface ISafety { + bool SpeedVerySlow { get; } + bool SpeedSlow { get; } + bool SpeedNormal { get; } + bool SpeedMedium { get; } + bool SpeedOptimal { get; } + bool SpeedFast { get; } + bool SpeedVeryFast { get; } + bool LidarFrontProtectField { get; } + bool LidarBackProtectField { get; } + bool LidarFrontTimProtectField { get; } + bool SetMutedLoad(bool muted); + bool SetMutedBase(bool muted); + bool SetHorizontalLoad(); } diff --git a/RobotApp/Interfaces/ISensorIMU.cs b/RobotApp/Interfaces/ISensorIMU.cs index d070a4f..68a6b92 100644 --- a/RobotApp/Interfaces/ISensorIMU.cs +++ b/RobotApp/Interfaces/ISensorIMU.cs @@ -5,6 +5,11 @@ /// public interface ISensorIMU { + /// + /// Trạng thái sẵn sàng của cảm biến IMU + /// + bool IsReady { get; } + /// /// Góc xoay của robot (đơn vị độ) /// diff --git a/RobotApp/Program.cs b/RobotApp/Program.cs index fa67d52..dd6b94f 100644 --- a/RobotApp/Program.cs +++ b/RobotApp/Program.cs @@ -5,7 +5,9 @@ using MudBlazor.Services; using RobotApp.Components; using RobotApp.Components.Account; using RobotApp.Data; +using RobotApp.Services; using RobotApp.Services.Robot; +using RobotApp.Services.Robot.Simulation; var builder = WebApplication.CreateBuilder(args); @@ -46,7 +48,9 @@ builder.Services.AddSingleton, IdentityNoOpEmailSe builder.Services.AddSingleton(typeof(RobotApp.Services.Logger<>)); builder.Services.AddSingleton(); -builder.Services.AddHostedService(sp => sp.GetRequiredService()); + +builder.Services.AddRobotSimulation(); +builder.Services.AddRobot(); var app = builder.Build(); await app.Services.SeedApplicationDbAsync(); diff --git a/RobotApp/RobotApp.csproj b/RobotApp/RobotApp.csproj index a0f9339..003dbb1 100644 --- a/RobotApp/RobotApp.csproj +++ b/RobotApp/RobotApp.csproj @@ -29,8 +29,4 @@ - - - - diff --git a/RobotApp/Services/Exceptions/ActionException.cs b/RobotApp/Services/Exceptions/ActionException.cs index 4348012..8fabf43 100644 --- a/RobotApp/Services/Exceptions/ActionException.cs +++ b/RobotApp/Services/Exceptions/ActionException.cs @@ -1,5 +1,15 @@ -namespace RobotApp.Services.Exceptions; +using RobotApp.VDA5050.State; -public class ActionException : OrderException +namespace RobotApp.Services.Exceptions; + +public class ActionException : Exception { + public ActionException(string message) : base(message) { } + public ActionException(string message, Exception inner) : base(message, inner) { } + public ActionException() : base() { } + public ActionException(Error error) : base() + { + Error = error; + } + public Error? Error { get; set; } } diff --git a/RobotApp/Services/Exceptions/ModbusException.cs b/RobotApp/Services/Exceptions/ModbusException.cs index 136495c..88ca6ff 100644 --- a/RobotApp/Services/Exceptions/ModbusException.cs +++ b/RobotApp/Services/Exceptions/ModbusException.cs @@ -1,16 +1,15 @@ -namespace RobotApp.Services.Exceptions; +using RobotApp.VDA5050.State; + +namespace RobotApp.Services.Exceptions; public class ModbusException : Exception { - public ModbusException() - { - } - public ModbusException(string message) - : base(message) - { - } - public ModbusException(string message, Exception inner) - : base(message, inner) + public ModbusException(string message) : base(message) { } + public ModbusException(string message, Exception inner) : base(message, inner) { } + public ModbusException() : base() { } + public ModbusException(Error error) : base() { + Error = error; } + public Error? Error { get; set; } } diff --git a/RobotApp/Services/Exceptions/PathPlannerException.cs b/RobotApp/Services/Exceptions/PathPlannerException.cs new file mode 100644 index 0000000..13ac794 --- /dev/null +++ b/RobotApp/Services/Exceptions/PathPlannerException.cs @@ -0,0 +1,15 @@ +using RobotApp.VDA5050.State; + +namespace RobotApp.Services.Exceptions; + +public class PathPlannerException : Exception +{ + public PathPlannerException(string message) : base(message) { } + public PathPlannerException(string message, Exception inner) : base(message, inner) { } + public PathPlannerException() : base() { } + public PathPlannerException(Error error) : base() + { + Error = error; + } + public Error? Error { get; set; } +} diff --git a/RobotApp/Services/Exceptions/SimulationException.cs b/RobotApp/Services/Exceptions/SimulationException.cs new file mode 100644 index 0000000..cb6366c --- /dev/null +++ b/RobotApp/Services/Exceptions/SimulationException.cs @@ -0,0 +1,15 @@ +using RobotApp.VDA5050.State; + +namespace RobotApp.Services.Exceptions; + +public class SimulationException : Exception +{ + public SimulationException(string message) : base(message) { } + public SimulationException(string message, Exception inner) : base(message, inner) { } + public SimulationException() : base() { } + public SimulationException(Error error) : base() + { + Error = error; + } + public Error? Error { get; set; } +} diff --git a/RobotApp/Services/ModbusTcpClient.cs b/RobotApp/Services/ModbusTcpClient.cs index ee5c547..64a2c35 100644 --- a/RobotApp/Services/ModbusTcpClient.cs +++ b/RobotApp/Services/ModbusTcpClient.cs @@ -13,11 +13,12 @@ public class ModbusTcpClient(string IpAddress, int Port, byte ClientId) : IDispo private NetworkStream? stream; private bool disposed = false; + private readonly Lock LockObject = new(); 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; + private const int numberOfRetries = 3; ~ModbusTcpClient() { @@ -100,10 +101,29 @@ public class ModbusTcpClient(string IpAddress, int Port, byte ClientId) : IDispo disposed = true; } } + + private uint GetNextTransactionId() + { + lock (LockObject) + { + return ++transactionIdentifierInternal; + } + } + + private static bool ShouldRetry(Exception ex) + { + return ex is ConnectionAbortedException || + ex is TimeoutException || + ex is SocketException || + ex is InvalidOperationException || + (ex is IOException && ex.InnerException is SocketException); + } private byte[] Write(byte functionCode, ushort startingAddress, ushort quantity, CancellationToken? cancellationToken = null) => Write(functionCode, startingAddress, quantity, [], cancellationToken); private byte[] Write(byte functionCode, ushort startingAddress, ushort quantity, byte[] multipleData, CancellationToken? cancellationToken = null) + => Write(functionCode, startingAddress, quantity, multipleData, 0, cancellationToken); + private byte[] Write(byte functionCode, ushort startingAddress, ushort quantity, byte[] multipleData, int retryAttempt, CancellationToken? cancellationToken = null) { try { @@ -112,11 +132,11 @@ public class ModbusTcpClient(string IpAddress, int Port, byte ClientId) : IDispo if (!Connect() || !IsConnected || stream is null) throw new ConnectionAbortedException(); } - transactionIdentifierInternal++; + var transactionId = GetNextTransactionId(); byte[] writeData = new byte[12 + multipleData.Length]; var dataLength = multipleData.Length + 6; - writeData[0] = (byte)(transactionIdentifierInternal >> 8); - writeData[1] = (byte)(transactionIdentifierInternal & 0xFF); + writeData[0] = (byte)(transactionId >> 8); + writeData[1] = (byte)(transactionId & 0xFF); writeData[2] = 0x00; writeData[3] = 0x00; writeData[4] = (byte)(dataLength >> 8); @@ -171,6 +191,11 @@ public class ModbusTcpClient(string IpAddress, int Port, byte ClientId) : IDispo Array.Copy(readData, 9, receiveData, 0, receiveData.Length); return receiveData; } + catch (Exception ex) when (ShouldRetry(ex) && retryAttempt >= numberOfRetries) + { + Dispose(true); + return Write(functionCode, startingAddress, quantity, multipleData, retryAttempt + 1, cancellationToken); + } catch (Exception ex) when (!(ex is ModbusException || ex is TimeoutException)) { throw new ModbusException("Communication error", ex); diff --git a/RobotApp/Services/Robot/Navigation/Algorithm/PurePursuit.cs b/RobotApp/Services/Robot/Navigation/Algorithm/PurePursuit.cs deleted file mode 100644 index 5901346..0000000 --- a/RobotApp/Services/Robot/Navigation/Algorithm/PurePursuit.cs +++ /dev/null @@ -1,156 +0,0 @@ -//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/Robot/Navigation/DifferentialNavigation.cs b/RobotApp/Services/Robot/Navigation/DifferentialNavigation.cs deleted file mode 100644 index bc88f3c..0000000 --- a/RobotApp/Services/Robot/Navigation/DifferentialNavigation.cs +++ /dev/null @@ -1,22 +0,0 @@ -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 deleted file mode 100644 index c2d8e78..0000000 --- a/RobotApp/Services/Robot/Navigation/NavigationController.cs +++ /dev/null @@ -1,61 +0,0 @@ -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/Robot/Navigation/NavigationManager.cs b/RobotApp/Services/Robot/Navigation/NavigationManager.cs deleted file mode 100644 index f6b38c2..0000000 --- a/RobotApp/Services/Robot/Navigation/NavigationManager.cs +++ /dev/null @@ -1,6 +0,0 @@ -namespace RobotApp.Services.Robot.Navigation -{ - public class NavigationManager - { - } -} diff --git a/RobotApp/Services/Robot/RobotAction.cs b/RobotApp/Services/Robot/RobotAction.cs index ea5ad31..02c9893 100644 --- a/RobotApp/Services/Robot/RobotAction.cs +++ b/RobotApp/Services/Robot/RobotAction.cs @@ -3,7 +3,7 @@ using RobotApp.VDA5050.State; namespace RobotApp.Services.Robot; -public class RobotAction : IInstanceActions +public class RobotAction : IInstantActions { public ActionState[] ActionStates { get; private set; } = []; diff --git a/RobotApp/Services/Robot/RobotBattery.cs b/RobotApp/Services/Robot/RobotBattery.cs index d145843..c7c302a 100644 --- a/RobotApp/Services/Robot/RobotBattery.cs +++ b/RobotApp/Services/Robot/RobotBattery.cs @@ -1,6 +1,36 @@ -namespace RobotApp.Services.Robot +using RobotApp.Interfaces; + +namespace RobotApp.Services.Robot; + +public class RobotBattery : IBattery { - public class RobotBattery - { - } + public bool IsReady { get; private set; } = true; + + public double Voltage => throw new NotImplementedException(); + + public double Current => throw new NotImplementedException(); + + public double SOC => throw new NotImplementedException(); + + public double SOH => throw new NotImplementedException(); + + public BatteryStatus Status => throw new NotImplementedException(); + + public int ChargeTime => throw new NotImplementedException(); + + public int DischargeTime => throw new NotImplementedException(); + + public double Temperature => throw new NotImplementedException(); + + public double RemainingCapacity => throw new NotImplementedException(); + + public double RemainingEnergy => throw new NotImplementedException(); + + public int DischargeCycles => throw new NotImplementedException(); + + public int ChargeCycles => throw new NotImplementedException(); + + public bool IsCharging => throw new NotImplementedException(); + + public string[] ErrorStatus => throw new NotImplementedException(); } diff --git a/RobotApp/Services/Robot/RobotConfiguration.cs b/RobotApp/Services/Robot/RobotConfiguration.cs index 97aa1bb..c2dfc59 100644 --- a/RobotApp/Services/Robot/RobotConfiguration.cs +++ b/RobotApp/Services/Robot/RobotConfiguration.cs @@ -6,4 +6,17 @@ public class RobotConfiguration { public static string SerialNumber { get; set; } = "Robot-Demo"; public static NavigationType NavigationType { get; set; } = NavigationType.Differential; + public static VDA5050.VDA5050Setting VDA5050Setting { get; set; } = new() + { + HostServer = "172.20.235.170", + Port = 1885, + UserName = "robotics", + Password = "robotics", + Manufacturer = "PhenikaaX", + Version = "0.0.1", + PublishRepeat = 2, + }; + public static string PLCAddress { get; set; } = "127.0.0.1"; + public static int PLCPort { get; set; } = 502; + public static int PLCUnitId { get; set; } = 1; } diff --git a/RobotApp/Services/Robot/RobotConnection.cs b/RobotApp/Services/Robot/RobotConnection.cs index 5befe24..dbd4511 100644 --- a/RobotApp/Services/Robot/RobotConnection.cs +++ b/RobotApp/Services/Robot/RobotConnection.cs @@ -6,17 +6,13 @@ using System.Text.Json; namespace RobotApp.Services.Robot; -public class RobotConnection(Logger Logger, Logger MQTTClientLogger) : BackgroundService +public class RobotConnection(Logger Logger, Logger MQTTClientLogger) { - private readonly VDA5050Setting VDA5050Setting = new() - { - HostServer = "172.20.235.170", - Port = 1886, - }; + private readonly VDA5050Setting VDA5050Setting = RobotConfiguration.VDA5050Setting; private MQTTClient? MqttClient; public bool IsConnected => MqttClient is not null && MqttClient.IsConnected; - public readonly static string SerialNumber = "RobotDemo"; + public readonly static string SerialNumber = RobotConfiguration.SerialNumber; private void OrderChanged(string data) { @@ -50,20 +46,17 @@ public class RobotConnection(Logger Logger, Logger return new(false, "Chưa có kết nối tới broker"); } - protected override async Task ExecuteAsync(CancellationToken stoppingToken) + public async Task StartConnection(CancellationToken cancellationToken) { - await Task.Yield(); - MqttClient = new MQTTClient(SerialNumber, VDA5050Setting, MQTTClientLogger); MqttClient.OrderChanged += OrderChanged; MqttClient.InstanceActionsChanged += InstanceActionsChanged; - await MqttClient.ConnectAsync(stoppingToken); - await MqttClient.SubscribeAsync(stoppingToken); + await MqttClient.ConnectAsync(cancellationToken); + await MqttClient.SubscribeAsync(cancellationToken); } - public override async Task StopAsync(CancellationToken cancellationToken) + public async Task StopConnection() { - if(MqttClient is not null) await MqttClient.DisposeAsync(); - await base.StopAsync(cancellationToken); + if (MqttClient is not null) await MqttClient.DisposeAsync(); } } diff --git a/RobotApp/Services/Robot/RobotController.cs b/RobotApp/Services/Robot/RobotController.cs index 6d422d5..cc99562 100644 --- a/RobotApp/Services/Robot/RobotController.cs +++ b/RobotApp/Services/Robot/RobotController.cs @@ -1,13 +1,14 @@ using RobotApp.Interfaces; using RobotApp.Services.Exceptions; +using RobotApp.Services.State; +using RobotApp.VDA5050.InstantAction; using RobotApp.VDA5050.Order; -using RobotApp.VDA5050.State; namespace RobotApp.Services.Robot; public class RobotController(IOrder OrderManager, INavigation NavigationManager, - IInstanceActions ActionManager, + IInstantActions ActionManager, IBattery BatteryManager, ILocalization Localization, IPeripheral PeripheralManager, @@ -15,7 +16,8 @@ public class RobotController(IOrder OrderManager, IError ErrorManager, IInfomation InfomationManager, Logger Logger, - RobotConnection ConnectionManager) : BackgroundService + RobotConnection ConnectionManager, + RobotStateMachine StateManager) : BackgroundService { private readonly Mutex NewOrderMutex = new(); private readonly Mutex NewInstanceMutex = new(); @@ -25,11 +27,25 @@ public class RobotController(IOrder OrderManager, protected override Task ExecuteAsync(CancellationToken stoppingToken) { + InitializationingHandler(); + UpdateStateTimer = new(UpdateStateInterval, UpdateStateHandler, Logger); UpdateStateTimer.Start(); return Task.CompletedTask; } + private void InitializationingHandler() + { + try + { + StateManager.InitializeHierarchyStates(); + } + catch + { + + } + } + 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 @@ -43,13 +59,13 @@ public class RobotController(IOrder OrderManager, { 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}")); + if (order.OrderId != OrderManager.OrderId) throw new OrderException(RobotErrors.Error1001(OrderManager.OrderId, order.OrderId)); OrderManager.UpdateOrder(order.OrderUpdateId, order.Nodes, order.Edges); } - else if (PeripheralManager.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 if (PeripheralManager.PeripheralMode != PeripheralMode.AUTOMATIC) throw new OrderException(RobotErrors.Error1006(PeripheralManager.PeripheralMode)); + else if (ActionManager.HasActionRunning) throw new OrderException(RobotErrors.Error1007()); + else if (ErrorManager.HasFatalError) throw new OrderException(RobotErrors.Error1008()); + else if (NavigationManager.Driving) throw new OrderException(RobotErrors.Error1009()); else OrderManager.StartOrder(order.OrderId, order.Nodes, order.Edges); } catch (OrderException orEx) @@ -71,7 +87,7 @@ public class RobotController(IOrder OrderManager, } } - public void NewInstanceActionUpdated() + public void NewInstanceActionUpdated(InstantActionsMsg action) { if (NewInstanceMutex.WaitOne(2000)) { diff --git a/RobotApp/Services/Robot/RobotErrors.cs b/RobotApp/Services/Robot/RobotErrors.cs index 132e6d4..f7eb7ec 100644 --- a/RobotApp/Services/Robot/RobotErrors.cs +++ b/RobotApp/Services/Robot/RobotErrors.cs @@ -1,5 +1,6 @@ -using RobotApp.VDA5050.State; -using RobotApp.VDA5050.Type; +using Microsoft.AspNetCore.Components; +using RobotApp.Interfaces; +using RobotApp.VDA5050.State; namespace RobotApp.Services.Robot; @@ -38,4 +39,35 @@ public class RobotErrors ErrorReferences = [] }; } + + public static Error Error1001(string oldOrderId, string newOrderId) + => CreateError(ErrorType.INITIALIZE_ORDER, "1001", ErrorLevel.WARNING, $"Có order đang được thực hiện. OrderId: {oldOrderId}, OrderId mới: {newOrderId}"); + public static Error Error1002(int nodesLength) + => CreateError(ErrorType.INITIALIZE_ORDER, "1002", ErrorLevel.WARNING, $"Order Nodes không hợp lệ. Kích thước: {nodesLength}"); + public static Error Error1003(int edgesLength) + => CreateError(ErrorType.INITIALIZE_ORDER, "1003", ErrorLevel.WARNING, $"Order Edges không hợp lệ. Kích thước: {edgesLength}"); + public static Error Error1004(int nodesLength, int edgesLength) + => 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: {edgesLength}, kích thước nodes: {nodesLength}"); + public static Error Error1005() + => CreateError(ErrorType.INITIALIZE_ORDER, "1005", ErrorLevel.WARNING, $"Không có order đang được thực hiện."); + public static Error Error1006(PeripheralMode peripheralMode) + => 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: {peripheralMode}"); + public static Error Error1007() + => 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."); + public static Error Error1008() + => 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."); + public static Error Error1009() + => 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."); + public static Error Error1010(string nodeId, int sequenceId, int correctIndex) + => CreateError(ErrorType.INITIALIZE_ORDER, "1010", ErrorLevel.WARNING, $"Order Nodes không đúng thứ tự. NodeId: {nodeId}, SequenceId: {sequenceId}, Vị trí đúng: {correctIndex}"); + public static Error Error1011(string edgeId, int sequenceId, int correctIndex) + => CreateError(ErrorType.INITIALIZE_ORDER, "1011", ErrorLevel.WARNING, $"Order Edges không đúng thứ tự. EdgeId: {edgeId}, SequenceId: {sequenceId}, Vị trí đúng: {correctIndex}"); + public static Error Error1012(NavigationState state) + => 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: {state}"); + public static Error Error1013() + => new(); + public static Error Error1014(string edgeId, string nodeId) + => CreateError(ErrorType.INITIALIZE_ORDER, "1014", ErrorLevel.WARNING, $"Edge {edgeId} chứa StartNodeId {nodeId} không tồn tại trong Nodes"); + public static Error Error1015(string edgeId, string nodeId) + => CreateError(ErrorType.INITIALIZE_ORDER, "1015", ErrorLevel.WARNING, $"Edge {edgeId} chứa {nodeId} không tồn tại trong Nodes"); } diff --git a/RobotApp/Services/Robot/RobotLoads.cs b/RobotApp/Services/Robot/RobotLoads.cs index bdbcc27..d671692 100644 --- a/RobotApp/Services/Robot/RobotLoads.cs +++ b/RobotApp/Services/Robot/RobotLoads.cs @@ -1,6 +1,5 @@ -namespace RobotApp.Services.Robot +namespace RobotApp.Services.Robot; + +public class RobotLoads { - public class RobotLoads - { - } } diff --git a/RobotApp/Services/Robot/RobotLocalization.cs b/RobotApp/Services/Robot/RobotLocalization.cs new file mode 100644 index 0000000..27b8fcc --- /dev/null +++ b/RobotApp/Services/Robot/RobotLocalization.cs @@ -0,0 +1,83 @@ +using RobotApp.Common.Shares; +using RobotApp.Interfaces; +using RobotApp.Services.Robot.Simulation; + +namespace RobotApp.Services.Robot; + +public class Xloc +{ + public double X { get; set; } + public double Y { get; set; } + public double Theta { get; set; } + public string SlamState { get; set; } = string.Empty; + public string SlamStateDetail { get; set; } = string.Empty; + public string CurrentActiveMap { get; set; } = string.Empty; + public double Reliability { get; set; } + public double MatchingScore { get; set; } + public bool IsReady { get; set; } +} + +public class RobotLocalization(IConfiguration Configuration, SimulationVisualization SimVisualization) : ILocalization +{ + public double X => IsSimulation ? SimVisualization.X : Xloc.X; + + public double Y => IsSimulation ? SimVisualization.Y : Xloc.Y; + + public double Theta => IsSimulation ? SimVisualization.Theta * Math.PI / 180 : Xloc.Theta; + + public string SlamState => IsSimulation ? "Localization" : Xloc.SlamState; + + public string SlamStateDetail => IsSimulation ? "" : Xloc.SlamStateDetail; + + public string CurrentActiveMap => IsSimulation ? "" : Xloc.CurrentActiveMap; + + public double Reliability => IsSimulation ? 100 : Xloc.Reliability; + + public double MatchingScore => IsSimulation ? 100 : Xloc.MatchingScore; + + public bool IsReady => IsSimulation || Xloc.IsReady; + + + private readonly Xloc Xloc = new(); + private readonly bool IsSimulation = Configuration.GetValue("Robot:Simulation", false); + + public MessageResult ActivateMap(string mapName) + { + throw new NotImplementedException(); + } + + public MessageResult ChangeMapOrigin(double x, double y, double theta) + { + throw new NotImplementedException(); + } + + public MessageResult SetInitializePosition(double x, double y, double theta) + { + throw new NotImplementedException(); + } + + public MessageResult StartLocalization() + { + throw new NotImplementedException(); + } + + public MessageResult StartMapping(double resolution) + { + throw new NotImplementedException(); + } + + public MessageResult StopLocalization() + { + throw new NotImplementedException(); + } + + public MessageResult StopMapping(string mapName) + { + throw new NotImplementedException(); + } + + public MessageResult SwitchMap(string mapName, double x, double y, double theta) + { + throw new NotImplementedException(); + } +} diff --git a/RobotApp/Services/Robot/RobotNavigation.cs b/RobotApp/Services/Robot/RobotNavigation.cs new file mode 100644 index 0000000..2ce534a --- /dev/null +++ b/RobotApp/Services/Robot/RobotNavigation.cs @@ -0,0 +1,71 @@ +using RobotApp.Interfaces; +using RobotApp.Services.Robot.Simulation; +using RobotApp.Services.Robot.Simulation.Navigation; +using RobotApp.VDA5050.Order; + +namespace RobotApp.Services.Robot; + +public class RobotNavigation(SimulationModel SimModel) : INavigation +{ + public bool IsReady => IsSimulation; + public bool Driving => IsSimulation ? SimNavigation is not null ? SimNavigation.Driving : false : false; + public double VelocityX => IsSimulation ? SimNavigation is not null ? SimNavigation.VelocityX : 0 : 0; + public double VelocityY => IsSimulation ? SimNavigation is not null ? SimNavigation.VelocityY : 0 : 0; + public double Omega => IsSimulation ? SimNavigation is not null ? SimNavigation.Omega : 0 : 0; + public NavigationState State => IsSimulation ? SimNavigation is not null ? SimNavigation.State : NavigationState.None : NavigationState.None; + + + private SimulationNavigation? SimNavigation; + private bool IsSimulation => SimModel.Enable; + + + public void CancelMovement() + { + throw new NotImplementedException(); + } + + public void Move(Node[] nodes, Edge[] edges) + { + throw new NotImplementedException(); + } + + public void MoveStraight(double x, double y) + { + throw new NotImplementedException(); + } + + public void Paused() + { + throw new NotImplementedException(); + } + + public void Resume() + { + throw new NotImplementedException(); + } + + public void Rotate(double angle) + { + throw new NotImplementedException(); + } + + public void UpdateOrder(int lastBaseSequence) + { + throw new NotImplementedException(); + } + + public void RefreshOrder(Node[] nodes, Edge[] edges) + { + throw new NotImplementedException(); + } + + public void UpdateOrder(string lastBaseNodeId) + { + throw new NotImplementedException(); + } + + public void Refresh() + { + throw new NotImplementedException(); + } +} diff --git a/RobotApp/Services/Robot/RobotOrderController.cs b/RobotApp/Services/Robot/RobotOrderController.cs index fe8762a..97d595b 100644 --- a/RobotApp/Services/Robot/RobotOrderController.cs +++ b/RobotApp/Services/Robot/RobotOrderController.cs @@ -6,7 +6,7 @@ using Action = RobotApp.VDA5050.InstantAction.Action; namespace RobotApp.Services.Robot; -public class RobotOrderController(INavigation NavigationManager, IInstanceActions ActionManager, IError ErrorManager, Logger Logger) : IOrder +public class RobotOrderController(INavigation NavigationManager, IInstantActions ActionManager, IError ErrorManager, Logger Logger) : IOrder { public string OrderId { get; private set; } = string.Empty; public int OrderUpdateId { get; private set; } @@ -22,17 +22,18 @@ public class RobotOrderController(INavigation NavigationManager, IInstanceAction private WatchTimer? OrderTimer; private int BaseSequenceId = 0; + public void StartOrder(string orderId, Node[] nodes, Edge[] edges) { 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}")); + if (!string.IsNullOrEmpty(OrderId) && orderId != OrderId) throw new OrderException(RobotErrors.Error1001(OrderId, orderId)); + if (nodes.Length < 2) throw new OrderException(RobotErrors.Error1002(nodes.Length)); + if (edges.Length < 1) throw new OrderException(RobotErrors.Error1003(edges.Length)); + if (edges.Length != nodes.Length - 1) throw new OrderException(RobotErrors.Error1004(nodes.Length, edges.Length)); + if (NavigationManager.State != NavigationState.Idle) throw new OrderException(RobotErrors.Error1012(NavigationManager.State)); for (int i = 0; i < nodes.Length; i++) { @@ -52,12 +53,12 @@ public class RobotOrderController(INavigation NavigationManager, IInstanceAction } 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].SequenceId != i) throw new OrderException(RobotErrors.Error1010(nodes[i].NodeId, nodes[i].SequenceId, i)); + if (i < nodes.Length - 1 && edges[i].SequenceId != i) throw new OrderException(RobotErrors.Error1011(edges[i].EdgeId, edges[i].SequenceId, i)); if (nodes[i].Released) BaseSequenceId = nodes[i].SequenceId; } - NodeStates = nodes.Select(n => new NodeState + NodeStates = [.. nodes.Select(n => new NodeState { NodeId = n.NodeId, Released = n.Released, @@ -70,15 +71,15 @@ public class RobotOrderController(INavigation NavigationManager, IInstanceAction Theta = n.NodePosition.Theta, MapId = n.NodePosition.MapId } - }).ToArray(); - EdgeStates = edges.Select(e => new EdgeState + })]; + 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)]); @@ -111,7 +112,7 @@ public class RobotOrderController(INavigation NavigationManager, IInstanceAction { 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 (string.IsNullOrEmpty(OrderId)) throw new OrderException(RobotErrors.Error1005()); if (orderUpdateId > OrderUpdateId) { OrderUpdateId = orderUpdateId; diff --git a/RobotApp/Services/Robot/RobotPathPlanner.cs b/RobotApp/Services/Robot/RobotPathPlanner.cs index 18b1828..0d08ff2 100644 --- a/RobotApp/Services/Robot/RobotPathPlanner.cs +++ b/RobotApp/Services/Robot/RobotPathPlanner.cs @@ -2,7 +2,7 @@ using RobotApp.Common.Shares.Enums; using RobotApp.Common.Shares.Model; using RobotApp.Services.Exceptions; -using RobotApp.Services.Robot.Navigation; +using RobotApp.Services.Robot.Simulation.Navigation; using RobotApp.VDA5050.Order; using RobotApp.VDA5050.State; @@ -14,20 +14,20 @@ public class RobotPathPlanner(IConfiguration Configuration) 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}")); + if (nodes.Length < 2) throw new PathPlannerException(RobotErrors.Error1002(nodes.Length)); + if (edges.Length < 1) throw new PathPlannerException(RobotErrors.Error1003(edges.Length)); + if (edges.Length != nodes.Length - 1) throw new PathPlannerException(RobotErrors.Error1004(nodes.Length, edges.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}")); + if (nodes.Length < 2) throw new PathPlannerException(RobotErrors.Error1002(nodes.Length)); + if (edges.Length < 1) throw new PathPlannerException(RobotErrors.Error1003(edges.Length)); + if (edges.Length != nodes.Length - 1) throw new PathPlannerException(RobotErrors.Error1004(nodes.Length, edges.Length)); - List navigationNode = [new() + List navigationNode = [new() { NodeId = nodes[0].NodeId, X = nodes[0].X, @@ -40,8 +40,8 @@ public class RobotPathPlanner(IConfiguration Configuration) { 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}")); + if (startNode is null) throw new PathPlannerException(RobotErrors.Error1014(edge.EdgeId, edge.StartNodeId)); + if (endNode is null) throw new PathPlannerException(RobotErrors.Error1014(edge.EdgeId, edge.EndNodeId)); var EdgeCalculatorModel = new EdgeCalculatorModel() { diff --git a/RobotApp/Services/Robot/RobotPeripheral.cs b/RobotApp/Services/Robot/RobotPeripheral.cs new file mode 100644 index 0000000..7ea64df --- /dev/null +++ b/RobotApp/Services/Robot/RobotPeripheral.cs @@ -0,0 +1,77 @@ +using RobotApp.Interfaces; + +namespace RobotApp.Services.Robot; + +public class RobotPeripheral : BackgroundService, IPeripheral, ISafety +{ + public PeripheralMode PeripheralMode { get; private set; } = PeripheralMode.SERVICE; + public bool IsReady { get; private set; } = true; + public bool Emergency => throw new NotImplementedException(); + public bool Bumper => throw new NotImplementedException(); + public bool LiftedUp => throw new NotImplementedException(); + public bool LiftedDown => throw new NotImplementedException(); + public bool LiftHome => throw new NotImplementedException(); + public bool LidarFrontProtectField => throw new NotImplementedException(); + public bool LidarBackProtectField => throw new NotImplementedException(); + public bool LidarFrontTimProtectField => throw new NotImplementedException(); + public bool LeftMotorReady => throw new NotImplementedException(); + public bool RightMotorReady => throw new NotImplementedException(); + public bool LiftMotorReady => throw new NotImplementedException(); + public bool SwitchLock => throw new NotImplementedException(); + public bool SwitchManual => throw new NotImplementedException(); + public bool SwitchAuto => throw new NotImplementedException(); + public bool ButtonStart => throw new NotImplementedException(); + public bool ButtonStop => throw new NotImplementedException(); + public bool ButtonReset => throw new NotImplementedException(); + public bool HasLoad => throw new NotImplementedException(); + public bool MutedBase => throw new NotImplementedException(); + public bool MutedLoad => throw new NotImplementedException(); + public bool StartedCharging => throw new NotImplementedException(); + public bool SpeedVerySlow => throw new NotImplementedException(); + public bool SpeedSlow => throw new NotImplementedException(); + public bool SpeedNormal => throw new NotImplementedException(); + public bool SpeedMedium => throw new NotImplementedException(); + public bool SpeedOptimal => throw new NotImplementedException(); + public bool SpeedFast => throw new NotImplementedException(); + public bool SpeedVeryFast => throw new NotImplementedException(); + + public bool SetHorizontalLoad() + { + throw new NotImplementedException(); + } + + public bool SetMutedBase(bool muted) + { + throw new NotImplementedException(); + } + + public bool SetMutedLoad(bool muted) + { + throw new NotImplementedException(); + } + + public bool SetOnCharging() + { + throw new NotImplementedException(); + } + + public bool SetProccessState() + { + throw new NotImplementedException(); + } + + public bool SetSytemState() + { + throw new NotImplementedException(); + } + + protected override Task ExecuteAsync(CancellationToken stoppingToken) + { + throw new NotImplementedException(); + } + + public override Task StopAsync(CancellationToken cancellationToken) + { + return base.StopAsync(cancellationToken); + } +} diff --git a/RobotApp/Services/Robot/RobotSafety.cs b/RobotApp/Services/Robot/RobotSafety.cs deleted file mode 100644 index c90f7e4..0000000 --- a/RobotApp/Services/Robot/RobotSafety.cs +++ /dev/null @@ -1,6 +0,0 @@ -namespace RobotApp.Services.Robot -{ - public class RobotSafety - { - } -} diff --git a/RobotApp/Services/Robot/RobotVisualization.cs b/RobotApp/Services/Robot/RobotVisualization.cs index e2de5c6..4edefff 100644 --- a/RobotApp/Services/Robot/RobotVisualization.cs +++ b/RobotApp/Services/Robot/RobotVisualization.cs @@ -1,6 +1,34 @@ -namespace RobotApp.Services.Robot +using RobotApp.Interfaces; +using RobotApp.VDA5050.Visualization; + +namespace RobotApp.Services.Robot; + +public class RobotVisualization(ILocalization Localization) { - public class RobotVisualization + public VisualizationMsg Visualization => GetVisualizationMsg(); + public string SerialNumber { get; set; } = string.Empty; + + private uint HeaderId; + private VisualizationMsg GetVisualizationMsg() { + return new VisualizationMsg() + { + HeaderId = HeaderId++, + SerialNumber = SerialNumber, + MapId = Localization.CurrentActiveMap, + MapDescription = string.Empty, + AgvPosition = new AgvPosition() + { + X = Localization.X, + Y = Localization.Y, + Theta = Localization.Theta + }, + Velocity = new Velocity() + { + Vx = 0, + Vy = 0, + Omega = 0 + } + }; } } diff --git a/RobotApp/Services/Robot/Navigation/Algorithm/FuzzyLogic.cs b/RobotApp/Services/Robot/Simulation/Navigation/Algorithm/FuzzyLogic.cs similarity index 99% rename from RobotApp/Services/Robot/Navigation/Algorithm/FuzzyLogic.cs rename to RobotApp/Services/Robot/Simulation/Navigation/Algorithm/FuzzyLogic.cs index 817ed22..79e6a16 100644 --- a/RobotApp/Services/Robot/Navigation/Algorithm/FuzzyLogic.cs +++ b/RobotApp/Services/Robot/Simulation/Navigation/Algorithm/FuzzyLogic.cs @@ -1,4 +1,4 @@ -namespace RobotApp.Services.Robot.Navigation.Algorithm; +namespace RobotApp.Services.Robot.Simulation.Navigation.Algorithm; public class FuzzyLogic { diff --git a/RobotApp/Services/Robot/Navigation/Algorithm/PID.cs b/RobotApp/Services/Robot/Simulation/Navigation/Algorithm/PID.cs similarity index 72% rename from RobotApp/Services/Robot/Navigation/Algorithm/PID.cs rename to RobotApp/Services/Robot/Simulation/Navigation/Algorithm/PID.cs index d28112b..52f7ee2 100644 --- a/RobotApp/Services/Robot/Navigation/Algorithm/PID.cs +++ b/RobotApp/Services/Robot/Simulation/Navigation/Algorithm/PID.cs @@ -1,4 +1,4 @@ -namespace RobotApp.Services.Robot.Navigation.Algorithm; +namespace RobotApp.Services.Robot.Simulation.Navigation.Algorithm; public class PID { @@ -29,13 +29,12 @@ public class PID return this; } - public double PID_step(double error, double min, double max) + public double PID_step(double error, double min, double max, double timeSample) { 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 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; diff --git a/RobotApp/Services/Robot/Simulation/Navigation/Algorithm/PurePursuit.cs b/RobotApp/Services/Robot/Simulation/Navigation/Algorithm/PurePursuit.cs new file mode 100644 index 0000000..ee69fbe --- /dev/null +++ b/RobotApp/Services/Robot/Simulation/Navigation/Algorithm/PurePursuit.cs @@ -0,0 +1,158 @@ +using RobotApp.Common.Shares.Enums; + +namespace RobotApp.Services.Robot.Simulation.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/Robot/Simulation/Navigation/DifferentialNavigation.cs b/RobotApp/Services/Robot/Simulation/Navigation/DifferentialNavigation.cs new file mode 100644 index 0000000..513e8c1 --- /dev/null +++ b/RobotApp/Services/Robot/Simulation/Navigation/DifferentialNavigation.cs @@ -0,0 +1,78 @@ +using RobotApp.Common.Shares.Enums; +using RobotApp.Interfaces; + +namespace RobotApp.Services.Robot.Simulation.Navigation; + +public class DifferentialNavigation : SimulationNavigation +{ + private readonly Logger Logger; + + public DifferentialNavigation(IServiceProvider ServiceProvider) : base(ServiceProvider) + { + using var scope = ServiceProvider.CreateScope(); + Logger = scope.ServiceProvider.GetRequiredService>(); + } + + protected override void NavigationHandler() + { + try + { + if (NavState == NavigationState.Rotating) + { + if (RotatePID is not null) + { + double Error = Visualization.Theta - TargetAngle; + if (Error > 180) Error -= 360; + else if (Error < -180) Error += 360; + if (Math.Abs(Error) < 1) + { + if (NavigationPath is not null && NavigationPath.Length > 2) NavState = NavigationState.Moving; + else Dispose(); + } + else + { + var SpeedCal = RotatePID.PID_step(Error * Math.PI / 180, AngularVelocity, -AngularVelocity, CycleHandlerMilliseconds / 1000.0); + VelocityController.SetSpeed(SpeedCal, SpeedCal, CycleHandlerMilliseconds / 1000.0); + } + } + } + else if (NavState == NavigationState.Moving) + { + if (NavigationPath is not null && SimulationOrderNodes.Length > 1 && CurrentBaseNode is not null) + { + if (MovePID is not null && MoveFuzzy is not null && MovePurePursuit is not null) + { + var DistanceToGoal = Math.Sqrt(Math.Pow(Visualization.X - SimulationOrderNodes[^1].X, 2) + Math.Pow(Visualization.Y - SimulationOrderNodes[^1].Y, 2)); + var DistanceToCheckingNode = Math.Sqrt(Math.Pow(Visualization.X - CurrentBaseNode.X, 2) + Math.Pow(Visualization.Y - CurrentBaseNode.Y, 2)); + var deviation = CurrentBaseNode.NodeId == SimulationOrderNodes[^1].NodeId ? 0.02 : 0.1; + if (DistanceToCheckingNode > deviation) + { + double SpeedTarget = MovePID.PID_step(DistanceToCheckingNode, SimulationModel.MaxVelocity, 0, CycleHandlerMilliseconds / 1000.0); + double AngularVel = MovePurePursuit.PurePursuit_step(Visualization.X, Visualization.Y, Visualization.Theta * Math.PI / 180); + AngularVel *= NavigationPath[MovePurePursuit.OnNodeIndex].Direction == RobotDirection.FORWARD ? 1 : -1; + (double AngularVelocityLeft, double AngularVelocityRight) = MoveFuzzy.Fuzzy_step(SpeedTarget, AngularVel, CycleHandlerMilliseconds / 1000.0); + + if (NavigationPath[MovePurePursuit.OnNodeIndex].Direction == RobotDirection.FORWARD) + { + AngularVelocityLeft /= SimulationModel.RadiusWheel; + AngularVelocityRight = AngularVelocityRight / SimulationModel.RadiusWheel * -1; + } + else + { + AngularVelocityLeft = AngularVelocityLeft / SimulationModel.RadiusWheel * -1; + AngularVelocityRight /= SimulationModel.RadiusWheel; + } + VelocityController.SetSpeed(AngularVelocityLeft, AngularVelocityRight, CycleHandlerMilliseconds / 1000.0); + } + else if (DistanceToGoal < 0.02) Dispose(); + else NavState = NavigationState.Waiting; + } + } + } + } + catch (Exception ex) + { + Logger.Write($"Error in DifferentialNavigation: {ex.Message}", LogLevel.Error); + } + } +} diff --git a/RobotApp/Services/Robot/Simulation/Navigation/ForkliftNavigation.cs b/RobotApp/Services/Robot/Simulation/Navigation/ForkliftNavigation.cs new file mode 100644 index 0000000..d113b5b --- /dev/null +++ b/RobotApp/Services/Robot/Simulation/Navigation/ForkliftNavigation.cs @@ -0,0 +1,23 @@ +namespace RobotApp.Services.Robot.Simulation.Navigation; + +public class ForkliftNavigation : SimulationNavigation +{ + private readonly Logger Logger; + public ForkliftNavigation(IServiceProvider ServiceProvider) : base(ServiceProvider) + { + using var scope = ServiceProvider.CreateScope(); + Logger = scope.ServiceProvider.GetRequiredService>(); + } + + protected override void NavigationHandler() + { + try + { + // Implement differential drive navigation logic here + } + catch (Exception ex) + { + Logger.Write($"Error in ForkliftNavigationSevice: {ex.Message}", LogLevel.Error); + } + } +} diff --git a/RobotApp/Services/Robot/Navigation/NavigationNode.cs b/RobotApp/Services/Robot/Simulation/Navigation/NavigationNode.cs similarity index 67% rename from RobotApp/Services/Robot/Navigation/NavigationNode.cs rename to RobotApp/Services/Robot/Simulation/Navigation/NavigationNode.cs index ccfc219..56e7d41 100644 --- a/RobotApp/Services/Robot/Navigation/NavigationNode.cs +++ b/RobotApp/Services/Robot/Simulation/Navigation/NavigationNode.cs @@ -1,6 +1,6 @@ using RobotApp.Common.Shares.Enums; -namespace RobotApp.Services.Robot.Navigation; +namespace RobotApp.Services.Robot.Simulation.Navigation; public class NavigationNode { @@ -9,5 +9,7 @@ public class NavigationNode public double Y { get; set; } public double Theta { get; set; } public RobotDirection Direction { get; set; } + public double AllowedDeviationXY { get; set; } + public double AllowedDeviationTheta { get; set; } public string Description { get; set; } = string.Empty; } diff --git a/RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigation.cs b/RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigation.cs new file mode 100644 index 0000000..1910bcd --- /dev/null +++ b/RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigation.cs @@ -0,0 +1,192 @@ +using Microsoft.AspNetCore.Components; +using RobotApp.Common.Shares; +using RobotApp.Common.Shares.Enums; +using RobotApp.Interfaces; +using RobotApp.Services.Exceptions; +using RobotApp.Services.Robot.Simulation.Navigation.Algorithm; +using RobotApp.VDA5050.Order; +using RobotApp.VDA5050.State; +using System.IO; + +namespace RobotApp.Services.Robot.Simulation.Navigation; + +public class SimulationNavigation : INavigation, IDisposable +{ + public NavigationState State => NavState; + public bool Driving => NavDriving; + public bool IsReady => true; + public double VelocityX => Visualization.Vx; + public double VelocityY => Visualization.Vy; + public double Omega => Visualization.Omega; + + protected NavigationState NavState = NavigationState.None; + protected bool NavDriving = false; + + protected const int CycleHandlerMilliseconds = 50; + private WatchTimer? NavigationTimer; + + protected double TargetAngle = 0; + protected PID? RotatePID; + protected readonly double AngularVelocity; + + protected PID? MovePID; + protected FuzzyLogic? MoveFuzzy; + protected PurePursuit? MovePurePursuit; + + protected Node[] OrderNodes = []; + protected Edge[] OrderEdges = []; + protected NavigationNode[] SimulationOrderNodes = []; + + protected readonly SimulationVisualization Visualization; + protected readonly SimulationVelocity VelocityController; + protected readonly SimulationModel SimulationModel; + protected readonly RobotPathPlanner PathPlanner; + protected NavigationNode[] NavigationPath = []; + protected NavigationNode? CurrentBaseNode; + protected NavigationState ResumeState = NavigationState.None; + + private readonly Logger Logger; + private bool IsIdle => NavState == NavigationState.Idle || NavState == NavigationState.Canceled; + + public SimulationNavigation(IServiceProvider ServiceProvider) + { + using var scope = ServiceProvider.CreateScope(); + Logger = scope.ServiceProvider.GetRequiredService>(); + Visualization = scope.ServiceProvider.GetRequiredService(); + SimulationModel = scope.ServiceProvider.GetRequiredService(); + PathPlanner = scope.ServiceProvider.GetRequiredService(); + VelocityController = new(Visualization, SimulationModel); + } + + protected void HandleNavigationStart() + { + NavigationTimer = new(CycleHandlerMilliseconds, NavigationHandler, Logger); + NavigationTimer.Start(); + } + + protected void HandleNavigationStop() + { + NavigationTimer?.Dispose(); + } + + protected virtual void NavigationHandler() { } + + public void CancelMovement() + { + Dispose(); + NavState = NavigationState.Canceled; + } + + public void Move(Node[] nodes, Edge[] edges) + { + if (!IsIdle) throw new SimulationException(RobotErrors.Error1012(NavState)); + NavState = NavigationState.Initializing; + if (nodes.Length < 2) throw new SimulationException(RobotErrors.Error1002(nodes.Length)); + if (edges.Length < 1) throw new SimulationException(RobotErrors.Error1003(edges.Length)); + if (edges.Length != nodes.Length - 1) throw new SimulationException(RobotErrors.Error1004(nodes.Length, edges.Length)); + + var pathDirection = PathPlanner.GetNavigationNode(Visualization.Theta, nodes, edges); + NavigationPath = PathPlanner.PathSplit(pathDirection, edges); + + MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6); + MoveFuzzy = new FuzzyLogic().WithGainP(1.1); + MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath([.. NavigationPath]); + + double angleFoward = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X) * 180 / Math.PI; + double angleBacward = Math.Atan2(NavigationPath[0].Y - NavigationPath[1].Y, NavigationPath[0].X - NavigationPath[1].X) * 180 / Math.PI; + OrderNodes = nodes; + OrderEdges = edges; + SimulationOrderNodes = pathDirection; + Rotate(SimulationOrderNodes[0].Direction == RobotDirection.FORWARD ? angleFoward : angleBacward); + } + + public void MoveStraight(double x, double y) + { + if (!IsIdle) throw new SimulationException(RobotErrors.Error1012(NavState)); + var headRobotNode = new NavigationNode() + { + X = Visualization.X * Math.Acos(Visualization.Theta * Math.PI / 180), + Y = Visualization.Y * Math.Asin(Visualization.Theta * Math.PI / 180), + }; + var goalNode = new NavigationNode() + { + NodeId = "RobotGoal", + X = x, + Y = y, + }; + var currentRobotNode = new NavigationNode() + { + NodeId = "RobotCurrentNode", + X = Visualization.X, + Y = Visualization.Y, + }; + goalNode.Theta = MathExtensions.GetVectorAngle(currentRobotNode.X, currentRobotNode.Y, headRobotNode.X, headRobotNode.Y, goalNode.X, goalNode.Y) > 90 ? + Math.Atan2(currentRobotNode.Y - goalNode.Y, currentRobotNode.X - goalNode.X) : + Math.Atan2(goalNode.Y - currentRobotNode.Y, goalNode.X - currentRobotNode.X); + currentRobotNode.Theta = goalNode.Theta; + NavigationPath = PathPlanner.PathSplit([currentRobotNode, goalNode], [new Edge() + { + EdgeId = "Straight edge", + Trajectory = new Trajectory() + { + Degree = 1, + ControlPoints = [] + }, + StartNodeId = currentRobotNode.NodeId, + EndNodeId = goalNode.NodeId, + }]); + MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6); + MoveFuzzy = new FuzzyLogic().WithGainP(1.1); + MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath(NavigationPath); + double Angle = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X); + Rotate(Angle * 180 / Math.PI); + UpdateOrder(goalNode.NodeId); + } + + public void Paused() + { + ResumeState = State; + NavState = NavigationState.Paused; + } + + public void Resume() + { + NavState = ResumeState; + ResumeState = NavigationState.None; + } + + public void Rotate(double angle) + { + if (!IsIdle) throw new SimulationException(RobotErrors.Error1012(NavState)); + RotatePID = new PID().WithKp(10).WithKi(0.01).WithKd(0.1); + TargetAngle = MathExtensions.NormalizeAngle(angle); + NavState = NavigationState.Rotating; + HandleNavigationStart(); + } + + public void UpdateOrder(string lastBaseNodeId) + { + } + + public void RefreshOrder(Node[] nodes, Edge[] edges) + { + throw new NotImplementedException(); + } + + public void Dispose() + { + HandleNavigationStop(); + VelocityController.Stop(); + OrderNodes = []; + OrderEdges = []; + SimulationOrderNodes = []; + NavDriving = false; + NavState = NavigationState.None; + GC.SuppressFinalize(this); + } + + public void Refresh() + { + NavState = NavigationState.Idle; + } +} diff --git a/RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigationManager.cs b/RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigationManager.cs new file mode 100644 index 0000000..2a5283f --- /dev/null +++ b/RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigationManager.cs @@ -0,0 +1,12 @@ +using RobotApp.Common.Shares.Enums; + +namespace RobotApp.Services.Robot.Simulation.Navigation; + +public class SimulationNavigationManager +{ + public static SimulationNavigation GetNavigation(NavigationType type, IServiceProvider ServiceProvider) + { + if (type == NavigationType.Forklift) return new ForkliftNavigation(ServiceProvider); + return new DifferentialNavigation(ServiceProvider); + } +} diff --git a/RobotApp/Services/Robot/Simulation/SimulationExtensions.cs b/RobotApp/Services/Robot/Simulation/SimulationExtensions.cs new file mode 100644 index 0000000..1676768 --- /dev/null +++ b/RobotApp/Services/Robot/Simulation/SimulationExtensions.cs @@ -0,0 +1,13 @@ +using System.Diagnostics.CodeAnalysis; + +namespace RobotApp.Services.Robot.Simulation; + +public static class SimulationExtensions +{ + public static IServiceCollection AddRobotSimulation(this IServiceCollection services) + { + services.AddSingleton(); + services.AddSingleton(); + return services; + } +} diff --git a/RobotApp/Services/Robot/Simulation/SimulationModel.cs b/RobotApp/Services/Robot/Simulation/SimulationModel.cs new file mode 100644 index 0000000..b274074 --- /dev/null +++ b/RobotApp/Services/Robot/Simulation/SimulationModel.cs @@ -0,0 +1,16 @@ +using RobotApp.Common.Shares.Enums; + +namespace RobotApp.Services.Robot.Simulation; + +public class SimulationModel(IConfiguration Configuration) +{ + public readonly bool Enable = Configuration.GetValue("Simulation:Enable", false); + public readonly double RadiusWheel = Configuration.GetValue("Simulation:RadiusWheel", 0.1); + public readonly double Width = Configuration.GetValue("Simulation:Width", 0.606); + public readonly double Length = Configuration.GetValue("Simulation:Length", 1.106); + public readonly double MaxVelocity = Configuration.GetValue("Simulation:MaxVelocity", 1.5); + public readonly double MaxAngularVelocity = Configuration.GetValue("Simulation:MaxAngularVelocity", 0.3); + public readonly double Acceleration = Configuration.GetValue("Simulation:Acceleration", 2); + public readonly double Deceleration = Configuration.GetValue("Simulation:Deceleration", 1); + public readonly NavigationType NavigationType = Configuration.GetValue("Simulation:NavigationType", NavigationType.Differential); +} diff --git a/RobotApp/Services/Robot/Simulation/SimulationVelocity.cs b/RobotApp/Services/Robot/Simulation/SimulationVelocity.cs new file mode 100644 index 0000000..63885ac --- /dev/null +++ b/RobotApp/Services/Robot/Simulation/SimulationVelocity.cs @@ -0,0 +1,56 @@ +namespace RobotApp.Services.Robot.Simulation; + +public class SimulationVelocity(SimulationVisualization Visualization, SimulationModel Model) +{ + private readonly double Acceleration = Model.Acceleration; + private readonly double Deceleration = Model.Deceleration; + private double AngularVelLeft; + private double AngularVelRight; + + private (double angularVelLeft, double angularVelRight) AccelerationCalculator(double wL, double wR, double wL_Current, double wR_Current) + { + var angularVelLeft = wL_Current; + var angularVelRight = wR_Current; + if (wL_Current == 0 || wL / wL_Current < 0) + { + if (wL != 0) angularVelLeft += wL / Math.Abs(wL) * Acceleration; + else angularVelLeft = wL; + } + else + { + if (Math.Abs(wL) - Math.Abs(wL_Current) > Acceleration) angularVelLeft += Acceleration * wL_Current / Math.Abs(wL_Current); + else if (Math.Abs(wL_Current) - Math.Abs(wL) > Deceleration) angularVelLeft -= Deceleration * wL_Current / Math.Abs(wL_Current); + else angularVelLeft = wL; + } + + if (wR_Current == 0 || wR / wR_Current < 0) + { + if (wR != 0) angularVelRight += wR / Math.Abs(wR) * Acceleration; + else angularVelRight = wR; + } + else + { + if (Math.Abs(wR) - Math.Abs(wR_Current) > Acceleration) angularVelRight += Acceleration * wR_Current / Math.Abs(wR_Current); + else if (Math.Abs(wR_Current) - Math.Abs(wR) > Deceleration) angularVelRight -= Deceleration * wR_Current / Math.Abs(wR_Current); + else angularVelRight = wR; + } + + if (Math.Abs(angularVelLeft) > Math.Abs(wL)) angularVelLeft = wL; + if (Math.Abs(angularVelRight) > Math.Abs(wR)) angularVelRight = wR; + return (angularVelLeft, angularVelRight); + } + + public bool SetSpeed(double angularVelLeft, double angularVelRight, double sampleTime) + { + (AngularVelLeft, AngularVelRight) = AccelerationCalculator(angularVelLeft, angularVelRight, AngularVelLeft, AngularVelRight); + //Console.WriteLine($"AngularVelLeft = {AngularVelLeft:0.####}, AngularVelRight = {AngularVelRight:0.####}"); + _ = Visualization.UpdatePosition(AngularVelLeft, AngularVelRight, sampleTime); + return true; + } + + public void Stop() + { + (AngularVelLeft, AngularVelRight) = (0, 0); + _ = Visualization.UpdatePosition(AngularVelLeft, AngularVelRight, 0.05); + } +} diff --git a/RobotApp/Services/Robot/Simulation/SimulationVisualization.cs b/RobotApp/Services/Robot/Simulation/SimulationVisualization.cs new file mode 100644 index 0000000..8fccf47 --- /dev/null +++ b/RobotApp/Services/Robot/Simulation/SimulationVisualization.cs @@ -0,0 +1,39 @@ +namespace RobotApp.Services.Robot.Simulation; + +public class SimulationVisualization(SimulationModel Model) +{ + public double X { get; private set; } + public double Y { get; private set; } + public double Theta { get; private set; } + public double Vx { get; private set; } + public double Vy { get; private set; } + public double Omega { get; private set; } + + private readonly double RadiusWheel = Model.RadiusWheel; + private readonly double RadiusRobot = Model.Width / 2; + + public (double x, double y, double angle) UpdatePosition(double wL, double wR, double time) + { + Theta = (Theta + time * (-wR - wL) * RadiusWheel / RadiusRobot * 180 / Math.PI) % 360; + X += time * (-wR + wL) * RadiusWheel * Math.Cos(Theta * Math.PI / 180) / 2; + Y += time * (-wR + wL) * RadiusWheel * Math.Sin(Theta * Math.PI / 180) / 2; + _ = UpdateVelocity(wL, wR); + if (Theta > 180) Theta -= 360; + else if (Theta < -180) Theta += 360; + return (X, Y, Theta); + } + + public (double vx, double vy, double omega) UpdateVelocity(double wL, double wR) + { + Vx = (-wR + wL) * RadiusWheel / 2; + Omega = (-wR - wL) * RadiusWheel / RadiusRobot; + return (Vx, 0, Omega); + } + + public void LocalizationInitialize(double x, double y, double theta) + { + X = x; + Y = y; + Theta = theta; + } +} diff --git a/RobotApp/Services/RobotExtensions.cs b/RobotApp/Services/RobotExtensions.cs new file mode 100644 index 0000000..6f5d31e --- /dev/null +++ b/RobotApp/Services/RobotExtensions.cs @@ -0,0 +1,39 @@ +using RobotApp.Interfaces; +using RobotApp.Services.Robot; +using RobotApp.Services.State; +using System.Diagnostics.CodeAnalysis; + +namespace RobotApp.Services; + +public static class RobotExtensions +{ + public static IServiceCollection AddRobot(this IServiceCollection services) + { + services.AddSingleton(); + + services.AddInterfaceServiceSingleton(); + return services; + } + + public static IServiceCollection AddInterfaceServiceSingleton(this IServiceCollection services) where TService : class where TImplementation : class, TService + { + services.AddSingleton(); + services.AddSingleton(sp => sp.GetRequiredService()); + return services; + } + + public static IServiceCollection AddHostedServiceSingleton<[DynamicallyAccessedMembers(DynamicallyAccessedMemberTypes.PublicConstructors)] THostedService>(this IServiceCollection services) where THostedService : class, IHostedService + { + services.AddSingleton(); + services.AddHostedService(sp => sp.GetRequiredService()); + return services; + } + + public static IServiceCollection AddHostedInterfaceServiceSingleton(this IServiceCollection services) where TService : class where THostedService : class, IHostedService, TService + { + services.AddSingleton(); + services.AddSingleton(sp => sp.GetRequiredService()); + services.AddHostedService(sp => sp.GetRequiredService()); + return services; + } +} diff --git a/RobotApp/Services/State/ACTState.cs b/RobotApp/Services/State/ACTState.cs new file mode 100644 index 0000000..2fc956d --- /dev/null +++ b/RobotApp/Services/State/ACTState.cs @@ -0,0 +1,7 @@ +using RobotApp.Common.Shares.Enums; + +namespace RobotApp.Services.State; + +public class ACTState(ACTStateType state, IRobotState? superState = null) : RobotState(state, superState) where T : Enum +{ +} diff --git a/RobotApp/Services/State/AutoState.cs b/RobotApp/Services/State/AutoState.cs new file mode 100644 index 0000000..575dff8 --- /dev/null +++ b/RobotApp/Services/State/AutoState.cs @@ -0,0 +1,7 @@ +using RobotApp.Common.Shares.Enums; + +namespace RobotApp.Services.State; + +public class AutoState(AutoStateType state, IRobotState? superState = null) : RobotState(state, superState) where T : Enum +{ +} diff --git a/RobotApp/Services/State/ExecutingState.cs b/RobotApp/Services/State/ExecutingState.cs new file mode 100644 index 0000000..84eddf2 --- /dev/null +++ b/RobotApp/Services/State/ExecutingState.cs @@ -0,0 +1,7 @@ +using RobotApp.Common.Shares.Enums; + +namespace RobotApp.Services.State; + +public class ExecutingState(ExecutingStateType state, IRobotState? superState = null) : RobotState(state, superState) where T : Enum +{ +} diff --git a/RobotApp/Services/State/FaultState.cs b/RobotApp/Services/State/FaultState.cs new file mode 100644 index 0000000..c473b6b --- /dev/null +++ b/RobotApp/Services/State/FaultState.cs @@ -0,0 +1,7 @@ +using RobotApp.Common.Shares.Enums; + +namespace RobotApp.Services.State; + +public class FaultState(FaultStateType state, IRobotState? superState = null) : RobotState(state, superState) where T : Enum +{ +} diff --git a/RobotApp/Services/State/IRobotState.cs b/RobotApp/Services/State/IRobotState.cs new file mode 100644 index 0000000..8b401a6 --- /dev/null +++ b/RobotApp/Services/State/IRobotState.cs @@ -0,0 +1,21 @@ +namespace RobotApp.Services.State; + +public interface IRobotState +{ + Enum Name { get; } + Type Type { get; } + IRobotState? SuperState { get; } + List SubStates { get; set; } + IRobotState? ActiveSubState { get;} + IRobotState? HistoryState { get;} + bool HasSubStates => SubStates.Count > 0; + bool IsActive => ActiveSubState != null; + + event Action? OnEnter; + event Action? OnExit; + event Action? OnTransition; + void Enter(); + void Exit(); + bool CanTransitionTo(IRobotState targetState); + void Update(); +} diff --git a/RobotApp/Services/State/ManualState.cs b/RobotApp/Services/State/ManualState.cs new file mode 100644 index 0000000..e79c596 --- /dev/null +++ b/RobotApp/Services/State/ManualState.cs @@ -0,0 +1,7 @@ +using RobotApp.Common.Shares.Enums; + +namespace RobotApp.Services.State; + +public class ManualState(ManualStateType state, IRobotState? superState = null) : RobotState(state, superState) where T : Enum +{ +} diff --git a/RobotApp/Services/State/MoveState.cs b/RobotApp/Services/State/MoveState.cs new file mode 100644 index 0000000..ee975c5 --- /dev/null +++ b/RobotApp/Services/State/MoveState.cs @@ -0,0 +1,7 @@ +using RobotApp.Common.Shares.Enums; + +namespace RobotApp.Services.State; + +public class MoveState(MoveStateType state, IRobotState? superState = null) : RobotState(state, superState) where T : Enum +{ +} diff --git a/RobotApp/Services/State/PlanState.cs b/RobotApp/Services/State/PlanState.cs new file mode 100644 index 0000000..dff8208 --- /dev/null +++ b/RobotApp/Services/State/PlanState.cs @@ -0,0 +1,7 @@ +using RobotApp.Common.Shares.Enums; + +namespace RobotApp.Services.State; + +public class PlanState(PlanStateType state, IRobotState? superState = null) : RobotState(state, superState) where T : Enum +{ +} diff --git a/RobotApp/Services/State/RobotState.cs b/RobotApp/Services/State/RobotState.cs index 78c63e8..5640e50 100644 --- a/RobotApp/Services/State/RobotState.cs +++ b/RobotApp/Services/State/RobotState.cs @@ -1,8 +1,38 @@ namespace RobotApp.Services.State; -public abstract class RobotState(T type) where T : Enum +public abstract class RobotState(Enum name, IRobotState? superState = null) : IRobotState where T : Enum { - public readonly T Type = type; - public virtual void Enter() { } - public virtual void Exit() { } + public Enum Name { get; private set; } = name; + public Type Type { get; private set; } = typeof(T); + public IRobotState? SuperState { get; private set; } = superState; + public List SubStates { get; set; } = []; + public IRobotState? ActiveSubState { get; protected set; } + public IRobotState? HistoryState { get; protected set; } + public bool HasSubStates => SubStates.Count > 0; + public bool IsActive => ActiveSubState != null; + public override string ToString() => $"{GetType().Name}({Name})"; + + public event Action? OnEnter; + public event Action? OnExit; + public event Action? OnTransition; + + public virtual void Enter() + { + OnEnter?.Invoke(this); + } + + public virtual void Exit() + { + OnExit?.Invoke(this); + } + + public virtual bool CanTransitionTo(IRobotState targetState) + { + return true; + } + + public virtual void Update() + { + ActiveSubState?.Update(); + } } \ No newline at end of file diff --git a/RobotApp/Services/State/RobotStateMachine.cs b/RobotApp/Services/State/RobotStateMachine.cs index e69de29..46809e6 100644 --- a/RobotApp/Services/State/RobotStateMachine.cs +++ b/RobotApp/Services/State/RobotStateMachine.cs @@ -0,0 +1,352 @@ +using RobotApp.Common.Shares.Enums; + +namespace RobotApp.Services.State; + +public record RobotStateMachine(Logger Logger) : IDisposable +{ + private readonly Dictionary> StateRegistry = []; + + public IRobotState? CurrentState { get; private set; } + public event Action? OnStateChanged; + public bool IsInitialized = false; + + private readonly Lock StateLock = new(); + + private void PrintStateAdded() + { + Console.WriteLine("=== All Registered States ==="); + var allStates = StateRegistry.Values + .SelectMany(dict => dict.Values) + .Where(state => state.SuperState == null) + .OrderBy(state => state.Type.Name) + .ThenBy(state => state.Name.ToString()); + + foreach (var rootState in allStates) + { + PrintStateSimple(rootState, 0); + } + + var totalStates = StateRegistry.Values.Sum(dict => dict.Count); + Console.WriteLine($"\nTotal: {totalStates} states registered"); + Console.WriteLine("==============================\n"); + } + + private void PrintStateSimple(IRobotState state, int depth) + { + var indent = new string(' ', depth * 4); + var marker = state == CurrentState ? " [CURRENT]" : ""; + var activeMarker = state.SuperState?.ActiveSubState == state ? " [ACTIVE]" : ""; + + Console.WriteLine($"{indent}- {state.Name}{marker}{activeMarker}"); + + foreach (var subState in state.SubStates.OrderBy(s => s.Name.ToString())) + { + PrintStateSimple(subState, depth + 1); + } + } + + public void InitializeHierarchyStates() + { + if (IsInitialized) return; + RegisterState(new RootState(RootStateType.System)); + RegisterState(new RootState(RootStateType.Auto)); + RegisterState(new RootState(RootStateType.Manual)); + RegisterState(new RootState(RootStateType.Service)); + RegisterState(new RootState(RootStateType.Stop)); + RegisterState(new RootState(RootStateType.Fault)); + SetupSystemState(); + SetupAutoState(); + SetupManualState(); + SetupServiceState(); + SetupStopState(); + SetupFaultState(); + + var systemState = GetState(RootStateType.System); + lock (StateLock) + { + systemState.Enter(); + CurrentState = systemState; + } + + PrintStateAdded(); + IsInitialized = true; + Logger.Info($"Khởi tạo thành công State Machine với State hiện tại: {GetCurrentStatePath()}"); + } + + private void SetupSystemState() + { + var systemlState = GetState(RootStateType.System) ?? throw new InvalidOperationException($"Failed to get state RootStateType.Auto"); + var initializingState = new SystemState(SystemStateType.Initializing, systemlState); + var standbyState = new SystemState(SystemStateType.Standby, systemlState); + var shutting_DownState = new SystemState(SystemStateType.Shutting_Down, systemlState); + + systemlState.SubStates.AddRange([initializingState, standbyState, shutting_DownState]); + + RegisterState(initializingState); + RegisterState(standbyState); + RegisterState(shutting_DownState); + } + + private void SetupAutoState() + { + var autoState = GetState(RootStateType.Auto) ?? throw new InvalidOperationException($"Failed to get state RootStateType.Auto"); + var idleState = new AutoState(AutoStateType.Idle, autoState); + var executingState = new AutoState(AutoStateType.Executing, autoState); + var pausedState = new AutoState(AutoStateType.Paused, autoState); + var holdingState = new AutoState(AutoStateType.Holding, autoState); + var cancelingState = new AutoState(AutoStateType.Canceling, autoState); + var recoveringState = new AutoState(AutoStateType.Recovering, autoState); + var remote_Override_State = new AutoState(AutoStateType.Remote_Override, autoState); + + autoState.SubStates.AddRange([ idleState, executingState, pausedState, holdingState, + cancelingState, recoveringState, remote_Override_State ]); + + RegisterState(idleState); + RegisterState(executingState); + RegisterState(pausedState); + RegisterState(holdingState); + RegisterState(cancelingState); + RegisterState(recoveringState); + RegisterState(remote_Override_State); + + SetupExecutingState(); + } + + private void SetupManualState() + { + var manualState = GetState(RootStateType.Manual) ?? throw new InvalidOperationException($"Failed to get state RootStateType.Auto"); + var idleState = new ManualState(ManualStateType.Idle, manualState); + var executingState = new ManualState(ManualStateType.Active, manualState); + + manualState.SubStates.AddRange([idleState, executingState]); + + RegisterState(idleState); + RegisterState(executingState); + } + + private void SetupServiceState() + { + var serviceState = GetState(RootStateType.Service) ?? throw new InvalidOperationException($"Failed to get state RootStateType.Auto"); + var idleState = new ServiceState(ServiceStateType.Idle, serviceState); + var executingState = new ServiceState(ServiceStateType.Active, serviceState); + + serviceState.SubStates.AddRange([idleState, executingState]); + + RegisterState(idleState); + RegisterState(executingState); + } + + private void SetupStopState() + { + var stopState = GetState(RootStateType.Stop) ?? throw new InvalidOperationException($"Failed to get state RootStateType.Auto"); + var emcStopState = new StopState(StopStateType.EMC, stopState); + var protectiveState = new StopState(StopStateType.Protective, stopState); + var manualStopState = new StopState(StopStateType.Manual, stopState); + + stopState.SubStates.AddRange([emcStopState, protectiveState, manualStopState]); + + RegisterState(emcStopState); + RegisterState(protectiveState); + RegisterState(manualStopState); + } + + private void SetupFaultState() + { + var faultState = GetState(RootStateType.Fault) ?? throw new InvalidOperationException($"Failed to get state RootStateType.Auto"); + var navigationFaultState = new FaultState(FaultStateType.Navigation, faultState); + var localizationFaultState = new FaultState(FaultStateType.Localization, faultState); + var shelftFaultState = new FaultState(FaultStateType.Shielf, faultState); + var batteryFaultState = new FaultState(FaultStateType.Battery, faultState); + var driverFaultState = new FaultState(FaultStateType.Driver, faultState); + var peripheralsFaultState = new FaultState(FaultStateType.Peripherals, faultState); + var safetyFaultState = new FaultState(FaultStateType.Safety, faultState); + var comunicationFaultState = new FaultState(FaultStateType.Communication, faultState); + + faultState.SubStates.AddRange([navigationFaultState, localizationFaultState, shelftFaultState, + batteryFaultState, driverFaultState, peripheralsFaultState, safetyFaultState, + comunicationFaultState]); + + RegisterState(navigationFaultState); + RegisterState(localizationFaultState); + RegisterState(shelftFaultState); + RegisterState(batteryFaultState); + RegisterState(driverFaultState); + RegisterState(peripheralsFaultState); + RegisterState(safetyFaultState); + RegisterState(comunicationFaultState); + } + + private void SetupExecutingState() + { + var executingState = GetState(AutoStateType.Executing) ?? throw new InvalidOperationException($"Failed to get state RootStateType.Auto"); + var planningState = new ExecutingState(ExecutingStateType.Planning, executingState); + var movingState = new ExecutingState(ExecutingStateType.Moving, executingState); + var actStopState = new ExecutingState(ExecutingStateType.ACT, executingState); + + executingState.SubStates.AddRange([planningState, movingState, actStopState]); + + RegisterState(planningState); + RegisterState(movingState); + RegisterState(actStopState); + + SetupACTState(); + SetupMoveState(); + SetupPlanState(); + } + + private void SetupACTState() + { + var ACTState = GetState(ExecutingStateType.ACT) ?? throw new InvalidOperationException($"Failed to get state RootStateType.Auto"); + var dockingState = new ACTState(ACTStateType.Docking, ACTState); + var dockedState = new ACTState(ACTStateType.Docked, ACTState); + var chargingState = new ACTState(ACTStateType.Charging, ACTState); + var undockingState = new ACTState(ACTStateType.Undocking, ACTState); + var loadingState = new ACTState(ACTStateType.Loading, ACTState); + var unLoadingState = new ACTState(ACTStateType.Unloading, ACTState); + var techActionState = new ACTState(ACTStateType.TechAction, ACTState); + + ACTState.SubStates.AddRange([dockingState, dockedState, chargingState, undockingState, + loadingState, unLoadingState, techActionState]); + + RegisterState(dockingState); + RegisterState(dockedState); + RegisterState(chargingState); + RegisterState(undockingState); + RegisterState(loadingState); + RegisterState(unLoadingState); + RegisterState(techActionState); + } + + private void SetupMoveState() + { + var moveState = GetState(ExecutingStateType.Moving) ?? throw new InvalidOperationException($"Failed to get state RootStateType.Auto"); + var navigationState = new MoveState(MoveStateType.Navigation, moveState); + var avoidabceState = new MoveState(MoveStateType.Avoidance, moveState); + var approachState = new MoveState(MoveStateType.Approach, moveState); + var trackingState = new MoveState(MoveStateType.Tracking, moveState); + var repositioningState = new MoveState(MoveStateType.Repositioning, moveState); + + moveState.SubStates.AddRange([navigationState, avoidabceState, approachState, trackingState, repositioningState]); + + RegisterState(navigationState); + RegisterState(avoidabceState); + RegisterState(approachState); + RegisterState(trackingState); + RegisterState(repositioningState); + } + + private void SetupPlanState() + { + var planState = GetState(ExecutingStateType.Planning) ?? throw new InvalidOperationException($"Failed to get state RootStateType.Auto"); + var taskPlanState = new PlanState(PlanStateType.Task, planState); + var pathPlanState = new PlanState(PlanStateType.Path, planState); + + planState.SubStates.AddRange([taskPlanState, pathPlanState]); + + RegisterState(taskPlanState); + RegisterState(pathPlanState); + } + + private void RegisterState(RobotState state) where T : Enum + { + var enumType = typeof(T); + StateRegistry.TryAdd(enumType, []); + StateRegistry[enumType][state.Name] = state; + } + + public RobotState? TryGetState(T stateType) where T : Enum + { + var enumType = typeof(T); + if (StateRegistry.TryGetValue(enumType, out var states) && states.TryGetValue(stateType, out var state) && state is RobotState robotState) + { + return robotState; + } + return null; + } + + public RobotState GetState(T stateType) where T : Enum + { + return TryGetState(stateType) ?? throw new ArgumentException($"Failed to get state {stateType}"); + } + + public bool TransitionTo(T stateType) where T : Enum + { + if (!IsInitialized) + { + Logger.Warning("State Machine chưa được khởi tạo"); + return false; + } + lock (StateLock) + { + try + { + var targetState = TryGetState(stateType); + if (targetState == null) + { + Logger.Warning($"State {stateType} không được tìm thấy"); + return false; + } + + if (CurrentState != null) + { + if (!CurrentState.CanTransitionTo(targetState)) + { + Logger.Warning($"Không thể chuyển State từ {CurrentState.Name} thành {targetState.Name}"); + return false; + } + + CurrentState.Exit(); + } + + var previousState = CurrentState; + CurrentState = targetState; + CurrentState.Enter(); + + Logger.Info($"Chuyển đổi State thành công: {GetStatePath(previousState)} -> {GetStatePath(targetState)}"); + OnStateChanged?.Invoke(previousState, CurrentState); + return true; + } + catch (Exception ex) + { + Logger.Error($"Lỗi khi chuyển đổi State: {ex.Message}"); + return false; + } + } + } + + public void Update() + { + if (!IsInitialized) + { + Logger.Warning("State Machine chưa được khởi tạo"); + return; + } + lock (StateLock) + { + CurrentState?.Update(); + } + } + + public string GetCurrentStatePath() + { + if (CurrentState == null) return "No State"; + return GetStatePath(CurrentState); + } + + private static string GetStatePath(IRobotState? state) + { + var path = new List(); + var current = state; + while (current != null) + { + path.Insert(0, current.Name.ToString()); + current = current.SuperState; + } + return string.Join(".", path); + } + + public void Dispose() + { + GC.SuppressFinalize(this); + } +} \ No newline at end of file diff --git a/RobotApp/Services/State/RootState.cs b/RobotApp/Services/State/RootState.cs new file mode 100644 index 0000000..46dff3d --- /dev/null +++ b/RobotApp/Services/State/RootState.cs @@ -0,0 +1,36 @@ +using RobotApp.Common.Shares.Enums; + +namespace RobotApp.Services.State; + +public class RootState(RootStateType state, IRobotState? superState = null) : RobotState(state, superState) where T : Enum +{ + public override void Enter() + { + switch(Name) + { + case RootStateType.System: + ActiveSubState = SubStates.FirstOrDefault(s => s.Name.Equals(SystemStateType.Initializing)); + ActiveSubState?.Enter(); + break; + case RootStateType.Auto: + if(HistoryState is not null) ActiveSubState = HistoryState; + else ActiveSubState = SubStates.FirstOrDefault(s => s.Name.Equals(AutoStateType.Idle)); + ActiveSubState?.Enter(); + break; + } + } + + public override void Exit() + { + } + + public override bool CanTransitionTo(IRobotState targetState) + { + return true; + } + + public override void Update() + { + ActiveSubState?.Update(); + } +} diff --git a/RobotApp/Services/State/ServiceState.cs b/RobotApp/Services/State/ServiceState.cs new file mode 100644 index 0000000..cbf8f48 --- /dev/null +++ b/RobotApp/Services/State/ServiceState.cs @@ -0,0 +1,7 @@ +using RobotApp.Common.Shares.Enums; + +namespace RobotApp.Services.State; + +public class ServiceState(ServiceStateType state, IRobotState? superState = null) : RobotState(state, superState) where T : Enum +{ +} diff --git a/RobotApp/Services/State/StopState.cs b/RobotApp/Services/State/StopState.cs new file mode 100644 index 0000000..3e89182 --- /dev/null +++ b/RobotApp/Services/State/StopState.cs @@ -0,0 +1,8 @@ +using RobotApp.Common.Shares.Enums; + +namespace RobotApp.Services.State +{ + public class StopState(StopStateType state, IRobotState? superState = null) : RobotState(state, superState) where T : Enum + { + } +} diff --git a/RobotApp/Services/State/SystemState.cs b/RobotApp/Services/State/SystemState.cs new file mode 100644 index 0000000..f6d8f9d --- /dev/null +++ b/RobotApp/Services/State/SystemState.cs @@ -0,0 +1,21 @@ +using RobotApp.Common.Shares.Enums; + +namespace RobotApp.Services.State; + +public class SystemState(SystemStateType state, IRobotState? superState = null) : RobotState(state, superState) where T : Enum +{ + public override void Enter() + { + } + public override void Exit() + { + } + public override bool CanTransitionTo(IRobotState targetState) + { + return true; + } + public override void Update() + { + ActiveSubState?.Update(); + } +} diff --git a/RobotApp/appsettings.json b/RobotApp/appsettings.json index e36d9c8..ce4af3a 100644 --- a/RobotApp/appsettings.json +++ b/RobotApp/appsettings.json @@ -5,8 +5,20 @@ "Logging": { "LogLevel": { "Default": "Information", - "Microsoft.AspNetCore": "Warning" + "Microsoft.AspNetCore": "Warning", + "Microsoft.EntityFrameworkCore": "Warning" } }, - "AllowedHosts": "*" + "AllowedHosts": "*", + "Simulation": { + "Enable": true, + "RadiusWheel": 0.1, + "Width": 0.606, + "Length": 1.106, + "MaxVelocity": 1.5, + "MaxAngularVelocity": 0.3, + "Acceleration": 2, + "Deceleration": 10, + "NavigationType": "Differential" + } }