DangNV/RobotApp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
Đăng Nguyễn 2025-10-16 14:53:22 +07:00
parent b2df5b22b7
commit 90dcb67b60
38 changed files with 934 additions and 224 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
RobotApp.Common.Shares/Enums/StateType.cs
View File

@ -43,6 +43,7 @@ public enum ServiceStateType
public enum StopStateType
{
EMC,
Bumber,
Protective,
Manual,
}

1
RobotApp.VDA5050/State/Error.cs
View File

@ -20,6 +20,7 @@ public class ErrorReferences
public enum ErrorType
{
INITIALIZE_ORDER,
READ_PERIPHERAL_FAILURE,
}
public class Error

8
RobotApp/Interfaces/IDriver.cs
View File

@ -11,14 +11,14 @@ public interface IDriver
bool IsReady { get; }
/// <summary>
/// Tốc độ động cơ bên trái
/// Tốc độ di chuyển thẳng của robot (m/s)
/// </summary>
double LeftVelocity { get; }
double LinearVelocity { get; }
/// <summary>
/// Tốc độ động cơ bên phải
/// Tốc độ quay của robot (rad/s)
/// </summary>
double RightVelocity { get; }
double AngularVelocity { get; }
/// <summary>
/// Điều khiển tốc độ động cơ trái và phải

3
RobotApp/Interfaces/IError.cs
View File

@ -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();
}

58
RobotApp/Interfaces/IPeripheral.cs
View File

@ -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<PeripheralMode>? OnPeripheralModeChanged;
event Action<PeripheralButton>? OnButtonPressed;
event Action<StopStateType>? 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);
}

29
RobotApp/Interfaces/ISafety.cs
View File

@ -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<SafetySpeed>? OnSafetySpeedChanged;
SafetySpeed SafetySpeed { get; }
void SetMutedLoad(bool muted);
void SetMutedBase(bool muted);
void SetHorizontalLoad(bool value);
}

17
RobotApp/Interfaces/ISensorIMU.cs
View File

@ -1,17 +0,0 @@
namespace RobotApp.Interfaces;
/// <summary>
/// Interface cảm biến IMU
/// </summary>
public interface ISensorIMU
{
/// <summary>
/// Trạng thái sẵn sàng của cảm biến IMU
/// </summary>
bool IsReady { get; }
/// <summary>
/// Góc xoay của robot (đơn vị độ)
/// </summary>
double Angle { get; }
}

1
RobotApp/Program.cs
View File

@ -47,7 +47,6 @@ builder.Services.AddIdentity<ApplicationUser, ApplicationRole>(options =>
builder.Services.AddSingleton<IEmailSender<ApplicationUser>, IdentityNoOpEmailSender>();
builder.Services.AddSingleton(typeof(RobotApp.Services.Logger<>));
builder.Services.AddSingleton<RobotConnection>();
builder.Services.AddRobotSimulation();
builder.Services.AddRobot();

4
RobotApp/RobotApp.csproj
View File

@ -29,4 +29,8 @@
<PackageReference Include="MQTTnet" Version="5.0.1.1416" />
</ItemGroup>
<ItemGroup>
<Folder Include="Tests\" />
</ItemGroup>
</Project>

2
RobotApp/Services/ModbusTcpClient.cs
View File

@ -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;

19
RobotApp/Services/Robot/RobotConfiguration.cs
View File

@ -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();
}

4
RobotApp/Services/Robot/RobotConnection.cs
View File

@ -6,13 +6,13 @@ using System.Text.Json;
namespace RobotApp.Services.Robot;
public class RobotConnection(Logger<RobotConnection> Logger, Logger<MQTTClient> MQTTClientLogger)
public class RobotConnection(RobotConfiguration RobotConfiguration, Logger<RobotConnection> Logger, Logger<MQTTClient> 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)
{

21
RobotApp/Services/Robot/RobotController.cs
View File

@ -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<RobotController>? 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());

110
RobotApp/Services/Robot/RobotControllerInitialize.cs Normal file
View File

@ -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);
}
}

22
RobotApp/Services/Robot/RobotDriver.cs Normal file
View File

@ -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();
}
}

