From 90dcb67b6055124b3bc642ec05d1f5fbabcdff6d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C4=90=C4=83ng=20Nguy=E1=BB=85n?= Date: Thu, 16 Oct 2025 14:53:22 +0700 Subject: [PATCH] update --- RobotApp.Common.Shares/Enums/StateType.cs | 1 + RobotApp.VDA5050/State/Error.cs | 1 + RobotApp/Interfaces/IDriver.cs | 8 +- RobotApp/Interfaces/IError.cs | 3 +- RobotApp/Interfaces/IPeripheral.cs | 58 +++- RobotApp/Interfaces/ISafety.cs | 29 +- RobotApp/Interfaces/ISensorIMU.cs | 17 -- RobotApp/Program.cs | 1 - RobotApp/RobotApp.csproj | 4 + RobotApp/Services/ModbusTcpClient.cs | 2 +- RobotApp/Services/Robot/RobotConfiguration.cs | 19 +- RobotApp/Services/Robot/RobotConnection.cs | 4 +- RobotApp/Services/Robot/RobotController.cs | 21 +- .../Robot/RobotControllerInitialize.cs | 110 +++++++ RobotApp/Services/Robot/RobotDriver.cs | 22 ++ RobotApp/Services/Robot/RobotErrors.cs | 40 ++- RobotApp/Services/Robot/RobotInfomations.cs | 6 +- RobotApp/Services/Robot/RobotLocalization.cs | 4 +- RobotApp/Services/Robot/RobotNavigation.cs | 16 +- RobotApp/Services/Robot/RobotPeripheral.cs | 287 +++++++++++++++--- .../Services/Robot/RobotPeripheralAddress.cs | 68 +++++ .../Services/Robot/RobotPeripheralData.cs | 32 ++ RobotApp/Services/Robot/RobotVisualization.cs | 48 ++- .../Navigation/DifferentialNavigation.cs | 10 +- .../Navigation/SimulationNavigation.cs | 11 +- .../Robot/Simulation/SimulationExtensions.cs | 1 - .../Robot/Simulation/SimulationModel.cs | 19 +- .../Simulation/SimulationVisualization.cs | 6 +- RobotApp/Services/RobotExtensions.cs | 23 ++ RobotApp/Services/RobotStateMachineService.cs | 0 RobotApp/Services/State/AutoState.cs | 28 ++ RobotApp/Services/State/FaultState.cs | 8 + RobotApp/Services/State/ManualState.cs | 12 +- RobotApp/Services/State/RobotStateMachine.cs | 98 +++--- RobotApp/Services/State/RootState.cs | 84 ++++- RobotApp/Services/State/ServiceState.cs | 10 + RobotApp/Services/State/StopState.cs | 11 +- RobotApp/Services/State/SystemState.cs | 36 ++- 38 files changed, 934 insertions(+), 224 deletions(-) delete mode 100644 RobotApp/Interfaces/ISensorIMU.cs create mode 100644 RobotApp/Services/Robot/RobotControllerInitialize.cs create mode 100644 RobotApp/Services/Robot/RobotDriver.cs create mode 100644 RobotApp/Services/Robot/RobotPeripheralAddress.cs create mode 100644 RobotApp/Services/Robot/RobotPeripheralData.cs delete mode 100644 RobotApp/Services/RobotStateMachineService.cs diff --git a/RobotApp.Common.Shares/Enums/StateType.cs b/RobotApp.Common.Shares/Enums/StateType.cs index 84b9b98..745b93e 100644 --- a/RobotApp.Common.Shares/Enums/StateType.cs +++ b/RobotApp.Common.Shares/Enums/StateType.cs @@ -43,6 +43,7 @@ public enum ServiceStateType public enum StopStateType { EMC, + Bumber, Protective, Manual, } diff --git a/RobotApp.VDA5050/State/Error.cs b/RobotApp.VDA5050/State/Error.cs index 9fb6eb1..926d004 100644 --- a/RobotApp.VDA5050/State/Error.cs +++ b/RobotApp.VDA5050/State/Error.cs @@ -20,6 +20,7 @@ public class ErrorReferences public enum ErrorType { INITIALIZE_ORDER, + READ_PERIPHERAL_FAILURE, } public class Error diff --git a/RobotApp/Interfaces/IDriver.cs b/RobotApp/Interfaces/IDriver.cs index 1224c57..c52d1c2 100644 --- a/RobotApp/Interfaces/IDriver.cs +++ b/RobotApp/Interfaces/IDriver.cs @@ -11,14 +11,14 @@ public interface IDriver bool IsReady { get; } /// - /// Tốc độ động cơ bên trái + /// Tốc độ di chuyển thẳng của robot (m/s) /// - double LeftVelocity { get; } + double LinearVelocity { get; } /// - /// Tốc độ động cơ bên phải + /// Tốc độ quay của robot (rad/s) /// - double RightVelocity { get; } + double AngularVelocity { get; } /// /// Điều khiển tốc độ động cơ trái và phải diff --git a/RobotApp/Interfaces/IError.cs b/RobotApp/Interfaces/IError.cs index 17db062..09ff344 100644 --- a/RobotApp/Interfaces/IError.cs +++ b/RobotApp/Interfaces/IError.cs @@ -6,6 +6,7 @@ public interface IError { bool HasFatalError { get; } void AddError(Error error, TimeSpan? clearAfter = null); - void DeleteError(string errorType); + void DeleteErrorType(string errorType); + void DeleteErrorHint(string hint); void ClearAllErrors(); } diff --git a/RobotApp/Interfaces/IPeripheral.cs b/RobotApp/Interfaces/IPeripheral.cs index 1290792..86419e2 100644 --- a/RobotApp/Interfaces/IPeripheral.cs +++ b/RobotApp/Interfaces/IPeripheral.cs @@ -1,35 +1,75 @@ -namespace RobotApp.Interfaces; +using RobotApp.Common.Shares.Enums; + +namespace RobotApp.Interfaces; public enum PeripheralMode { - AUTOMATIC, + AUTO, MANUAL, SERVICE, } + +public enum PeripheralButton +{ + Start, + Reset, + Stop, +} + +public enum SystemState +{ + INIT, + NOPOSE, + PAUSED, + IDLE, + PROCCESSING, + CHARGING, + OVERRIDE, + ERROR, + NONE, +} + +public enum ProccessingState +{ + Move, + Lifting, + LiftRotating, + None, +} + public interface IPeripheral { + event Action? OnPeripheralModeChanged; + event Action? OnButtonPressed; + event Action? OnStop; + bool IsReady { get; } PeripheralMode PeripheralMode { get; } + bool Emergency { get; } bool Bumper { get; } + bool LidarFrontProtectField { get; } + bool LidarBackProtectField { get; } + bool LidarFrontTimProtectField { get; } + bool LiftedUp { get; } bool LiftedDown { get; } bool LiftHome { get; } + bool LeftMotorReady { get; } bool RightMotorReady { get; } bool LiftMotorReady { get; } - bool SwitchLock { get; } - bool SwitchManual { get; } - bool SwitchAuto { get; } + bool ButtonStart { get; } bool ButtonStop { get; } bool ButtonReset { get; } + bool HasLoad { get; } bool MutedBase { get; } bool MutedLoad { get; } - bool StartedCharging { get; } + bool EnabledCharging { get; } - bool SetSytemState(); - bool SetProccessState(); - bool SetOnCharging(); + void SetSytemState(SystemState state); + void SetProccessState(ProccessingState state); + void SetOnCharging(bool value); } diff --git a/RobotApp/Interfaces/ISafety.cs b/RobotApp/Interfaces/ISafety.cs index 8351a4e..6b73636 100644 --- a/RobotApp/Interfaces/ISafety.cs +++ b/RobotApp/Interfaces/ISafety.cs @@ -1,18 +1,21 @@ namespace RobotApp.Interfaces; +public enum SafetySpeed +{ + Very_Slow, + Slow, + Normal, + Medium, + Optimal, + Fast, + Very_Fast +} + public interface ISafety { - bool SpeedVerySlow { get; } - 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(); + event Action? OnSafetySpeedChanged; + SafetySpeed SafetySpeed { get; } + void SetMutedLoad(bool muted); + void SetMutedBase(bool muted); + void SetHorizontalLoad(bool value); } diff --git a/RobotApp/Interfaces/ISensorIMU.cs b/RobotApp/Interfaces/ISensorIMU.cs deleted file mode 100644 index 68a6b92..0000000 --- a/RobotApp/Interfaces/ISensorIMU.cs +++ /dev/null @@ -1,17 +0,0 @@ -namespace RobotApp.Interfaces; - -/// -/// Interface cảm biến IMU -/// -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ị độ) - /// - double Angle { get; } -} diff --git a/RobotApp/Program.cs b/RobotApp/Program.cs index dd6b94f..358ace5 100644 --- a/RobotApp/Program.cs +++ b/RobotApp/Program.cs @@ -47,7 +47,6 @@ builder.Services.AddIdentity(options => builder.Services.AddSingleton, IdentityNoOpEmailSender>(); builder.Services.AddSingleton(typeof(RobotApp.Services.Logger<>)); -builder.Services.AddSingleton(); builder.Services.AddRobotSimulation(); builder.Services.AddRobot(); diff --git a/RobotApp/RobotApp.csproj b/RobotApp/RobotApp.csproj index 003dbb1..b89db01 100644 --- a/RobotApp/RobotApp.csproj +++ b/RobotApp/RobotApp.csproj @@ -29,4 +29,8 @@ + + + + diff --git a/RobotApp/Services/ModbusTcpClient.cs b/RobotApp/Services/ModbusTcpClient.cs index 64a2c35..620bad3 100644 --- a/RobotApp/Services/ModbusTcpClient.cs +++ b/RobotApp/Services/ModbusTcpClient.cs @@ -7,7 +7,7 @@ namespace RobotApp.Services; public class ModbusTcpClient(string IpAddress, int Port, byte ClientId) : IDisposable { - public bool IsConnected => !disposed && tcpClient != null && tcpClient.Client.Connected && stream != null; + public bool IsConnected => !disposed && tcpClient != null && tcpClient.Client.Connected && tcpClient.Connected && stream != null; private TcpClient? tcpClient; private NetworkStream? stream; diff --git a/RobotApp/Services/Robot/RobotConfiguration.cs b/RobotApp/Services/Robot/RobotConfiguration.cs index c2dfc59..54a7c22 100644 --- a/RobotApp/Services/Robot/RobotConfiguration.cs +++ b/RobotApp/Services/Robot/RobotConfiguration.cs @@ -1,22 +1,25 @@ using RobotApp.Common.Shares.Enums; +using RobotApp.Services.Robot.Simulation; namespace RobotApp.Services.Robot; public class RobotConfiguration { - public static string SerialNumber { get; set; } = "Robot-Demo"; - public static NavigationType NavigationType { get; set; } = NavigationType.Differential; - public static VDA5050.VDA5050Setting VDA5050Setting { get; set; } = new() + public string SerialNumber { get; set; } = "Robot-Demo"; + public NavigationType NavigationType { get; set; } = NavigationType.Differential; + public VDA5050.VDA5050Setting VDA5050Setting { get; set; } = new() { - HostServer = "172.20.235.170", - Port = 1885, + HostServer = "172.20.235.176", + Port = 1883, UserName = "robotics", Password = "robotics", Manufacturer = "PhenikaaX", Version = "0.0.1", PublishRepeat = 2, }; - public static string PLCAddress { get; set; } = "127.0.0.1"; - public static int PLCPort { get; set; } = 502; - public static int PLCUnitId { get; set; } = 1; + public string PLCAddress { get; set; } = "127.0.0.1"; + public int PLCPort { get; set; } = 502; + public byte PLCUnitId { get; set; } = 1; + public bool IsSimulation { get; set; } = true; + public SimulationModel SimulationModel { get; set; } = new(); } diff --git a/RobotApp/Services/Robot/RobotConnection.cs b/RobotApp/Services/Robot/RobotConnection.cs index dbd4511..8aca3a2 100644 --- a/RobotApp/Services/Robot/RobotConnection.cs +++ b/RobotApp/Services/Robot/RobotConnection.cs @@ -6,13 +6,13 @@ using System.Text.Json; namespace RobotApp.Services.Robot; -public class RobotConnection(Logger Logger, Logger MQTTClientLogger) +public class RobotConnection(RobotConfiguration RobotConfiguration, Logger Logger, Logger MQTTClientLogger) { private readonly VDA5050Setting VDA5050Setting = RobotConfiguration.VDA5050Setting; private MQTTClient? MqttClient; public bool IsConnected => MqttClient is not null && MqttClient.IsConnected; - public readonly static string SerialNumber = RobotConfiguration.SerialNumber; + public readonly string SerialNumber = RobotConfiguration.SerialNumber; private void OrderChanged(string data) { diff --git a/RobotApp/Services/Robot/RobotController.cs b/RobotApp/Services/Robot/RobotController.cs index cc99562..06f97ce 100644 --- a/RobotApp/Services/Robot/RobotController.cs +++ b/RobotApp/Services/Robot/RobotController.cs @@ -6,7 +6,7 @@ using RobotApp.VDA5050.Order; namespace RobotApp.Services.Robot; -public class RobotController(IOrder OrderManager, +public partial class RobotController(IOrder OrderManager, INavigation NavigationManager, IInstantActions ActionManager, IBattery BatteryManager, @@ -25,25 +25,18 @@ public class RobotController(IOrder OrderManager, private WatchTimer? UpdateStateTimer; private const int UpdateStateInterval = 1000; - protected override Task ExecuteAsync(CancellationToken stoppingToken) + protected override async Task ExecuteAsync(CancellationToken stoppingToken) { - InitializationingHandler(); + await InitializationingHandler(stoppingToken); UpdateStateTimer = new(UpdateStateInterval, UpdateStateHandler, Logger); UpdateStateTimer.Start(); - return Task.CompletedTask; } - private void InitializationingHandler() + public override async Task StopAsync(CancellationToken cancellationToken) { - try - { - StateManager.InitializeHierarchyStates(); - } - catch - { - - } + StopHandler(); + await base.StopAsync(cancellationToken); } private void UpdateStateHandler() @@ -57,12 +50,12 @@ public class RobotController(IOrder OrderManager, { try { + if (PeripheralManager.PeripheralMode != PeripheralMode.AUTO) throw new OrderException(RobotErrors.Error1006(PeripheralManager.PeripheralMode)); if (!string.IsNullOrEmpty(OrderManager.OrderId)) { if (order.OrderId != OrderManager.OrderId) throw new OrderException(RobotErrors.Error1001(OrderManager.OrderId, order.OrderId)); OrderManager.UpdateOrder(order.OrderUpdateId, order.Nodes, order.Edges); } - else if (PeripheralManager.PeripheralMode != PeripheralMode.AUTOMATIC) throw new OrderException(RobotErrors.Error1006(PeripheralManager.PeripheralMode)); else if (ActionManager.HasActionRunning) throw new OrderException(RobotErrors.Error1007()); else if (ErrorManager.HasFatalError) throw new OrderException(RobotErrors.Error1008()); else if (NavigationManager.Driving) throw new OrderException(RobotErrors.Error1009()); diff --git a/RobotApp/Services/Robot/RobotControllerInitialize.cs b/RobotApp/Services/Robot/RobotControllerInitialize.cs new file mode 100644 index 0000000..3965f67 --- /dev/null +++ b/RobotApp/Services/Robot/RobotControllerInitialize.cs @@ -0,0 +1,110 @@ +using RobotApp.Common.Shares.Enums; +using RobotApp.Interfaces; + +namespace RobotApp.Services.Robot; + +public partial class RobotController +{ + private readonly AutoResetEvent StartButtonPressedEvent = new(false); + private readonly AutoResetEvent StopButtonPressedEvent = new(false); + private readonly AutoResetEvent ResetButtonPressedEvent = new(false); + private async Task InitializationingHandler(CancellationToken cancellationToken) + { + try + { + StateManager.InitializeHierarchyStates(); + PeripheralManager.SetSytemState(SystemState.INIT); + PeripheralManager.OnPeripheralModeChanged += SwichModeChanged; + PeripheralManager.OnButtonPressed += OnButtonPressed; + PeripheralManager.OnStop += OnStop; + + while (!cancellationToken.IsCancellationRequested) + { + if (StateManager.CurrentStateName == Enum.GetName(SystemStateType.Initializing)) + { + if (PeripheralManager.IsReady && + //PeripheralManager.LiftMotorReady && + //PeripheralManager.LeftMotorReady && + //PeripheralManager.RightMotorReady && + BatteryManager.IsReady && + Localization.IsReady && + NavigationManager.IsReady) break; + } + await Task.Delay(1000, cancellationToken); + } + Logger.Info("Robot đã khởi tạo xong. Đang kết nối tới Fleet Manager."); + + await ConnectionManager.StartConnection(cancellationToken); + Logger.Info("Robot đã kết nối tới Fleet Manager."); + StateManager.TransitionTo(SystemStateType.Standby); + + StartButtonPressedEvent.Reset(); + Logger.Info("Chờ nút Start được nhấn để vào chế độ hoạt động..."); + if (StartButtonPressedEvent.WaitOne()) + { + StateManager.TransitionTo(RootStateType.Auto); + PeripheralManager.SetSytemState(SystemState.IDLE); + } + } + catch (Exception ex) + { + Logger.Error($"Khởi tạo RobotController xảy ra lỗi: {ex.Message} - {ex.StackTrace}"); + } + } + + private void StopHandler() + { + UpdateStateTimer?.Stop(); + PeripheralManager.OnPeripheralModeChanged -= SwichModeChanged; + PeripheralManager.OnButtonPressed -= OnButtonPressed; + PeripheralManager.OnStop -= OnStop; + } + + private void SwichModeChanged(PeripheralMode mode) + { + switch (mode) + { + case PeripheralMode.AUTO: + if (StartButtonPressedEvent.WaitOne()) + { + StateManager.TransitionTo(RootStateType.Auto); + PeripheralManager.SetSytemState(SystemState.IDLE); + } + break; + case PeripheralMode.MANUAL: + StateManager.TransitionTo(RootStateType.Manual); + PeripheralManager.SetSytemState(SystemState.NONE); + break; + case PeripheralMode.SERVICE: + StateManager.TransitionTo(RootStateType.Service); + PeripheralManager.SetSytemState(SystemState.NONE); + break; + default: + break; + } + } + + private void OnButtonPressed(PeripheralButton buttonPressed) + { + switch(buttonPressed) + { + case PeripheralButton.Start: + StartButtonPressedEvent.Set(); + break; + case PeripheralButton.Reset: + ResetButtonPressedEvent.Set(); + break; + case PeripheralButton.Stop: + StopButtonPressedEvent.Set(); + break; + default: + break; + } + } + + private void OnStop(StopStateType state) + { + StateManager.TransitionToStop(state); + PeripheralManager.SetSytemState(SystemState.PAUSED); + } +} diff --git a/RobotApp/Services/Robot/RobotDriver.cs b/RobotApp/Services/Robot/RobotDriver.cs new file mode 100644 index 0000000..c950b51 --- /dev/null +++ b/RobotApp/Services/Robot/RobotDriver.cs @@ -0,0 +1,22 @@ +using RobotApp.Interfaces; + +namespace RobotApp.Services.Robot; + +public class RobotDriver : IDriver +{ + public bool IsReady => throw new NotImplementedException(); + + public double LinearVelocity => throw new NotImplementedException(); + + public double AngularVelocity => throw new NotImplementedException(); + + public bool ControlVelocity(double left, double right) + { + throw new NotImplementedException(); + } + + public bool Stop(bool isFree) + { + throw new NotImplementedException(); + } +} diff --git a/RobotApp/Services/Robot/RobotErrors.cs b/RobotApp/Services/Robot/RobotErrors.cs index f7eb7ec..4e2f750 100644 --- a/RobotApp/Services/Robot/RobotErrors.cs +++ b/RobotApp/Services/Robot/RobotErrors.cs @@ -1,13 +1,14 @@ -using Microsoft.AspNetCore.Components; -using RobotApp.Interfaces; +using RobotApp.Interfaces; using RobotApp.VDA5050.State; namespace RobotApp.Services.Robot; -public class RobotErrors +public class RobotErrors : IError { private readonly List Errors = []; + public bool HasFatalError => Errors.Any(e => e.ErrorLevel == ErrorLevel.FATAL.ToString()); + public void AddError(Error error, TimeSpan? clearAfter = null) { lock (Errors) @@ -22,12 +23,36 @@ public class RobotErrors await Task.Delay(clearAfter.Value); lock (Errors) { - Errors.RemoveAll(e => e.ErrorType == error.ErrorType); + Errors.RemoveAll(e => e.ErrorHint == error.ErrorHint); } }); } } + public void DeleteErrorType(string errorType) + { + lock (Errors) + { + Errors.RemoveAll(e => e.ErrorType == errorType); + } + } + + public void DeleteErrorHint(string errorHint) + { + lock (Errors) + { + Errors.RemoveAll(e => e.ErrorHint == errorHint); + } + } + + public void ClearAllErrors() + { + lock (Errors) + { + Errors.Clear(); + } + } + public static Error CreateError(ErrorType type, string hint, ErrorLevel level, string description) { return new Error() @@ -70,4 +95,11 @@ public class RobotErrors => CreateError(ErrorType.INITIALIZE_ORDER, "1014", ErrorLevel.WARNING, $"Edge {edgeId} chứa StartNodeId {nodeId} không tồn tại trong Nodes"); public static Error Error1015(string edgeId, string nodeId) => CreateError(ErrorType.INITIALIZE_ORDER, "1015", ErrorLevel.WARNING, $"Edge {edgeId} chứa {nodeId} không tồn tại trong Nodes"); + + public static Error Error2001() + => CreateError(ErrorType.READ_PERIPHERAL_FAILURE, "2001", ErrorLevel.FATAL, "Có lỗi xảy ra trong quá trình đọc tín hiệu từ hệ thống ngoại vi(PLC)"); + public static Error Error2002() + => CreateError(ErrorType.READ_PERIPHERAL_FAILURE, "2002", ErrorLevel.FATAL, "Có lỗi xảy ra trong quá trình gửi tín hiệu tới hệ thống ngoại vi(PLC)"); + public static Error Error2003() + => CreateError(ErrorType.READ_PERIPHERAL_FAILURE, "2003", ErrorLevel.FATAL, "Mất kết nối với hệ thống ngoại vi(PLC)"); } diff --git a/RobotApp/Services/Robot/RobotInfomations.cs b/RobotApp/Services/Robot/RobotInfomations.cs index ad5299b..b329cff 100644 --- a/RobotApp/Services/Robot/RobotInfomations.cs +++ b/RobotApp/Services/Robot/RobotInfomations.cs @@ -1,6 +1,8 @@ -namespace RobotApp.Services.Robot +using RobotApp.Interfaces; + +namespace RobotApp.Services.Robot { - public class RobotInfomations + public class RobotInfomations : IInfomation { } } diff --git a/RobotApp/Services/Robot/RobotLocalization.cs b/RobotApp/Services/Robot/RobotLocalization.cs index 27b8fcc..7d5c30f 100644 --- a/RobotApp/Services/Robot/RobotLocalization.cs +++ b/RobotApp/Services/Robot/RobotLocalization.cs @@ -17,7 +17,7 @@ public class Xloc public bool IsReady { get; set; } } -public class RobotLocalization(IConfiguration Configuration, SimulationVisualization SimVisualization) : ILocalization +public class RobotLocalization(RobotConfiguration RobotConfiguration, SimulationVisualization SimVisualization) : ILocalization { public double X => IsSimulation ? SimVisualization.X : Xloc.X; @@ -39,7 +39,7 @@ public class RobotLocalization(IConfiguration Configuration, SimulationVisualiza private readonly Xloc Xloc = new(); - private readonly bool IsSimulation = Configuration.GetValue("Robot:Simulation", false); + private readonly bool IsSimulation = RobotConfiguration.IsSimulation; public MessageResult ActivateMap(string mapName) { diff --git a/RobotApp/Services/Robot/RobotNavigation.cs b/RobotApp/Services/Robot/RobotNavigation.cs index 2ce534a..5d3b6c4 100644 --- a/RobotApp/Services/Robot/RobotNavigation.cs +++ b/RobotApp/Services/Robot/RobotNavigation.cs @@ -1,11 +1,10 @@ using RobotApp.Interfaces; -using RobotApp.Services.Robot.Simulation; using RobotApp.Services.Robot.Simulation.Navigation; using RobotApp.VDA5050.Order; namespace RobotApp.Services.Robot; -public class RobotNavigation(SimulationModel SimModel) : INavigation +public class RobotNavigation : INavigation { public bool IsReady => IsSimulation; public bool Driving => IsSimulation ? SimNavigation is not null ? SimNavigation.Driving : false : false; @@ -14,10 +13,17 @@ public class RobotNavigation(SimulationModel SimModel) : INavigation public double Omega => IsSimulation ? SimNavigation is not null ? SimNavigation.Omega : 0 : 0; public NavigationState State => IsSimulation ? SimNavigation is not null ? SimNavigation.State : NavigationState.None : NavigationState.None; + private readonly SimulationNavigation? SimNavigation; + private readonly bool IsSimulation; - private SimulationNavigation? SimNavigation; - private bool IsSimulation => SimModel.Enable; - + public RobotNavigation(RobotConfiguration RobotConfiguration, IServiceProvider ServiceProvider) + { + IsSimulation = RobotConfiguration.IsSimulation; + if (IsSimulation) + { + SimNavigation = SimulationNavigationManager.GetNavigation(RobotConfiguration.SimulationModel.NavigationType, ServiceProvider); + } + } public void CancelMovement() { diff --git a/RobotApp/Services/Robot/RobotPeripheral.cs b/RobotApp/Services/Robot/RobotPeripheral.cs index 7ea64df..7d63cd8 100644 --- a/RobotApp/Services/Robot/RobotPeripheral.cs +++ b/RobotApp/Services/Robot/RobotPeripheral.cs @@ -1,77 +1,268 @@ -using RobotApp.Interfaces; +using RobotApp.Common.Shares.Enums; +using RobotApp.Interfaces; namespace RobotApp.Services.Robot; -public class RobotPeripheral : BackgroundService, IPeripheral, ISafety +public partial class RobotPeripheral(RobotConfiguration RobotConfiguration, IError ErrorManager, Logger Logger) : 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 IsReady { get; private set; } = false; - public bool SetHorizontalLoad() + public event Action? OnSafetySpeedChanged; + public event Action? OnPeripheralModeChanged; + public event Action? OnButtonPressed; + public event Action? OnStop; + + private const int ReaderInterval = 100; + private WatchTimer? PeripheralReaderTimer; + + public void SetHorizontalLoad(bool value) { - throw new NotImplementedException(); + WritePLC(client => client.WriteSingleCoil(SetHorizontalLoadAddress, value)); } - public bool SetMutedBase(bool muted) + public void SetMutedBase(bool muted) { - throw new NotImplementedException(); + WritePLC(client => client.WriteSingleCoil(SetMutedBaseAddress, muted)); } - public bool SetMutedLoad(bool muted) + public void SetMutedLoad(bool muted) { - throw new NotImplementedException(); + WritePLC(client => client.WriteSingleCoil(SetMutedLoadAddress, muted)); } - public bool SetOnCharging() + public void SetOnCharging(bool value) { - throw new NotImplementedException(); + WritePLC(client => client.WriteSingleCoil(EnableChargingAddress, value)); } - public bool SetProccessState() + public void SetProccessState(ProccessingState state) { - throw new NotImplementedException(); + WritePLC(client => + { + switch (state) + { + case ProccessingState.Move: + client.WriteMultipleCoils(RobotExecuteStartAddress, RobotExecuteMoveState); + break; + case ProccessingState.Lifting: + client.WriteMultipleCoils(RobotExecuteStartAddress, RobotExecuteLiftingState); + break; + case ProccessingState.LiftRotating: + client.WriteMultipleCoils(RobotExecuteStartAddress, RobotExecuteLiftRotatingState); + break; + case ProccessingState.None: + client.WriteMultipleCoils(RobotExecuteStartAddress, RobotExecuteClearState); + break; + default: + break; + } + }); } - public bool SetSytemState() + public void SetSytemState(SystemState state) { - throw new NotImplementedException(); + WritePLC(client => + { + if (state != SystemState.ERROR) client.WriteSingleCoil(RobotStateErrorAddress, false); + else client.WriteMultipleCoils(RobotStateStartAddress, RobotClearState); + switch (state) + { + case SystemState.INIT: + client.WriteMultipleCoils(RobotStateStartAddress, RobotInitState); + break; + case SystemState.NOPOSE: + client.WriteMultipleCoils(RobotStateStartAddress, RobotNoPoseState); + break; + case SystemState.PAUSED: + client.WriteMultipleCoils(RobotStateStartAddress, RobotPauseState); + break; + case SystemState.IDLE: + client.WriteMultipleCoils(RobotStateStartAddress, RobotIdleState); + break; + case SystemState.PROCCESSING: + client.WriteMultipleCoils(RobotStateStartAddress, RobotProccessingState); + break; + case SystemState.CHARGING: + client.WriteMultipleCoils(RobotStateStartAddress, RobotCharingState); + break; + case SystemState.ERROR: + client.WriteSingleCoil(RobotStateErrorAddress, true); + break; + default: + break; + } + }); } - protected override Task ExecuteAsync(CancellationToken stoppingToken) + private void WritePLC(Action writeAction) { - throw new NotImplementedException(); + if (IsReady && !RobotConfiguration.IsSimulation) + { + try + { + if (ModbusTcpClient.TryConnect(RobotConfiguration.PLCAddress, RobotConfiguration.PLCPort, RobotConfiguration.PLCUnitId, out ModbusTcpClient? client) && client is not null) + { + writeAction(client); + client.Dispose(); + ErrorManager.DeleteErrorHint("2002"); + } + } + catch (Exception ex) + { + ErrorManager.AddError(RobotErrors.Error2002(), TimeSpan.FromSeconds(10)); + Logger.Error($"Có lỗi xảy ra trong quá trình gửi tín hiệu tới hệ thống ngoại vi(PLC): {ex.Message}"); + } + } } - public override Task StopAsync(CancellationToken cancellationToken) + private void ReadButtons(ModbusTcpClient client) { - return base.StopAsync(cancellationToken); + bool[] buttons = client.ReadCoils(ButtonStartAddress, 3); + if (buttons.Length == 3) + { + ButtonStart = buttons[0]; + ButtonStop = buttons[1]; + ButtonReset = buttons[2]; + if (ButtonStart) OnButtonPressed?.Invoke(PeripheralButton.Start); + if (ButtonStop) OnButtonPressed?.Invoke(PeripheralButton.Stop); + if (ButtonReset) OnButtonPressed?.Invoke(PeripheralButton.Reset); + } + } + + private void ReadSwitch(ModbusTcpClient client) + { + bool[] switchs = client.ReadCoils(SwicthStartAddress, 3); + if (switchs.Length == 3) + { + var oldMode = PeripheralMode; + if (switchs[0] && PeripheralMode != PeripheralMode.SERVICE) + { + PeripheralMode = PeripheralMode.SERVICE; + } + else if (switchs[1] && PeripheralMode != PeripheralMode.AUTO) + { + PeripheralMode = PeripheralMode.AUTO; + } + else if (switchs[2] && PeripheralMode != PeripheralMode.MANUAL) + { + PeripheralMode = PeripheralMode.MANUAL; + } + if (oldMode != PeripheralMode) OnPeripheralModeChanged?.Invoke(PeripheralMode); + } + } + + private void ReadSafetySpeed(ModbusTcpClient client) + { + bool[] speed = client.ReadCoils(SpeedStartAddress, 7); + if (speed.Length == 7) + { + var speedMap = new[] + { + (bit: speed[0], speed: SafetySpeed.Very_Slow), + (bit: speed[1], speed: SafetySpeed.Slow), + (bit: speed[2], speed: SafetySpeed.Normal), + (bit: speed[3], speed: SafetySpeed.Medium), + (bit: speed[4], speed: SafetySpeed.Optimal), + (bit: speed[5], speed: SafetySpeed.Fast), + (bit: speed[6], speed: SafetySpeed.Very_Fast) + }; + var activeSpeed = speedMap.FirstOrDefault(s => s.bit); + if (activeSpeed.speed != SafetySpeed) + { + SafetySpeed = activeSpeed.speed; + OnSafetySpeedChanged?.Invoke(SafetySpeed); + } + } + } + + private void ReadMotorReady(ModbusTcpClient client) + { + bool[] motors = client.ReadCoils(MotorReadyStartAddress, 3); + if (motors.Length == 3) + { + LeftMotorReady = motors[0]; + RightMotorReady = motors[1]; + LiftMotorReady = motors[2]; + } + } + + private void ReadSensors(ModbusTcpClient client) + { + bool[] sensors = client.ReadCoils(SafetyStartAddress, 5); + if (sensors.Length == 5) + { + Emergency = sensors[0]; + Bumper = sensors[1]; + LiftedUp = sensors[2]; + LiftedDown = sensors[3]; + LiftHome = sensors[4]; + } + } + + private void PeripheralReaderTimer_Elapsed() + { + if (IsReady && !RobotConfiguration.IsSimulation) + { + try + { + if (ModbusTcpClient.TryConnect(RobotConfiguration.PLCAddress, RobotConfiguration.PLCPort, RobotConfiguration.PLCUnitId, out ModbusTcpClient? client) && client is not null) + { + MutedBase = client.ReadCoils(MutedBaseAddress, 1)[0]; + MutedLoad = client.ReadCoils(MutedLoadAddress, 1)[0]; + EnabledCharging = client.ReadCoils(EnabledChargingAddress, 1)[0]; + HasLoad = client.ReadCoils(HasLoadAddress, 1)[0]; + LidarBackProtectField = client.ReadCoils(LidarBackProtectFieldAddress, 1)[0]; + LidarFrontProtectField = client.ReadCoils(LidarFrontProtectFieldAddress, 1)[0]; + LidarFrontTimProtectField = client.ReadCoils(LidarFrontTimProtectFieldAddress, 1)[0]; + ReadSensors(client); + ReadSwitch(client); + ReadSafetySpeed(client); + ReadMotorReady(client); + ReadButtons(client); + client.Dispose(); + } + ErrorManager.DeleteErrorHint("2001"); + + } + catch (Exception ex) + { + ErrorManager.AddError(RobotErrors.Error2001()); + Logger.Error($"Có lỗi xảy ra trong quá trình đọc tín hiệu từ hệ thống ngoại vi(PLC): {ex.Message}"); + } + } + } + + protected override async Task ExecuteAsync(CancellationToken stoppingToken) + { + await Task.Yield(); + while (!stoppingToken.IsCancellationRequested && !RobotConfiguration.IsSimulation) + { + try + { + if (ModbusTcpClient.TryConnect(RobotConfiguration.PLCAddress, RobotConfiguration.PLCPort, RobotConfiguration.PLCUnitId, out ModbusTcpClient? client) && client is not null) + { + client.Dispose(); + break; + } + await Task.Delay(2000, stoppingToken); + } + catch (TaskCanceledException) { break; } + catch (Exception ex) + { + Logger.Error($"Có lỗi xảy ra trong quá tình kết nối với hệ thống ngoại vi(PLC): {ex.Message}"); + await Task.Delay(5000, stoppingToken); + } + } + IsReady = true; + PeripheralReaderTimer = new(ReaderInterval, PeripheralReaderTimer_Elapsed, Logger); + PeripheralReaderTimer.Start(); + } + + public override async Task StopAsync(CancellationToken cancellationToken) + { + await Task.Yield(); + PeripheralReaderTimer?.Dispose(); + IsReady = false; + _ = base.StopAsync(cancellationToken); } } diff --git a/RobotApp/Services/Robot/RobotPeripheralAddress.cs b/RobotApp/Services/Robot/RobotPeripheralAddress.cs new file mode 100644 index 0000000..00e88f8 --- /dev/null +++ b/RobotApp/Services/Robot/RobotPeripheralAddress.cs @@ -0,0 +1,68 @@ +namespace RobotApp.Services.Robot; + +public partial class RobotPeripheral +{ + public static readonly ushort SafetyStartAddress = 0x0835; + public static readonly ushort EmergencyAddress = 0x0835; + public static readonly ushort BumperAddress = 0x0836; + public static readonly ushort LiftedUpAddress = 0x0837; + public static readonly ushort LiftedDownAddress = 0x0838; + public static readonly ushort LiftHomeAddress = 0x0839; + + public static readonly ushort LidarFrontProtectFieldAddress = 0x0830; + public static readonly ushort LidarBackProtectFieldAddress = 0x0831; + public static readonly ushort LidarFrontTimProtectFieldAddress = 0x083A; + + public static readonly ushort MotorReadyStartAddress = 0x0823; + public static readonly ushort LeftMotorReadyAddress = 0x0823; + public static readonly ushort RightMotorReadyAddress = 0x0824; + public static readonly ushort LiftMotorReadyAddress = 0x0825; + + public static readonly ushort SwicthStartAddress = 0x0814; + public static readonly ushort SwitchLockAddress = 0x0814; + public static readonly ushort SwitchAutoAddress = 0x0815; + public static readonly ushort SwitchManualAddress = 0x0816; + + public static readonly ushort ButtonStartAddress = 0x0840; + public static readonly ushort StartButtonAddress = 0x0840; + public static readonly ushort StopButtonAddress = 0x0841; + public static readonly ushort ResetButtonAddress = 0x0842; + + public static readonly ushort SetHorizontalLoadAddress = 0x09CC; + public static readonly ushort HasLoadAddress = 0x09D2; + public static readonly ushort EnabledChargingAddress = 0x0854; + public static readonly ushort EnableChargingAddress = 0x0853; + + public static readonly ushort SetMutedBaseAddress = 0x09CE; + public static readonly ushort MutedBaseAddress = 0x09CF; + + public static readonly ushort SetMutedLoadAddress = 0x09D0; + public static readonly ushort MutedLoadAddress = 0x09D1; + + public static readonly ushort SpeedStartAddress = 0x09C2; + public static readonly ushort SpeedVerySlowAddress = 0x09C2; + public static readonly ushort SpeedSlowAddress = 0x09C3; + public static readonly ushort SpeedNormalAddress = 0x09C4; + public static readonly ushort SpeedMediumAddress = 0x09C5; + public static readonly ushort SpeedOptimalAddress = 0x09C6; + public static readonly ushort SpeedFastAddress = 0x09C7; + public static readonly ushort SpeedVeryFastAddress = 0x09C8; + + public static readonly ushort RobotStateStartAddress = 0x092C; + public static readonly bool[] RobotClearState = [false, false, false, false, false, false, false]; + public static readonly bool[] RobotInitState = [true, false, false, false, false, false, false]; + public static readonly bool[] RobotNoPoseState = [false, true, false, false, false, false, false]; + public static readonly bool[] RobotPauseState = [false, false, true, false, false, false, false]; + public static readonly bool[] RobotIdleState = [false, false, false, true, false, false, false]; + public static readonly bool[] RobotProccessingState = [false, false, false, false, true, false, false]; + public static readonly bool[] RobotCharingState = [false, false, false, false, false, false, true]; + + public static readonly ushort RobotStateErrorAddress = 0x0936; + + public static readonly ushort RobotExecuteStartAddress = 0x0933; + public static readonly bool[] RobotExecuteClearState = [false, false, false]; + public static readonly bool[] RobotExecuteMoveState = [true, false, false]; + public static readonly bool[] RobotExecuteLiftingState = [false, true, false]; + public static readonly bool[] RobotExecuteLiftRotatingState = [false, false, true]; + +} diff --git a/RobotApp/Services/Robot/RobotPeripheralData.cs b/RobotApp/Services/Robot/RobotPeripheralData.cs new file mode 100644 index 0000000..f84eb4c --- /dev/null +++ b/RobotApp/Services/Robot/RobotPeripheralData.cs @@ -0,0 +1,32 @@ +using RobotApp.Interfaces; + +namespace RobotApp.Services.Robot; + +public partial class RobotPeripheral +{ + public PeripheralMode PeripheralMode { get; private set; } = PeripheralMode.SERVICE; + public bool Emergency { get; private set; } + public bool Bumper { get; private set; } + public bool LiftedUp { get; private set; } + public bool LiftedDown { get; private set; } + public bool LiftHome { get; private set; } + + public bool LidarFrontProtectField { get; private set; } + public bool LidarBackProtectField { get; private set; } + public bool LidarFrontTimProtectField { get; private set; } + + public bool LeftMotorReady { get; private set; } + public bool RightMotorReady { get; private set; } + public bool LiftMotorReady { get; private set; } + + public bool ButtonStart { get; private set; } + public bool ButtonStop { get; private set; } + public bool ButtonReset { get; private set; } + + public bool HasLoad { get; private set; } + public bool MutedBase { get; private set; } + public bool MutedLoad { get; private set; } + public bool EnabledCharging { get; private set; } + + public SafetySpeed SafetySpeed { get; private set; } +} diff --git a/RobotApp/Services/Robot/RobotVisualization.cs b/RobotApp/Services/Robot/RobotVisualization.cs index 4edefff..e741c0a 100644 --- a/RobotApp/Services/Robot/RobotVisualization.cs +++ b/RobotApp/Services/Robot/RobotVisualization.cs @@ -1,14 +1,17 @@ -using RobotApp.Interfaces; +using RobotApp.Common.Shares; +using RobotApp.Interfaces; +using RobotApp.VDA5050; using RobotApp.VDA5050.Visualization; namespace RobotApp.Services.Robot; -public class RobotVisualization(ILocalization Localization) +public class RobotVisualization(ILocalization Localization, INavigation Navigation, RobotConfiguration RobotConfiguration, RobotConnection RobotConnection, Logger Logger) : BackgroundService { - public VisualizationMsg Visualization => GetVisualizationMsg(); - public string SerialNumber { get; set; } = string.Empty; - + public string SerialNumber { get; set; } = RobotConfiguration.SerialNumber; private uint HeaderId; + + private WatchTimerAsync? UpdateTimer; + private const int UpdateInterval = 50; private VisualizationMsg GetVisualizationMsg() { return new VisualizationMsg() @@ -25,10 +28,39 @@ public class RobotVisualization(ILocalization Localization) }, Velocity = new Velocity() { - Vx = 0, - Vy = 0, - Omega = 0 + Vx = Navigation.VelocityX, + Vy = Navigation.VelocityY, + Omega = Navigation.Omega } }; } + + private async Task UpdateHandler() + { + try + { + await RobotConnection.Publish(VDA5050Topic.VISUALIZATION.ToTopicString(), System.Text.Json.JsonSerializer.Serialize(GetVisualizationMsg(), JsonOptionExtends.Write)); + } + catch { } + } + + protected override async Task ExecuteAsync(CancellationToken stoppingToken) + { + while(!stoppingToken.IsCancellationRequested) + { + if (RobotConnection.IsConnected) break; + await Task.Delay(1000); + } + if(!stoppingToken.IsCancellationRequested) + { + UpdateTimer = new(UpdateInterval, UpdateHandler, Logger); + UpdateTimer.Start(); + } + } + + public override Task StopAsync(CancellationToken cancellationToken) + { + UpdateTimer?.Dispose(); + return base.StopAsync(cancellationToken); + } } diff --git a/RobotApp/Services/Robot/Simulation/Navigation/DifferentialNavigation.cs b/RobotApp/Services/Robot/Simulation/Navigation/DifferentialNavigation.cs index 513e8c1..da3a4d4 100644 --- a/RobotApp/Services/Robot/Simulation/Navigation/DifferentialNavigation.cs +++ b/RobotApp/Services/Robot/Simulation/Navigation/DifferentialNavigation.cs @@ -47,20 +47,20 @@ public class DifferentialNavigation : SimulationNavigation var deviation = CurrentBaseNode.NodeId == SimulationOrderNodes[^1].NodeId ? 0.02 : 0.1; if (DistanceToCheckingNode > deviation) { - double SpeedTarget = MovePID.PID_step(DistanceToCheckingNode, SimulationModel.MaxVelocity, 0, CycleHandlerMilliseconds / 1000.0); + double SpeedTarget = MovePID.PID_step(DistanceToCheckingNode, RobotConfiguration.SimulationModel.MaxVelocity, 0, CycleHandlerMilliseconds / 1000.0); double AngularVel = MovePurePursuit.PurePursuit_step(Visualization.X, Visualization.Y, Visualization.Theta * Math.PI / 180); AngularVel *= NavigationPath[MovePurePursuit.OnNodeIndex].Direction == RobotDirection.FORWARD ? 1 : -1; (double AngularVelocityLeft, double AngularVelocityRight) = MoveFuzzy.Fuzzy_step(SpeedTarget, AngularVel, CycleHandlerMilliseconds / 1000.0); if (NavigationPath[MovePurePursuit.OnNodeIndex].Direction == RobotDirection.FORWARD) { - AngularVelocityLeft /= SimulationModel.RadiusWheel; - AngularVelocityRight = AngularVelocityRight / SimulationModel.RadiusWheel * -1; + AngularVelocityLeft /= RobotConfiguration.SimulationModel.RadiusWheel; + AngularVelocityRight = AngularVelocityRight / RobotConfiguration.SimulationModel.RadiusWheel * -1; } else { - AngularVelocityLeft = AngularVelocityLeft / SimulationModel.RadiusWheel * -1; - AngularVelocityRight /= SimulationModel.RadiusWheel; + AngularVelocityLeft = AngularVelocityLeft / RobotConfiguration.SimulationModel.RadiusWheel * -1; + AngularVelocityRight /= RobotConfiguration.SimulationModel.RadiusWheel; } VelocityController.SetSpeed(AngularVelocityLeft, AngularVelocityRight, CycleHandlerMilliseconds / 1000.0); } diff --git a/RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigation.cs b/RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigation.cs index 1910bcd..cf5f116 100644 --- a/RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigation.cs +++ b/RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigation.cs @@ -1,12 +1,9 @@ -using Microsoft.AspNetCore.Components; -using RobotApp.Common.Shares; +using RobotApp.Common.Shares; using RobotApp.Common.Shares.Enums; using RobotApp.Interfaces; using RobotApp.Services.Exceptions; using RobotApp.Services.Robot.Simulation.Navigation.Algorithm; using RobotApp.VDA5050.Order; -using RobotApp.VDA5050.State; -using System.IO; namespace RobotApp.Services.Robot.Simulation.Navigation; @@ -39,7 +36,7 @@ public class SimulationNavigation : INavigation, IDisposable protected readonly SimulationVisualization Visualization; protected readonly SimulationVelocity VelocityController; - protected readonly SimulationModel SimulationModel; + protected readonly RobotConfiguration RobotConfiguration; protected readonly RobotPathPlanner PathPlanner; protected NavigationNode[] NavigationPath = []; protected NavigationNode? CurrentBaseNode; @@ -53,9 +50,9 @@ public class SimulationNavigation : INavigation, IDisposable using var scope = ServiceProvider.CreateScope(); Logger = scope.ServiceProvider.GetRequiredService>(); Visualization = scope.ServiceProvider.GetRequiredService(); - SimulationModel = scope.ServiceProvider.GetRequiredService(); + RobotConfiguration = scope.ServiceProvider.GetRequiredService(); PathPlanner = scope.ServiceProvider.GetRequiredService(); - VelocityController = new(Visualization, SimulationModel); + VelocityController = new(Visualization, RobotConfiguration.SimulationModel); } protected void HandleNavigationStart() diff --git a/RobotApp/Services/Robot/Simulation/SimulationExtensions.cs b/RobotApp/Services/Robot/Simulation/SimulationExtensions.cs index 1676768..a1b1bda 100644 --- a/RobotApp/Services/Robot/Simulation/SimulationExtensions.cs +++ b/RobotApp/Services/Robot/Simulation/SimulationExtensions.cs @@ -7,7 +7,6 @@ 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 index b274074..e26f400 100644 --- a/RobotApp/Services/Robot/Simulation/SimulationModel.cs +++ b/RobotApp/Services/Robot/Simulation/SimulationModel.cs @@ -2,15 +2,14 @@ namespace RobotApp.Services.Robot.Simulation; -public class SimulationModel(IConfiguration Configuration) +public class SimulationModel { - public readonly bool Enable = Configuration.GetValue("Simulation:Enable", false); - public readonly double RadiusWheel = 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); + public readonly double RadiusWheel = 0.1; + public readonly double Width = 0.606; + public readonly double Length = 1.106; + public readonly double MaxVelocity = 1.5; + public readonly double MaxAngularVelocity = 0.3; + public readonly double Acceleration = 2; + public readonly double Deceleration = 10; + public readonly NavigationType NavigationType = NavigationType.Differential; } diff --git a/RobotApp/Services/Robot/Simulation/SimulationVisualization.cs b/RobotApp/Services/Robot/Simulation/SimulationVisualization.cs index 8fccf47..ff4cbc5 100644 --- a/RobotApp/Services/Robot/Simulation/SimulationVisualization.cs +++ b/RobotApp/Services/Robot/Simulation/SimulationVisualization.cs @@ -1,6 +1,6 @@ namespace RobotApp.Services.Robot.Simulation; -public class SimulationVisualization(SimulationModel Model) +public class SimulationVisualization(RobotConfiguration RobotConfiguration) { public double X { get; private set; } public double Y { get; private set; } @@ -9,8 +9,8 @@ public class SimulationVisualization(SimulationModel Model) public double Vy { get; private set; } public double Omega { get; private set; } - private readonly double RadiusWheel = Model.RadiusWheel; - private readonly double RadiusRobot = Model.Width / 2; + private readonly double RadiusWheel = RobotConfiguration.SimulationModel.RadiusWheel; + private readonly double RadiusRobot = RobotConfiguration.SimulationModel.Width / 2; public (double x, double y, double angle) UpdatePosition(double wL, double wR, double time) { diff --git a/RobotApp/Services/RobotExtensions.cs b/RobotApp/Services/RobotExtensions.cs index 6f5d31e..c9d435f 100644 --- a/RobotApp/Services/RobotExtensions.cs +++ b/RobotApp/Services/RobotExtensions.cs @@ -10,8 +10,22 @@ public static class RobotExtensions public static IServiceCollection AddRobot(this IServiceCollection services) { services.AddSingleton(); + services.AddSingleton(); + services.AddSingleton(); + services.AddSingleton(); + services.AddInterfaceServiceSingleton(); + services.AddInterfaceServiceSingleton(); + services.AddInterfaceServiceSingleton(); + services.AddInterfaceServiceSingleton(); + services.AddInterfaceServiceSingleton(); services.AddInterfaceServiceSingleton(); + services.AddInterfaceServiceSingleton(); + services.AddHostedInterfaceServiceSingleton(); + services.AddInterfaceServiceSingleton(); + + services.AddHostedServiceSingleton(); + services.AddHostedServiceSingleton(); return services; } @@ -36,4 +50,13 @@ public static class RobotExtensions services.AddHostedService(sp => sp.GetRequiredService()); return services; } + + public static IServiceCollection AddHostedInterfaceServiceSingleton(this IServiceCollection services) where TService1 : class where TService2 : class where THostedService : class, IHostedService, TService1, TService2 + { + services.AddSingleton(); + services.AddSingleton(sp => sp.GetRequiredService()); + services.AddSingleton(sp => sp.GetRequiredService()); + services.AddHostedService(sp => sp.GetRequiredService()); + return services; + } } diff --git a/RobotApp/Services/RobotStateMachineService.cs b/RobotApp/Services/RobotStateMachineService.cs deleted file mode 100644 index e69de29..0000000 diff --git a/RobotApp/Services/State/AutoState.cs b/RobotApp/Services/State/AutoState.cs index 575dff8..8a9518e 100644 --- a/RobotApp/Services/State/AutoState.cs +++ b/RobotApp/Services/State/AutoState.cs @@ -4,4 +4,32 @@ namespace RobotApp.Services.State; public class AutoState(AutoStateType state, IRobotState? superState = null) : RobotState(state, superState) where T : Enum { + public override void Enter() + { + base.Enter(); + switch (Name) + { + case AutoStateType.Idle: + case AutoStateType.Holding: + case AutoStateType.Paused: + case AutoStateType.Canceling: + case AutoStateType.Recovering: + case AutoStateType.Remote_Override: + case AutoStateType.Executing: + if (HistoryState is not null && SubStates.Contains(HistoryState)) ActiveSubState = HistoryState; + else ActiveSubState = SubStates.FirstOrDefault(s => s.Name.Equals(ExecutingStateType.Moving)); + ActiveSubState?.Enter(); + break; + } + } + public override void Exit() + { + if (Name.Equals(AutoStateType.Executing) && ActiveSubState != null) + { + HistoryState = ActiveSubState; + } + ActiveSubState?.Exit(); + ActiveSubState = null; + base.Exit(); + } } diff --git a/RobotApp/Services/State/FaultState.cs b/RobotApp/Services/State/FaultState.cs index c473b6b..9ef3d75 100644 --- a/RobotApp/Services/State/FaultState.cs +++ b/RobotApp/Services/State/FaultState.cs @@ -4,4 +4,12 @@ namespace RobotApp.Services.State; public class FaultState(FaultStateType state, IRobotState? superState = null) : RobotState(state, superState) where T : Enum { + public override bool CanTransitionTo(IRobotState targetState) + { + if (targetState == null) return false; + return targetState.Name.Equals(RootStateType.System) || + targetState.Name.Equals(RootStateType.Manual) || + targetState.Name.Equals(RootStateType.Service) || + targetState.Name.Equals(RootStateType.Stop); + } } diff --git a/RobotApp/Services/State/ManualState.cs b/RobotApp/Services/State/ManualState.cs index e79c596..4b2a6c9 100644 --- a/RobotApp/Services/State/ManualState.cs +++ b/RobotApp/Services/State/ManualState.cs @@ -3,5 +3,15 @@ namespace RobotApp.Services.State; public class ManualState(ManualStateType state, IRobotState? superState = null) : RobotState(state, superState) where T : Enum -{ +{ + public override bool CanTransitionTo(IRobotState targetState) + { + if (targetState == null) return false; + + return targetState.Name.Equals(RootStateType.Auto) || + targetState.Name.Equals(RootStateType.Service) || + targetState.Name.Equals(RootStateType.System) || + targetState.Name.Equals(RootStateType.Stop) || + targetState.Name.Equals(RootStateType.Fault); + } } diff --git a/RobotApp/Services/State/RobotStateMachine.cs b/RobotApp/Services/State/RobotStateMachine.cs index 46809e6..fdbe1bf 100644 --- a/RobotApp/Services/State/RobotStateMachine.cs +++ b/RobotApp/Services/State/RobotStateMachine.cs @@ -1,4 +1,5 @@ using RobotApp.Common.Shares.Enums; +using RobotApp.Interfaces; namespace RobotApp.Services.State; @@ -6,45 +7,13 @@ public record RobotStateMachine(Logger Logger) : IDisposable { private readonly Dictionary> StateRegistry = []; - public IRobotState? CurrentState { get; private set; } + public IRobotState CurrentState { get; private set; } = null!; public event Action? OnStateChanged; public bool IsInitialized = false; + public string CurrentStateName => CurrentState.Name.ToString(); private readonly Lock StateLock = new(); - private void PrintStateAdded() - { - Console.WriteLine("=== All Registered States ==="); - var allStates = StateRegistry.Values - .SelectMany(dict => dict.Values) - .Where(state => state.SuperState == null) - .OrderBy(state => state.Type.Name) - .ThenBy(state => state.Name.ToString()); - - foreach (var rootState in allStates) - { - PrintStateSimple(rootState, 0); - } - - var totalStates = StateRegistry.Values.Sum(dict => dict.Count); - Console.WriteLine($"\nTotal: {totalStates} states registered"); - Console.WriteLine("==============================\n"); - } - - private void PrintStateSimple(IRobotState state, int depth) - { - var indent = new string(' ', depth * 4); - var marker = state == CurrentState ? " [CURRENT]" : ""; - var activeMarker = state.SuperState?.ActiveSubState == state ? " [ACTIVE]" : ""; - - Console.WriteLine($"{indent}- {state.Name}{marker}{activeMarker}"); - - foreach (var subState in state.SubStates.OrderBy(s => s.Name.ToString())) - { - PrintStateSimple(subState, depth + 1); - } - } - public void InitializeHierarchyStates() { if (IsInitialized) return; @@ -62,17 +31,28 @@ public record RobotStateMachine(Logger Logger) : IDisposable SetupFaultState(); var systemState = GetState(RootStateType.System); - lock (StateLock) - { - systemState.Enter(); - CurrentState = systemState; - } + systemState.Enter(); - PrintStateAdded(); IsInitialized = true; Logger.Info($"Khởi tạo thành công State Machine với State hiện tại: {GetCurrentStatePath()}"); } + private void OnStateEnter(IRobotState state) + { + lock (StateLock) + { + CurrentState = state; + } + } + + private void OnStateExit(IRobotState state) + { + lock (StateLock) + { + + } + } + private void SetupSystemState() { var systemlState = GetState(RootStateType.System) ?? throw new InvalidOperationException($"Failed to get state RootStateType.Auto"); @@ -142,12 +122,14 @@ public record RobotStateMachine(Logger Logger) : IDisposable var emcStopState = new StopState(StopStateType.EMC, stopState); var protectiveState = new StopState(StopStateType.Protective, stopState); var manualStopState = new StopState(StopStateType.Manual, stopState); + var bumberStopState = new StopState(StopStateType.Bumber, stopState); - stopState.SubStates.AddRange([emcStopState, protectiveState, manualStopState]); + stopState.SubStates.AddRange([emcStopState, protectiveState, manualStopState, bumberStopState]); RegisterState(emcStopState); RegisterState(protectiveState); RegisterState(manualStopState); + RegisterState(bumberStopState); } private void SetupFaultState() @@ -251,6 +233,8 @@ public record RobotStateMachine(Logger Logger) : IDisposable { var enumType = typeof(T); StateRegistry.TryAdd(enumType, []); + state.OnEnter += OnStateEnter; + state.OnExit += OnStateExit; StateRegistry[enumType][state.Name] = state; } @@ -314,6 +298,38 @@ public record RobotStateMachine(Logger Logger) : IDisposable } } + public bool TransitionToStop(StopStateType stopType) + { + var stopState = GetState(RootStateType.Stop); + var subStopState = stopState.SubStates.FirstOrDefault(s => s.Name.Equals(stopType)); + if(subStopState is not null) + { + var transitionToStop = TransitionTo(RootStateType.Stop); + if (transitionToStop) + { + subStopState.Enter(); + return true; + } + } + return false; + } + + public bool TransitionToFault(FaultStateType faultType) + { + var faultState = GetState(RootStateType.Fault); + var subFaultState = faultState.SubStates.FirstOrDefault(s => s.Name.Equals(faultType)); + if (subFaultState is not null) + { + var transitionToStop = TransitionTo(RootStateType.Fault); + if (transitionToStop) + { + subFaultState.Enter(); + return true; + } + } + return false; + } + public void Update() { if (!IsInitialized) diff --git a/RobotApp/Services/State/RootState.cs b/RobotApp/Services/State/RootState.cs index 46dff3d..13e3386 100644 --- a/RobotApp/Services/State/RootState.cs +++ b/RobotApp/Services/State/RootState.cs @@ -6,31 +6,107 @@ public class RootState(RootStateType state, IRobotState? superState = null) : { public override void Enter() { - switch(Name) + base.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; + if (HistoryState is not null && SubStates.Contains(HistoryState)) ActiveSubState = HistoryState; else ActiveSubState = SubStates.FirstOrDefault(s => s.Name.Equals(AutoStateType.Idle)); ActiveSubState?.Enter(); break; + case RootStateType.Manual: + ActiveSubState = SubStates.FirstOrDefault(s => s.Name.Equals(ManualStateType.Idle)); + ActiveSubState?.Enter(); + break; + case RootStateType.Service: + ActiveSubState = SubStates.FirstOrDefault(s => s.Name.Equals(ServiceStateType.Idle)); + ActiveSubState?.Enter(); + break; + case RootStateType.Stop: + break; + + case RootStateType.Fault: + break; } } public override void Exit() { + if (Name.Equals(RootStateType.Auto) && ActiveSubState != null) + { + HistoryState = ActiveSubState; + } + + ActiveSubState?.Exit(); + ActiveSubState = null; + + base.Exit(); } public override bool CanTransitionTo(IRobotState targetState) { - return true; + if (targetState == null) return false; + if (targetState.Name.Equals(RootStateType.Stop) || targetState.Name.Equals(RootStateType.Fault)) return true; + + return Name switch + { + RootStateType.System => CanTransitionFromSystem(targetState), + RootStateType.Auto => CanTransitionFromAuto(targetState), + RootStateType.Manual => CanTransitionFromManual(targetState), + RootStateType.Service => CanTransitionFromService(targetState), + RootStateType.Stop => CanTransitionFromStop(targetState), + RootStateType.Fault => CanTransitionFromFault(targetState), + _ => false + }; } public override void Update() { - ActiveSubState?.Update(); + base.Update(); + } + + private static bool CanTransitionFromSystem(IRobotState targetState) + { + return targetState.Name.Equals(RootStateType.Auto) || + targetState.Name.Equals(RootStateType.Manual) || + targetState.Name.Equals(RootStateType.Service); + } + + private static bool CanTransitionFromAuto(IRobotState targetState) + { + return targetState.Name.Equals(RootStateType.Manual) || + targetState.Name.Equals(RootStateType.Service) || + targetState.Name.Equals(RootStateType.System); + } + + private static bool CanTransitionFromManual(IRobotState targetState) + { + return targetState.Name.Equals(RootStateType.Auto) || + targetState.Name.Equals(RootStateType.Service) || + targetState.Name.Equals(RootStateType.System); + } + + private static bool CanTransitionFromService(IRobotState targetState) + { + return targetState.Name.Equals(RootStateType.Auto) || + targetState.Name.Equals(RootStateType.Manual) || + targetState.Name.Equals(RootStateType.System); + } + + private static bool CanTransitionFromStop(IRobotState targetState) + { + return targetState.Name.Equals(RootStateType.System) || + targetState.Name.Equals(RootStateType.Manual); + } + + private static bool CanTransitionFromFault(IRobotState targetState) + { + return targetState.Name.Equals(RootStateType.System) || + targetState.Name.Equals(RootStateType.Manual) || + targetState.Name.Equals(RootStateType.Service); } } diff --git a/RobotApp/Services/State/ServiceState.cs b/RobotApp/Services/State/ServiceState.cs index cbf8f48..281e443 100644 --- a/RobotApp/Services/State/ServiceState.cs +++ b/RobotApp/Services/State/ServiceState.cs @@ -4,4 +4,14 @@ namespace RobotApp.Services.State; public class ServiceState(ServiceStateType state, IRobotState? superState = null) : RobotState(state, superState) where T : Enum { + public override bool CanTransitionTo(IRobotState targetState) + { + if (targetState == null) return false; + + return targetState.Name.Equals(RootStateType.Auto) || + targetState.Name.Equals(RootStateType.Service) || + targetState.Name.Equals(RootStateType.System) || + targetState.Name.Equals(RootStateType.Stop) || + targetState.Name.Equals(RootStateType.Fault); + } } diff --git a/RobotApp/Services/State/StopState.cs b/RobotApp/Services/State/StopState.cs index 3e89182..c9327d6 100644 --- a/RobotApp/Services/State/StopState.cs +++ b/RobotApp/Services/State/StopState.cs @@ -1,8 +1,15 @@ using RobotApp.Common.Shares.Enums; -namespace RobotApp.Services.State +namespace RobotApp.Services.State; + +public class StopState(StopStateType state, IRobotState? superState = null) : RobotState(state, superState) where T : Enum { - public class StopState(StopStateType state, IRobotState? superState = null) : RobotState(state, superState) where T : Enum + public override bool CanTransitionTo(IRobotState targetState) { + if (targetState == null) return false; + return targetState.Name.Equals(RootStateType.System) || + targetState.Name.Equals(RootStateType.Manual) || + targetState.Name.Equals(RootStateType.Service) || + targetState.Name.Equals(RootStateType.Fault); } } diff --git a/RobotApp/Services/State/SystemState.cs b/RobotApp/Services/State/SystemState.cs index f6d8f9d..e8d8753 100644 --- a/RobotApp/Services/State/SystemState.cs +++ b/RobotApp/Services/State/SystemState.cs @@ -4,18 +4,32 @@ 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(); + if (targetState == null) return false; + + switch (Name) + { + case SystemStateType.Initializing: + return targetState.Name.Equals(SystemStateType.Standby) || + targetState.Name.Equals(RootStateType.Fault) || + targetState.Name.Equals(RootStateType.Stop); + + case SystemStateType.Standby: + return targetState.Name.Equals(RootStateType.Auto) || + targetState.Name.Equals(RootStateType.Manual) || + targetState.Name.Equals(RootStateType.Service) || + targetState.Name.Equals(SystemStateType.Shutting_Down) || + targetState.Name.Equals(RootStateType.Stop) || + targetState.Name.Equals(RootStateType.Fault); + + case SystemStateType.Shutting_Down: + return targetState.Name.Equals(RootStateType.Stop) || + targetState.Name.Equals(SystemStateType.Initializing); + default: + break; + } + + return false; } }