40
RobotApp/Services/Robot/RobotErrors.cs
View File

@ -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<Error> 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)");
}

6
RobotApp/Services/Robot/RobotInfomations.cs
View File

@ -1,6 +1,8 @@
namespace RobotApp.Services.Robot
using RobotApp.Interfaces;
namespace RobotApp.Services.Robot
{
public class RobotInfomations
public class RobotInfomations : IInfomation
{
}
}

4
RobotApp/Services/Robot/RobotLocalization.cs
View File

@ -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<bool>("Robot:Simulation", false);
private readonly bool IsSimulation = RobotConfiguration.IsSimulation;
public MessageResult ActivateMap(string mapName)
{

16
RobotApp/Services/Robot/RobotNavigation.cs
View File

@ -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()
{

287
RobotApp/Services/Robot/RobotPeripheral.cs
View File

@ -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<RobotPeripheral> 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<SafetySpeed>? OnSafetySpeedChanged;
public event Action<PeripheralMode>? OnPeripheralModeChanged;
public event Action<PeripheralButton>? OnButtonPressed;
public event Action<StopStateType>? OnStop;
private const int ReaderInterval = 100;
private WatchTimer<RobotPeripheral>? 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<ModbusTcpClient> 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);
}
}

68
RobotApp/Services/Robot/RobotPeripheralAddress.cs Normal file
View File

@ -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];
}

32
RobotApp/Services/Robot/RobotPeripheralData.cs Normal file
View File

@ -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; }
}

48
RobotApp/Services/Robot/RobotVisualization.cs
View File

@ -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<RobotVisualization> 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<RobotVisualization>? 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);
}
}

10
RobotApp/Services/Robot/Simulation/Navigation/DifferentialNavigation.cs
View File

@ -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);
}

11
RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigation.cs
View File

@ -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<Logger<SimulationNavigation>>();
Visualization = scope.ServiceProvider.GetRequiredService<SimulationVisualization>();
SimulationModel = scope.ServiceProvider.GetRequiredService<SimulationModel>();
RobotConfiguration = scope.ServiceProvider.GetRequiredService<RobotConfiguration>();
PathPlanner = scope.ServiceProvider.GetRequiredService<RobotPathPlanner>();
VelocityController = new(Visualization, SimulationModel);
VelocityController = new(Visualization, RobotConfiguration.SimulationModel);
}
protected void HandleNavigationStart()

1
RobotApp/Services/Robot/Simulation/SimulationExtensions.cs
View File

@ -7,7 +7,6 @@ public static class SimulationExtensions
public static IServiceCollection AddRobotSimulation(this IServiceCollection services)
{
services.AddSingleton<SimulationVisualization>();
services.AddSingleton<SimulationModel>();
return services;
}
}

19
RobotApp/Services/Robot/Simulation/SimulationModel.cs
View File

@ -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;
}

6
RobotApp/Services/Robot/Simulation/SimulationVisualization.cs
View File

@ -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)
{

23
RobotApp/Services/RobotExtensions.cs
View File

@ -10,8 +10,22 @@ public static class RobotExtensions
public static IServiceCollection AddRobot(this IServiceCollection services)
{
services.AddSingleton<RobotStateMachine>();
services.AddSingleton<RobotConfiguration>();
services.AddSingleton<RobotPathPlanner>();
services.AddSingleton<RobotConnection>();
services.AddInterfaceServiceSingleton<IBattery, RobotBattery>();
services.AddInterfaceServiceSingleton<IDriver, RobotDriver>();
services.AddInterfaceServiceSingleton<IError, RobotErrors>();
services.AddInterfaceServiceSingleton<IInfomation, RobotInfomations>();
services.AddInterfaceServiceSingleton<IInstantActions, RobotAction>();
services.AddInterfaceServiceSingleton<ILocalization, RobotLocalization>();
services.AddInterfaceServiceSingleton<INavigation, RobotNavigation>();
services.AddHostedInterfaceServiceSingleton<IPeripheral, ISafety, RobotPeripheral>();
services.AddInterfaceServiceSingleton<IOrder, RobotOrderController>();
services.AddHostedServiceSingleton<RobotController>();
services.AddHostedServiceSingleton<RobotVisualization>();
return services;
}
@ -36,4 +50,13 @@ public static class RobotExtensions
services.AddHostedService(sp => sp.GetRequiredService<THostedService>());
return services;
}
public static IServiceCollection AddHostedInterfaceServiceSingleton<TService1, TService2, [DynamicallyAccessedMembers(DynamicallyAccessedMemberTypes.PublicConstructors)] THostedService>(this IServiceCollection services) where TService1 : class where TService2 : class where THostedService : class, IHostedService, TService1, TService2
{
services.AddSingleton<THostedService>();
services.AddSingleton<TService1>(sp => sp.GetRequiredService<THostedService>());
services.AddSingleton<TService2>(sp => sp.GetRequiredService<THostedService>());
services.AddHostedService(sp => sp.GetRequiredService<THostedService>());
return services;
}
}

0
RobotApp/Services/RobotStateMachineService.cs
View File

28
RobotApp/Services/State/AutoState.cs
View File

@ -4,4 +4,32 @@ namespace RobotApp.Services.State;
public class AutoState<T>(AutoStateType state, IRobotState? superState = null) : RobotState<T>(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();
}
}

8
RobotApp/Services/State/FaultState.cs
View File

@ -4,4 +4,12 @@ namespace RobotApp.Services.State;
public class FaultState<T>(FaultStateType state, IRobotState? superState = null) : RobotState<T>(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);
}
}

10
RobotApp/Services/State/ManualState.cs
View File

@ -4,4 +4,14 @@ namespace RobotApp.Services.State;
public class ManualState<T>(ManualStateType state, IRobotState? superState = null) : RobotState<T>(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);
}
}

96
RobotApp/Services/State/RobotStateMachine.cs
View File

@ -1,4 +1,5 @@
using RobotApp.Common.Shares.Enums;
using RobotApp.Interfaces;
namespace RobotApp.Services.State;
@ -6,45 +7,13 @@ public record RobotStateMachine(Logger<RobotStateMachine> Logger) : IDisposable
{
private readonly Dictionary<Type, Dictionary<Enum, IRobotState>> StateRegistry = [];
public IRobotState? CurrentState { get; private set; }
public IRobotState CurrentState { get; private set; } = null!;
public event Action<IRobotState?, IRobotState>? 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<RobotStateMachine> Logger) : IDisposable
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 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<RobotStateMachine> Logger) : IDisposable
var emcStopState = new StopState<StopStateType>(StopStateType.EMC, stopState);
var protectiveState = new StopState<StopStateType>(StopStateType.Protective, stopState);
var manualStopState = new StopState<StopStateType>(StopStateType.Manual, stopState);
var bumberStopState = new StopState<StopStateType>(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<RobotStateMachine> 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<RobotStateMachine> 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)

82
RobotApp/Services/State/RootState.cs
View File

@ -6,6 +6,7 @@ public class RootState<T>(RootStateType state, IRobotState? superState = null) :
{
public override void Enter()
{
base.Enter();
switch (Name)
{
case RootStateType.System:
@ -13,24 +14,99 @@ public class RootState<T>(RootStateType state, IRobotState? superState = null) :
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);
}
}

10
RobotApp/Services/State/ServiceState.cs
View File

@ -4,4 +4,14 @@ namespace RobotApp.Services.State;
public class ServiceState<T>(ServiceStateType state, IRobotState? superState = null) : RobotState<T>(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);
}
}

11
RobotApp/Services/State/StopState.cs
View File

@ -1,8 +1,15 @@
using RobotApp.Common.Shares.Enums;
namespace RobotApp.Services.State
{
namespace RobotApp.Services.State;
public class StopState<T>(StopStateType state, IRobotState? superState = null) : RobotState<T>(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);
}
}

34
RobotApp/Services/State/SystemState.cs
View File

@ -4,18 +4,32 @@ namespace RobotApp.Services.State;
public class SystemState<T>(SystemStateType state, IRobotState? superState = null) : RobotState<T>(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()
if (targetState == null) return false;
switch (Name)
{
ActiveSubState?.Update();
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;
}
}