Initial commit
This commit is contained in:
@@ -0,0 +1,71 @@
|
||||
namespace RobotNet10.RobotApp.Services.State;
|
||||
|
||||
public enum RobotEventType
|
||||
{
|
||||
// System Events
|
||||
Initialize,
|
||||
InitializeCompleted,
|
||||
Shutdown,
|
||||
ShutdownCompleted,
|
||||
|
||||
// Mode Transition Events
|
||||
EnterAuto,
|
||||
EnterManual,
|
||||
EnterService,
|
||||
EnterStop,
|
||||
EnterFault,
|
||||
ExitFault,
|
||||
|
||||
// Auto Mode Events
|
||||
StartExecution,
|
||||
PauseExecution,
|
||||
ResumeExecution,
|
||||
CancelExecution,
|
||||
CompleteExecution,
|
||||
StartRecovery,
|
||||
CompleteRecovery,
|
||||
RemoteOverride,
|
||||
|
||||
// Execution Events - Moving
|
||||
StartMoving,
|
||||
StartNavigation,
|
||||
StartAvoidance,
|
||||
StartApproach,
|
||||
StartTracking,
|
||||
StartRepositioning,
|
||||
CompleteMoving,
|
||||
|
||||
// Execution Events - ACT
|
||||
StartACT,
|
||||
StartDocking,
|
||||
CompleteDocking,
|
||||
StartCharging,
|
||||
CompleteCharging,
|
||||
StartUndocking,
|
||||
CompleteUndocking,
|
||||
StartLoading,
|
||||
CompleteLoading,
|
||||
StartUnloading,
|
||||
CompleteUnloading,
|
||||
StartTechAction,
|
||||
CompleteTechAction,
|
||||
CompleteACT,
|
||||
|
||||
// Stop Events
|
||||
EmergencyStop,
|
||||
BumperTriggered,
|
||||
ProtectiveStop,
|
||||
ManualStop,
|
||||
ReleaseStop,
|
||||
|
||||
// Fault Events
|
||||
NavigationFault,
|
||||
LocalizationFault,
|
||||
ShielfFault,
|
||||
BatteryFault,
|
||||
DriverFault,
|
||||
PeripheralsFault,
|
||||
SafetyFault,
|
||||
CommunicationFault,
|
||||
FaultResolved,
|
||||
}
|
||||
@@ -0,0 +1,336 @@
|
||||
using Appccelerate.StateMachine;
|
||||
using Appccelerate.StateMachine.AsyncMachine;
|
||||
using Appccelerate.StateMachine.AsyncMachine.Events;
|
||||
|
||||
namespace RobotNet10.RobotApp.Services.State;
|
||||
|
||||
public class RobotStateMachine(Logger<RobotStateMachine> Logger, RobotStateMachineExecute StateExecute)
|
||||
{
|
||||
private AsyncPassiveStateMachine<RobotStateType, RobotEventType>? _stateMachine;
|
||||
private RobotStateType _currentState = RobotStateType.System;
|
||||
public bool IsInitialized { get; private set; } = false;
|
||||
|
||||
public RobotStateType CurrentState => _currentState;
|
||||
public event EventHandler<StateChangedEventArgs>? StateChanged;
|
||||
|
||||
// Dictionary để track hierarchy relationships cho helper methods
|
||||
private readonly Dictionary<RobotStateType, RobotStateType> _stateHierarchies = [];
|
||||
|
||||
public async Task InitializeAsync()
|
||||
{
|
||||
if (IsInitialized) return;
|
||||
|
||||
var builder = new StateMachineDefinitionBuilder<RobotStateType, RobotEventType>();
|
||||
|
||||
// Build hierarchy map (chỉ cần track parent-child relationship)
|
||||
BuildHierarchyMap();
|
||||
|
||||
// ===========================
|
||||
// ROOT LEVEL - Hierarchical States
|
||||
// ===========================
|
||||
|
||||
// System State Hierarchy
|
||||
builder.In(RobotStateType.System)
|
||||
.On(RobotEventType.EnterAuto).Goto(RobotStateType.Auto)
|
||||
.On(RobotEventType.EnterManual).Goto(RobotStateType.Manual)
|
||||
.On(RobotEventType.EnterService).Goto(RobotStateType.Service)
|
||||
.On(RobotEventType.RemoteOverride).Goto(RobotStateType.Remote_Override)
|
||||
.On(RobotEventType.EnterStop).Goto(RobotStateType.Stop)
|
||||
.On(RobotEventType.EnterFault).Goto(RobotStateType.Fault);
|
||||
|
||||
builder.DefineHierarchyOn(RobotStateType.System)
|
||||
.WithHistoryType(HistoryType.Deep)
|
||||
.WithInitialSubState(RobotStateType.Initializing)
|
||||
.WithSubState(RobotStateType.Standby)
|
||||
.WithSubState(RobotStateType.Shutting_Down);
|
||||
|
||||
// Auto State Hierarchy
|
||||
builder.In(RobotStateType.Auto)
|
||||
.On(RobotEventType.EnterManual).Goto(RobotStateType.Manual)
|
||||
.On(RobotEventType.EnterService).Goto(RobotStateType.Service)
|
||||
.On(RobotEventType.RemoteOverride).Goto(RobotStateType.Remote_Override)
|
||||
.On(RobotEventType.EnterStop).Goto(RobotStateType.Stop)
|
||||
.On(RobotEventType.EnterFault).Goto(RobotStateType.Fault);
|
||||
|
||||
builder.DefineHierarchyOn(RobotStateType.Auto)
|
||||
.WithHistoryType(HistoryType.Deep)
|
||||
.WithInitialSubState(RobotStateType.Idle)
|
||||
.WithSubState(RobotStateType.Executing)
|
||||
.WithSubState(RobotStateType.Paused)
|
||||
.WithSubState(RobotStateType.Canceling)
|
||||
.WithSubState(RobotStateType.Recovering);
|
||||
|
||||
// Manual State
|
||||
builder.In(RobotStateType.Manual)
|
||||
.ExecuteOnEntry(() => { OnEnterState(RobotStateType.Manual); StateExecute.EntryManual(); })
|
||||
.ExecuteOnExit(StateExecute.ExitManual)
|
||||
.On(RobotEventType.EnterAuto).Goto(RobotStateType.Auto)
|
||||
.On(RobotEventType.EnterService).Goto(RobotStateType.Service)
|
||||
.On(RobotEventType.RemoteOverride).Goto(RobotStateType.Remote_Override)
|
||||
.On(RobotEventType.EnterStop).Goto(RobotStateType.Stop)
|
||||
.On(RobotEventType.EnterFault).Goto(RobotStateType.Fault);
|
||||
|
||||
// Service State
|
||||
builder.In(RobotStateType.Service)
|
||||
.ExecuteOnEntry(() => { OnEnterState(RobotStateType.Service); StateExecute.EntryService(); })
|
||||
.ExecuteOnExit(StateExecute.ExitService)
|
||||
.On(RobotEventType.EnterAuto).Goto(RobotStateType.Auto)
|
||||
.On(RobotEventType.EnterManual).Goto(RobotStateType.Manual)
|
||||
.On(RobotEventType.RemoteOverride).Goto(RobotStateType.Remote_Override)
|
||||
.On(RobotEventType.EnterStop).Goto(RobotStateType.Stop)
|
||||
.On(RobotEventType.EnterFault).Goto(RobotStateType.Fault);
|
||||
|
||||
// Stop State
|
||||
builder.In(RobotStateType.Stop)
|
||||
.ExecuteOnEntry(() => { OnEnterState(RobotStateType.Stop); StateExecute.EntryStop(); })
|
||||
.ExecuteOnExit(StateExecute.ExitStop)
|
||||
.On(RobotEventType.ReleaseStop).Goto(RobotStateType.System)
|
||||
.On(RobotEventType.EnterFault).Goto(RobotStateType.Fault);
|
||||
|
||||
// Fault State
|
||||
builder.In(RobotStateType.Fault)
|
||||
.ExecuteOnEntry(() => { OnEnterState(RobotStateType.Fault); StateExecute.EntryFault(); })
|
||||
.ExecuteOnExit(StateExecute.ExitFault)
|
||||
.On(RobotEventType.ExitFault).Goto(RobotStateType.System);
|
||||
|
||||
// Remote_Override State (top-level, peer of Service)
|
||||
builder.In(RobotStateType.Remote_Override)
|
||||
.ExecuteOnEntry(() => { OnEnterState(RobotStateType.Remote_Override); StateExecute.EntryRemoteOverride(); })
|
||||
.ExecuteOnExit(StateExecute.ExitRemoteOverride)
|
||||
.On(RobotEventType.EnterAuto).Goto(RobotStateType.Auto)
|
||||
.On(RobotEventType.EnterManual).Goto(RobotStateType.Manual)
|
||||
.On(RobotEventType.EnterService).Goto(RobotStateType.Service)
|
||||
.On(RobotEventType.EnterStop).Goto(RobotStateType.Stop)
|
||||
.On(RobotEventType.EnterFault).Goto(RobotStateType.Fault);
|
||||
|
||||
// ===========================
|
||||
// SYSTEM SUB-STATES
|
||||
// ===========================
|
||||
|
||||
builder.In(RobotStateType.Initializing)
|
||||
.ExecuteOnEntry(() => { OnEnterState(RobotStateType.Initializing); StateExecute.EntryInitializing(); })
|
||||
.On(RobotEventType.InitializeCompleted).Goto(RobotStateType.Standby)
|
||||
.On(RobotEventType.EnterFault).Goto(RobotStateType.Fault);
|
||||
|
||||
builder.In(RobotStateType.Standby)
|
||||
.ExecuteOnEntry(() => { OnEnterState(RobotStateType.Standby); StateExecute.EntryStandby(); })
|
||||
.On(RobotEventType.Shutdown).Goto(RobotStateType.Shutting_Down);
|
||||
|
||||
builder.In(RobotStateType.Shutting_Down)
|
||||
.ExecuteOnEntry(() => { OnEnterState(RobotStateType.Shutting_Down); StateExecute.EntryShuttingDown(); })
|
||||
.ExecuteOnExit(StateExecute.ExitShuttingDown)
|
||||
.On(RobotEventType.ShutdownCompleted).Goto(RobotStateType.Standby);
|
||||
|
||||
// ===========================
|
||||
// AUTO SUB-STATES
|
||||
// ===========================
|
||||
|
||||
builder.In(RobotStateType.Idle)
|
||||
.ExecuteOnEntry(() => { OnEnterState(RobotStateType.Idle); StateExecute.EntryIdle(); })
|
||||
.On(RobotEventType.StartExecution).Goto(RobotStateType.Executing);
|
||||
|
||||
// Executing State Hierarchy
|
||||
builder.In(RobotStateType.Executing)
|
||||
.ExecuteOnEntry(() => { OnEnterState(RobotStateType.Executing); StateExecute.EntryExecuting(); })
|
||||
.ExecuteOnExit(StateExecute.ExitExecuting)
|
||||
.On(RobotEventType.PauseExecution).Goto(RobotStateType.Paused)
|
||||
.On(RobotEventType.CancelExecution).Goto(RobotStateType.Canceling)
|
||||
.On(RobotEventType.CompleteExecution).Goto(RobotStateType.Idle);
|
||||
|
||||
builder.DefineHierarchyOn(RobotStateType.Executing)
|
||||
.WithHistoryType(HistoryType.Deep)
|
||||
.WithInitialSubState(RobotStateType.Moving)
|
||||
.WithSubState(RobotStateType.ACT);
|
||||
|
||||
builder.In(RobotStateType.Paused)
|
||||
.ExecuteOnEntry(() => { OnEnterState(RobotStateType.Paused); StateExecute.EntryPaused(); })
|
||||
.ExecuteOnExit(StateExecute.ExitPaused)
|
||||
.On(RobotEventType.ResumeExecution).Goto(RobotStateType.Executing)
|
||||
.On(RobotEventType.CancelExecution).Goto(RobotStateType.Canceling);
|
||||
|
||||
builder.In(RobotStateType.Canceling)
|
||||
.ExecuteOnEntry(() => { OnEnterState(RobotStateType.Canceling); StateExecute.EntryCanceling(); })
|
||||
.ExecuteOnExit(StateExecute.ExitCanceling)
|
||||
.On(RobotEventType.CompleteExecution).Goto(RobotStateType.Idle);
|
||||
|
||||
builder.In(RobotStateType.Recovering)
|
||||
.ExecuteOnEntry(() => { OnEnterState(RobotStateType.Recovering); StateExecute.EntryRecovering(); })
|
||||
.On(RobotEventType.CompleteRecovery).Goto(RobotStateType.Idle);
|
||||
|
||||
// ===========================
|
||||
// EXECUTING SUB-STATES
|
||||
// ===========================
|
||||
|
||||
// Moving State Hierarchy
|
||||
builder.In(RobotStateType.Moving)
|
||||
.ExecuteOnEntry(() => { OnEnterState(RobotStateType.Moving); StateExecute.EntryMoving(); })
|
||||
.ExecuteOnExit(StateExecute.ExitMoving)
|
||||
.On(RobotEventType.StartACT).Goto(RobotStateType.ACT)
|
||||
.On(RobotEventType.CompleteMoving).Goto(RobotStateType.Idle);
|
||||
|
||||
// ACT State Hierarchy
|
||||
builder.In(RobotStateType.ACT)
|
||||
.ExecuteOnEntry(() => { OnEnterState(RobotStateType.ACT); StateExecute.EntryACT(); })
|
||||
.ExecuteOnExit(StateExecute.ExitACT)
|
||||
.On(RobotEventType.StartMoving).Goto(RobotStateType.Moving)
|
||||
.On(RobotEventType.CompleteACT).Goto(RobotStateType.Idle);
|
||||
|
||||
builder.DefineHierarchyOn(RobotStateType.ACT)
|
||||
.WithHistoryType(HistoryType.Deep)
|
||||
.WithInitialSubState(RobotStateType.Docking)
|
||||
.WithSubState(RobotStateType.Docked)
|
||||
.WithSubState(RobotStateType.Charging)
|
||||
.WithSubState(RobotStateType.Undocking)
|
||||
.WithSubState(RobotStateType.Loading)
|
||||
.WithSubState(RobotStateType.Unloading)
|
||||
.WithSubState(RobotStateType.TechAction);
|
||||
|
||||
// ===========================
|
||||
// ACT SUB-STATES
|
||||
// ===========================
|
||||
|
||||
builder.In(RobotStateType.Docking)
|
||||
.ExecuteOnEntry(() => { OnEnterState(RobotStateType.Docking); StateExecute.EntryDocking(); })
|
||||
.On(RobotEventType.CompleteDocking).Goto(RobotStateType.Docked);
|
||||
|
||||
builder.In(RobotStateType.Docked)
|
||||
.ExecuteOnEntry(() => { OnEnterState(RobotStateType.Docked); StateExecute.EntryDocked(); })
|
||||
.On(RobotEventType.StartCharging).Goto(RobotStateType.Charging)
|
||||
.On(RobotEventType.StartUndocking).Goto(RobotStateType.Undocking)
|
||||
.On(RobotEventType.StartLoading).Goto(RobotStateType.Loading)
|
||||
.On(RobotEventType.StartUnloading).Goto(RobotStateType.Unloading);
|
||||
|
||||
builder.In(RobotStateType.Charging)
|
||||
.ExecuteOnEntry(() => { OnEnterState(RobotStateType.Charging); StateExecute.EntryCharging(); })
|
||||
.ExecuteOnExit(StateExecute.ExitCharging)
|
||||
.On(RobotEventType.CompleteCharging).Goto(RobotStateType.Docked);
|
||||
|
||||
builder.In(RobotStateType.Undocking)
|
||||
.ExecuteOnEntry(() => { OnEnterState(RobotStateType.Undocking); StateExecute.EntryUndocking(); })
|
||||
.On(RobotEventType.CompleteUndocking).Goto(RobotStateType.Docking);
|
||||
|
||||
builder.In(RobotStateType.Loading)
|
||||
.ExecuteOnEntry(() => { OnEnterState(RobotStateType.Loading); StateExecute.EntryLoading(); })
|
||||
.On(RobotEventType.CompleteLoading).Goto(RobotStateType.Docked);
|
||||
|
||||
builder.In(RobotStateType.Unloading)
|
||||
.ExecuteOnEntry(() => { OnEnterState(RobotStateType.Unloading); StateExecute.EntryUnloading(); })
|
||||
.On(RobotEventType.CompleteUnloading).Goto(RobotStateType.Docked);
|
||||
|
||||
builder.In(RobotStateType.TechAction)
|
||||
.ExecuteOnEntry(() => { OnEnterState(RobotStateType.TechAction); StateExecute.EntryTechAction(); })
|
||||
.On(RobotEventType.CompleteTechAction).Goto(RobotStateType.Docked);
|
||||
|
||||
// ===========================
|
||||
// CREATE STATE MACHINE
|
||||
// ===========================
|
||||
|
||||
_stateMachine = builder
|
||||
.WithInitialState(RobotStateType.System)
|
||||
.Build()
|
||||
.CreatePassiveStateMachine();
|
||||
|
||||
// Subscribe to state change events
|
||||
_stateMachine.TransitionCompleted += OnTransitionCompleted;
|
||||
|
||||
// Set IsInitialized = true TRƯỚC khi Start() để tránh deadlock
|
||||
// Vì EntryInitializing() có thể gọi ModuleInitializeAsync() chờ IsInitialized
|
||||
IsInitialized = true;
|
||||
|
||||
// Start state machine
|
||||
await _stateMachine.Start();
|
||||
|
||||
Logger.Info($"State Machine initialized successfully with current state: {CurrentState}");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Build hierarchy map - chỉ để track parent-child relationships cho helper methods
|
||||
/// Không cần build transitions vì Appccelerate tự động xử lý
|
||||
/// </summary>
|
||||
private void BuildHierarchyMap()
|
||||
{
|
||||
// System sub-states
|
||||
_stateHierarchies[RobotStateType.Initializing] = RobotStateType.System;
|
||||
_stateHierarchies[RobotStateType.Standby] = RobotStateType.System;
|
||||
_stateHierarchies[RobotStateType.Shutting_Down] = RobotStateType.System;
|
||||
|
||||
// Auto sub-states
|
||||
_stateHierarchies[RobotStateType.Idle] = RobotStateType.Auto;
|
||||
_stateHierarchies[RobotStateType.Executing] = RobotStateType.Auto;
|
||||
_stateHierarchies[RobotStateType.Paused] = RobotStateType.Auto;
|
||||
_stateHierarchies[RobotStateType.Canceling] = RobotStateType.Auto;
|
||||
_stateHierarchies[RobotStateType.Recovering] = RobotStateType.Auto;
|
||||
|
||||
// Executing sub-states
|
||||
_stateHierarchies[RobotStateType.Moving] = RobotStateType.Executing;
|
||||
_stateHierarchies[RobotStateType.ACT] = RobotStateType.Executing;
|
||||
|
||||
// ACT sub-states
|
||||
_stateHierarchies[RobotStateType.Docking] = RobotStateType.ACT;
|
||||
_stateHierarchies[RobotStateType.Docked] = RobotStateType.ACT;
|
||||
_stateHierarchies[RobotStateType.Charging] = RobotStateType.ACT;
|
||||
_stateHierarchies[RobotStateType.Undocking] = RobotStateType.ACT;
|
||||
_stateHierarchies[RobotStateType.Loading] = RobotStateType.ACT;
|
||||
_stateHierarchies[RobotStateType.Unloading] = RobotStateType.ACT;
|
||||
_stateHierarchies[RobotStateType.TechAction] = RobotStateType.ACT;
|
||||
}
|
||||
|
||||
public void Initialize() => InitializeAsync().GetAwaiter().GetResult();
|
||||
|
||||
public async Task FireAsync(RobotEventType eventType)
|
||||
{
|
||||
if (!IsInitialized)
|
||||
{
|
||||
Logger.Warning("State Machine not initialized!");
|
||||
return;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
if (_stateMachine != null)
|
||||
{
|
||||
await _stateMachine.Fire(eventType);
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Logger.Error($"Fire event {eventType} error: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
public void Fire(RobotEventType eventType) => FireAsync(eventType).GetAwaiter().GetResult();
|
||||
|
||||
public bool IsInState(RobotStateType state)
|
||||
{
|
||||
if (!IsInitialized) return false;
|
||||
if (CurrentState == state) return true;
|
||||
if(_stateHierarchies.TryGetValue(CurrentState, out RobotStateType parentState))
|
||||
{
|
||||
if(parentState == state) return true;
|
||||
|
||||
while (_stateHierarchies.TryGetValue(parentState, out parentState))
|
||||
{
|
||||
if (parentState == state) return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
private void OnEnterState(RobotStateType state)
|
||||
{
|
||||
_currentState = state;
|
||||
}
|
||||
|
||||
private void OnTransitionCompleted(object? sender, TransitionCompletedEventArgs<RobotStateType, RobotEventType> e)
|
||||
{
|
||||
Logger.Info($"State Transition: {e.StateId} -> Event: {e.EventId}");
|
||||
StateChanged?.Invoke(this, new StateChangedEventArgs(e.StateId, e.EventId));
|
||||
}
|
||||
}
|
||||
|
||||
public class StateChangedEventArgs(RobotStateType newState, RobotEventType eventType) : EventArgs
|
||||
{
|
||||
public RobotStateType NewState { get; } = newState;
|
||||
public RobotEventType EventType { get; } = eventType;
|
||||
}
|
||||
@@ -0,0 +1,890 @@
|
||||
using System.Threading;
|
||||
using RobotNet10.RobotApp.Interfaces;
|
||||
using RobotNet10.RobotApp.Motion;
|
||||
using RobotNet10.RobotApp.Services.Robot;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
|
||||
namespace RobotNet10.RobotApp.Services.State;
|
||||
|
||||
/// <summary>
|
||||
/// Class chứa các implementation methods cho Entry/Exit actions của State Machine
|
||||
/// Sử dụng IServiceProvider để có thể inject các services khác khi cần
|
||||
/// </summary>
|
||||
public class RobotStateMachineExecute(IServiceScopeFactory ServiceScopeFactory, IPlcController PlcController, Logger<RobotStateMachineExecute> Logger)
|
||||
{
|
||||
// ===========================
|
||||
// SYSTEM STATE HANDLERS
|
||||
// ===========================
|
||||
|
||||
/// <summary>
|
||||
/// Entry: System -> Initializing
|
||||
/// Thực hiện khởi tạo các component của robot
|
||||
/// </summary>
|
||||
public async Task EntryInitializingAsync()
|
||||
{
|
||||
Logger.Info("==> Entry Initializing State");
|
||||
|
||||
try
|
||||
{
|
||||
// TODO: Khởi tạo các services
|
||||
// - Kiểm tra kết nối phần cứng
|
||||
// - Khởi tạo Driver
|
||||
// - Khởi tạo Sensors
|
||||
// - Load cấu hình
|
||||
|
||||
using var scope = ServiceScopeFactory.CreateAsyncScope();
|
||||
var robotController = scope.ServiceProvider.GetRequiredService<RobotController>();
|
||||
await robotController.ModuleInitializeAsync();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Logger.Error($"Initialization error: {ex.Message}");
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
public void EntryInitializing() => EntryInitializingAsync().GetAwaiter().GetResult();
|
||||
|
||||
/// <summary>
|
||||
/// Entry: System -> Standby
|
||||
/// Robot đã sẵn sàng hoạt động
|
||||
/// </summary>
|
||||
public void EntryStandby()
|
||||
{
|
||||
Logger.Info("==> Entry Standby State");
|
||||
PlcController.SetSystemState(SystemState.IDLE);
|
||||
|
||||
using var scope = ServiceScopeFactory.CreateAsyncScope();
|
||||
var stateService = scope.ServiceProvider.GetRequiredService<RobotStates>();
|
||||
var visualizationService = scope.ServiceProvider.GetRequiredService<RobotVisualization>();
|
||||
stateService.Start();
|
||||
visualizationService.Start();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Entry: System -> Shutting_Down
|
||||
/// Bắt đầu quá trình tắt máy
|
||||
/// </summary>
|
||||
public async Task EntryShuttingDownAsync()
|
||||
{
|
||||
Logger.Info("==> Entry Shutting Down State");
|
||||
|
||||
try
|
||||
{
|
||||
// TODO:
|
||||
// - Lưu trạng thái hiện tại
|
||||
// - Dừng tất cả các task đang chạy
|
||||
// - Ngắt kết nối an toàn
|
||||
|
||||
using var scope = ServiceScopeFactory.CreateAsyncScope();
|
||||
var robotController = scope.ServiceProvider.GetRequiredService<RobotController>();
|
||||
robotController.StopHandler();
|
||||
|
||||
Logger.Info("Shutdown completed");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Logger.Error($"Shutdown error: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
public void EntryShuttingDown() => EntryShuttingDownAsync().GetAwaiter().GetResult();
|
||||
|
||||
/// <summary>
|
||||
/// Exit: System -> Shutting_Down
|
||||
/// </summary>
|
||||
public void ExitShuttingDown()
|
||||
{
|
||||
Logger.Info("<== Exit Shutting Down State");
|
||||
}
|
||||
|
||||
// ===========================
|
||||
// AUTO MODE STATE HANDLERS
|
||||
// ===========================
|
||||
|
||||
/// <summary>
|
||||
/// Entry: Auto -> Idle
|
||||
/// Robot sẵn sàng nhận nhiệm vụ mới
|
||||
/// </summary>
|
||||
public void EntryIdle()
|
||||
{
|
||||
Logger.Info("==> Entry Idle State");
|
||||
PlcController.SetSystemState(SystemState.IDLE);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Entry: Auto -> Executing
|
||||
/// Bắt đầu thực hiện nhiệm vụ
|
||||
/// </summary>
|
||||
public void EntryExecuting()
|
||||
{
|
||||
Logger.Info("==> Entry Executing State");
|
||||
using var scope = ServiceScopeFactory.CreateAsyncScope();
|
||||
var PlcController = scope.ServiceProvider.GetRequiredService<IPlcController>();
|
||||
PlcController.SetOperationState(OperationState.Move);
|
||||
// TODO:
|
||||
// - Lấy nhiệm vụ từ queue
|
||||
// - Khởi tạo execution context
|
||||
// - Bắt đầu timer theo dõi
|
||||
// - Cập nhật VDA5050 state = "EXECUTING"
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Exit: Auto -> Executing
|
||||
/// </summary>
|
||||
public void ExitExecuting()
|
||||
{
|
||||
Logger.Info("<== Exit Executing State");
|
||||
|
||||
using var scope = ServiceScopeFactory.CreateAsyncScope();
|
||||
var PlcController = scope.ServiceProvider.GetRequiredService<IPlcController>();
|
||||
PlcController.SetOperationState(OperationState.None);
|
||||
|
||||
// TODO:
|
||||
// - Cleanup execution context
|
||||
// - Dừng timer
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Entry: Auto -> Paused
|
||||
/// Tạm dừng thực hiện nhiệm vụ
|
||||
/// </summary>
|
||||
public void EntryPaused()
|
||||
{
|
||||
Logger.Info("==> Entry Paused State");
|
||||
|
||||
// TODO:
|
||||
// - Lưu trạng thái hiện tại
|
||||
// - Dừng robot
|
||||
// - Cập nhật VDA5050 state = "PAUSED"
|
||||
|
||||
// Dừng robot
|
||||
StopRobot();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Exit: Auto -> Paused
|
||||
/// Resume từ trạng thái pause
|
||||
/// </summary>
|
||||
public void ExitPaused()
|
||||
{
|
||||
Logger.Info("<== Exit Paused State");
|
||||
|
||||
// TODO:
|
||||
// - Khôi phục trạng thái
|
||||
// - Chuẩn bị tiếp tục thực hiện
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Entry: Auto -> Canceling
|
||||
/// Đang hủy nhiệm vụ
|
||||
/// </summary>
|
||||
public void EntryCanceling()
|
||||
{
|
||||
Logger.Info("==> Entry Canceling State");
|
||||
|
||||
// TODO:
|
||||
// - Dừng robot ngay lập tức
|
||||
// - Hủy tất cả các task con
|
||||
// - Cleanup resources
|
||||
|
||||
StopRobot();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Exit: Auto -> Canceling
|
||||
/// Hoàn tất việc hủy
|
||||
/// </summary>
|
||||
public void ExitCanceling()
|
||||
{
|
||||
Logger.Info("<== Exit Canceling State");
|
||||
|
||||
// TODO:
|
||||
// - Gửi thông báo nhiệm vụ đã hủy
|
||||
// - Reset execution context
|
||||
// - Cập nhật VDA5050 với error/cancelled
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Entry: Auto -> Recovering
|
||||
/// Đang khôi phục từ lỗi
|
||||
/// </summary>
|
||||
public async Task EntryRecoveringAsync()
|
||||
{
|
||||
Logger.Info("==> Entry Recovering State");
|
||||
|
||||
try
|
||||
{
|
||||
// TODO:
|
||||
// - Phân tích lỗi
|
||||
// - Thực hiện recovery procedure
|
||||
// - Kiểm tra trạng thái hệ thống
|
||||
|
||||
Logger.Info("Analyzing error...");
|
||||
await Task.Delay(100);
|
||||
|
||||
Logger.Info("Recovering system...");
|
||||
await Task.Delay(500);
|
||||
|
||||
Logger.Info("Recovery succeeded");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Logger.Error($"Recovery error: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
public void EntryRecovering() => EntryRecoveringAsync().GetAwaiter().GetResult();
|
||||
|
||||
/// <summary>
|
||||
/// Entry: Auto -> Remote_Override (OVERRIDE)
|
||||
/// Chuyển sang chế độ điều khiển từ xa - cho phép override tất cả safety
|
||||
/// </summary>
|
||||
public void EntryRemoteOverride()
|
||||
{
|
||||
Logger.Info("==> Entry Remote Override State (OVERRIDE)");
|
||||
PlcController.SetSystemState(SystemState.OVERRIDE);
|
||||
StopRobot();
|
||||
|
||||
// Set ManualControlService state to Override - cho phép điều khiển với full override
|
||||
try
|
||||
{
|
||||
using var scope = ServiceScopeFactory.CreateScope();
|
||||
var manualControlService = scope.ServiceProvider.GetRequiredService<ManualControlService>();
|
||||
manualControlService.SetState(ManualControlState.Override);
|
||||
Logger.Info("ManualControlService set to Override state");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Logger.Error($"Error setting ManualControlService to Override: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Exit: Auto -> Remote_Override (OVERRIDE)
|
||||
/// </summary>
|
||||
public void ExitRemoteOverride()
|
||||
{
|
||||
Logger.Info("<== Exit Remote Override State (OVERRIDE)");
|
||||
|
||||
// Clear ManualControlService external state
|
||||
try
|
||||
{
|
||||
using var scope = ServiceScopeFactory.CreateScope();
|
||||
var manualControlService = scope.ServiceProvider.GetRequiredService<ManualControlService>();
|
||||
manualControlService.ClearExternalState();
|
||||
Logger.Info("ManualControlService external state cleared");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Logger.Error($"Error clearing ManualControlService external state: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
// ===========================
|
||||
// EXECUTING - MOVING STATE HANDLERS
|
||||
// ===========================
|
||||
|
||||
/// <summary>
|
||||
/// Entry: Executing -> Moving
|
||||
/// Bắt đầu di chuyển
|
||||
/// </summary>
|
||||
public void EntryMoving()
|
||||
{
|
||||
Logger.Info("==> Entry Moving State");
|
||||
PlcController.SetSystemState(SystemState.PROCCESSING);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Exit: Executing -> Moving
|
||||
/// </summary>
|
||||
public void ExitMoving()
|
||||
{
|
||||
Logger.Info("<== Exit Moving State");
|
||||
|
||||
// TODO:
|
||||
// - Dừng robot
|
||||
// - Lưu vị trí cuối cùng
|
||||
|
||||
StopRobot();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Entry: Moving -> Navigation
|
||||
/// Chế độ navigation bình thường
|
||||
/// </summary>
|
||||
public void EntryNavigation()
|
||||
{
|
||||
Logger.Info("==> Entry Navigation State");
|
||||
|
||||
// TODO:
|
||||
// - Load path từ planner
|
||||
// - Bắt đầu path following
|
||||
// - Monitor obstacles
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Entry: Moving -> Avoidance
|
||||
/// Đang tránh chướng ngại vật
|
||||
/// </summary>
|
||||
public void EntryAvoidance()
|
||||
{
|
||||
Logger.Info("==> Entry Avoidance State");
|
||||
|
||||
// TODO:
|
||||
// - Giảm tốc độ
|
||||
// - Tính toán đường tránh
|
||||
// - Theo dõi obstacle
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Exit: Moving -> Avoidance
|
||||
/// </summary>
|
||||
public void ExitAvoidance()
|
||||
{
|
||||
Logger.Info("<== Exit Avoidance State");
|
||||
|
||||
// TODO:
|
||||
// - Quay lại tốc độ bình thường
|
||||
// - Resume path chính
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Entry: Moving -> Approach
|
||||
/// Tiếp cận mục tiêu (precision mode)
|
||||
/// </summary>
|
||||
public void EntryApproach()
|
||||
{
|
||||
Logger.Info("==> Entry Approach State");
|
||||
|
||||
// TODO:
|
||||
// - Chuyển sang precision mode
|
||||
// - Giảm tốc độ tối đa
|
||||
// - Sử dụng sensors chính xác cao
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Entry: Moving -> Tracking
|
||||
/// Theo dõi mục tiêu động
|
||||
/// </summary>
|
||||
public void EntryTracking()
|
||||
{
|
||||
Logger.Info("==> Entry Tracking State");
|
||||
|
||||
// TODO:
|
||||
// - Bật target tracking
|
||||
// - Theo dõi vị trí mục tiêu
|
||||
// - Điều chỉnh trajectory theo realtime
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Entry: Moving -> Repositioning
|
||||
/// Điều chỉnh lại vị trí
|
||||
/// </summary>
|
||||
public void EntryRepositioning()
|
||||
{
|
||||
Logger.Info("==> Entry Repositioning State");
|
||||
|
||||
// TODO:
|
||||
// - Tính toán vị trí mong muốn
|
||||
// - Di chuyển điều chỉnh nhỏ
|
||||
// - Kiểm tra orientation
|
||||
}
|
||||
|
||||
// ===========================
|
||||
// EXECUTING - ACT STATE HANDLERS
|
||||
// ===========================
|
||||
|
||||
/// <summary>
|
||||
/// Entry: Executing -> ACT
|
||||
/// Bắt đầu thực hiện action
|
||||
/// </summary>
|
||||
public void EntryACT()
|
||||
{
|
||||
Logger.Info("==> Entry ACT State");
|
||||
|
||||
// TODO:
|
||||
// - Dừng robot hoàn toàn
|
||||
// - Chuẩn bị thực hiện action
|
||||
// - Kiểm tra vị trí chính xác
|
||||
|
||||
StopRobot();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Exit: Executing -> ACT
|
||||
/// </summary>
|
||||
public void ExitACT()
|
||||
{
|
||||
Logger.Info("<== Exit ACT State");
|
||||
|
||||
// TODO:
|
||||
// - Cleanup action resources
|
||||
// - Kiểm tra kết quả action
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Entry: ACT -> Docking
|
||||
/// Đang thực hiện docking
|
||||
/// </summary>
|
||||
public async Task EntryDockingAsync()
|
||||
{
|
||||
Logger.Info("==> Entry Docking State");
|
||||
PlcController.SetSystemState(SystemState.DOCKING);
|
||||
|
||||
try
|
||||
{
|
||||
// TODO:
|
||||
// - Tìm vị trí dock station
|
||||
// - Align với dock
|
||||
// - Di chuyển vào dock từ từ
|
||||
|
||||
Logger.Info("Searching for dock station...");
|
||||
await Task.Delay(200);
|
||||
|
||||
Logger.Info("Aligning with dock...");
|
||||
await Task.Delay(300);
|
||||
|
||||
Logger.Info("Moving into dock...");
|
||||
await Task.Delay(500);
|
||||
|
||||
Logger.Info("Docking succeeded");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Logger.Error($"Docking error: {ex.Message}");
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
public void EntryDocking() => EntryDockingAsync().GetAwaiter().GetResult();
|
||||
|
||||
/// <summary>
|
||||
/// Entry: ACT -> Docked
|
||||
/// Đã docked thành công
|
||||
/// </summary>
|
||||
public void EntryDocked()
|
||||
{
|
||||
Logger.Info("==> Entry Docked State");
|
||||
PlcController.SetSystemState(SystemState.DOCKING);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Entry: ACT -> Charging
|
||||
/// Đang sạc pin
|
||||
/// </summary>
|
||||
public async Task EntryChargingAsync()
|
||||
{
|
||||
Logger.Info("==> Entry Charging State");
|
||||
PlcController.SetSystemState(SystemState.CHARGING);
|
||||
|
||||
try
|
||||
{
|
||||
// TODO:
|
||||
// - Bắt đầu charging
|
||||
// - Monitor battery level
|
||||
// - Monitor charging current/voltage
|
||||
|
||||
Logger.Info("Connecting charging power...");
|
||||
await Task.Delay(100);
|
||||
|
||||
Logger.Info("Starting battery charging...");
|
||||
// Charging loop sẽ được xử lý bởi battery service
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Logger.Error($"Charging error: {ex.Message}");
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
public void EntryCharging() => EntryChargingAsync().GetAwaiter().GetResult();
|
||||
|
||||
/// <summary>
|
||||
/// Exit: ACT -> Charging
|
||||
/// </summary>
|
||||
public void ExitCharging()
|
||||
{
|
||||
Logger.Info("<== Exit Charging State");
|
||||
|
||||
// TODO:
|
||||
// - Dừng charging
|
||||
// - Ngắt kết nối nguồn an toàn
|
||||
// - Log battery level
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Entry: ACT -> Undocking
|
||||
/// Đang rời khỏi dock
|
||||
/// </summary>
|
||||
public async Task EntryUndockingAsync()
|
||||
{
|
||||
Logger.Info("==> Entry Undocking State");
|
||||
PlcController.SetSystemState(SystemState.DOCKING);
|
||||
|
||||
try
|
||||
{
|
||||
// TODO:
|
||||
// - Kiểm tra an toàn
|
||||
// - Ngắt kết nối với dock
|
||||
// - Di chuyển ra khỏi dock
|
||||
|
||||
Logger.Info("Checking safety...");
|
||||
await Task.Delay(100);
|
||||
|
||||
Logger.Info("Disconnecting dock...");
|
||||
await Task.Delay(200);
|
||||
|
||||
Logger.Info("Moving out of dock...");
|
||||
await Task.Delay(500);
|
||||
|
||||
Logger.Info("Undocking succeeded");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Logger.Error($"Undocking error: {ex.Message}");
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
public void EntryUndocking() => EntryUndockingAsync().GetAwaiter().GetResult();
|
||||
|
||||
/// <summary>
|
||||
/// Entry: ACT -> Loading
|
||||
/// Đang tải hàng
|
||||
/// </summary>
|
||||
public async Task EntryLoadingAsync()
|
||||
{
|
||||
Logger.Info("==> Entry Loading State");
|
||||
PlcController.SetSystemState(SystemState.PROCCESSING);
|
||||
|
||||
try
|
||||
{
|
||||
// TODO:
|
||||
// - Kiểm tra payload capacity
|
||||
// - Điều khiển loading mechanism
|
||||
// - Xác nhận hàng đã được tải
|
||||
|
||||
Logger.Info("Preparing loading...");
|
||||
await Task.Delay(200);
|
||||
|
||||
Logger.Info("Loading cargo...");
|
||||
await Task.Delay(1000);
|
||||
|
||||
Logger.Info("Loading completed");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Logger.Error($"Loading error: {ex.Message}");
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
public void EntryLoading() => EntryLoadingAsync().GetAwaiter().GetResult();
|
||||
|
||||
/// <summary>
|
||||
/// Entry: ACT -> Unloading
|
||||
/// Đang dỡ hàng
|
||||
/// </summary>
|
||||
public async Task EntryUnloadingAsync()
|
||||
{
|
||||
Logger.Info("==> Entry Unloading State");
|
||||
PlcController.SetSystemState(SystemState.PROCCESSING);
|
||||
|
||||
try
|
||||
{
|
||||
// TODO:
|
||||
// - Kiểm tra vị trí unload
|
||||
// - Điều khiển unloading mechanism
|
||||
// - Xác nhận hàng đã được dỡ
|
||||
|
||||
Logger.Info("Preparing unloading...");
|
||||
await Task.Delay(200);
|
||||
|
||||
Logger.Info("Unloading cargo...");
|
||||
await Task.Delay(1000);
|
||||
|
||||
Logger.Info("Unloading completed");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Logger.Error($"Unloading error: {ex.Message}");
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
public void EntryUnloading() => EntryUnloadingAsync().GetAwaiter().GetResult();
|
||||
|
||||
/// <summary>
|
||||
/// Entry: ACT -> TechAction
|
||||
/// Thực hiện các action kỹ thuật đặc biệt
|
||||
/// </summary>
|
||||
public async Task EntryTechActionAsync()
|
||||
{
|
||||
Logger.Info("==> Entry Tech Action State");
|
||||
PlcController.SetSystemState(SystemState.PROCCESSING);
|
||||
|
||||
try
|
||||
{
|
||||
// TODO:
|
||||
// - Xác định loại tech action
|
||||
// - Thực hiện action tương ứng
|
||||
// - Log kết quả
|
||||
|
||||
Logger.Info("Executing tech action...");
|
||||
await Task.Delay(500);
|
||||
|
||||
Logger.Info("Tech action completed");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Logger.Error($"Tech action error: {ex.Message}");
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
public void EntryTechAction() => EntryTechActionAsync().GetAwaiter().GetResult();
|
||||
|
||||
// ===========================
|
||||
// MODE TRANSITION HANDLERS
|
||||
// ===========================
|
||||
|
||||
/// <summary>
|
||||
/// Entry: Manual Mode
|
||||
/// Chuyển sang chế độ thủ công
|
||||
/// </summary>
|
||||
public void EntryManual()
|
||||
{
|
||||
Logger.Info("==> Entry Manual Mode");
|
||||
PlcController.SetSystemState(SystemState.MANUAL);
|
||||
StopRobot();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Exit: Manual Mode
|
||||
/// </summary>
|
||||
public void ExitManual()
|
||||
{
|
||||
Logger.Info("<== Exit Manual Mode");
|
||||
|
||||
// TODO:
|
||||
// - Kiểm tra an toàn
|
||||
// - Tắt manual control
|
||||
// - Bật lại autonomous control
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Entry: Service Mode (MAINTENANCE)
|
||||
/// Chuyển sang chế độ bảo trì - cho phép điều khiển robot từ RF Handle
|
||||
/// </summary>
|
||||
public void EntryService()
|
||||
{
|
||||
Logger.Info("==> Entry Service Mode (MAINTENANCE)");
|
||||
PlcController.SetSystemState(SystemState.MAINTENANCE);
|
||||
StopRobot();
|
||||
|
||||
// Set ManualControlService state to Maintenance - cho phép điều khiển từ RF Handle
|
||||
try
|
||||
{
|
||||
using var scope = ServiceScopeFactory.CreateScope();
|
||||
var manualControlService = scope.ServiceProvider.GetRequiredService<ManualControlService>();
|
||||
manualControlService.SetState(ManualControlState.Maintenance);
|
||||
Logger.Info("ManualControlService set to Maintenance state");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Logger.Error($"Error setting ManualControlService to Maintenance: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Exit: Service Mode (MAINTENANCE)
|
||||
/// </summary>
|
||||
public void ExitService()
|
||||
{
|
||||
Logger.Info("<== Exit Service Mode (MAINTENANCE)");
|
||||
|
||||
// Clear ManualControlService external state
|
||||
try
|
||||
{
|
||||
using var scope = ServiceScopeFactory.CreateScope();
|
||||
var manualControlService = scope.ServiceProvider.GetRequiredService<ManualControlService>();
|
||||
manualControlService.ClearExternalState();
|
||||
Logger.Info("ManualControlService external state cleared");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Logger.Error($"Error clearing ManualControlService external state: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Entry: Stop State
|
||||
/// Robot bị dừng (Emergency, Bumper, etc.)
|
||||
/// </summary>
|
||||
public void EntryStop()
|
||||
{
|
||||
Logger.Warning("==> Entry STOP State");
|
||||
PlcController.SetSystemState(SystemState.PAUSED);
|
||||
EmergencyStop();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Exit: Stop State
|
||||
/// Giải phóng stop
|
||||
/// </summary>
|
||||
public void ExitStop()
|
||||
{
|
||||
Logger.Info("<== Exit Stop State");
|
||||
|
||||
// Giống Lock (SERVICE) → Auto/Manual: M815 pulse + fault reset + EnableAsync (RobotController.ApplyResetFromPlc).
|
||||
ApplyAlarmResetAndEnableMotorsLikeLock();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Entry: Fault State
|
||||
/// Robot gặp lỗi nghiêm trọng
|
||||
/// </summary>
|
||||
public void EntryFault()
|
||||
{
|
||||
Logger.Error("==> Entry FAULT State");
|
||||
PlcController.SetSystemState(SystemState.ERROR);
|
||||
EmergencyStop();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Exit: Fault State
|
||||
/// Đã khắc phục lỗi
|
||||
/// </summary>
|
||||
public void ExitFault()
|
||||
{
|
||||
Logger.Info("<== Exit Fault State");
|
||||
|
||||
// 1. Clear remaining fatal errors
|
||||
try
|
||||
{
|
||||
using var scope = ServiceScopeFactory.CreateScope();
|
||||
var errorManager = scope.ServiceProvider.GetRequiredService<IError>();
|
||||
errorManager.ClearFatalErrors();
|
||||
}
|
||||
catch (Exception ex) { Logger.Error($"ExitFault: Error clearing errors: {ex.Message}"); }
|
||||
|
||||
// 2. PLC alarm reset + servo: cùng chuỗi như thoát Lock / ApplyResetFromPlc
|
||||
ApplyAlarmResetAndEnableMotorsLikeLock();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Pulse M815 (alarm reset trên PLC), sau đó FaultReset + chờ + EnableAsync — khớp RobotController.ApplyResetFromPlc (không gọi TryClearFault).
|
||||
/// </summary>
|
||||
private void ApplyAlarmResetAndEnableMotorsLikeLock()
|
||||
{
|
||||
Logger.Info("ApplyAlarmResetAndEnableMotorsLikeLock: M815 + fault reset + enable (như Lock → Auto/Manual)");
|
||||
try { PlcController.WriteAlarmResetM815(); }
|
||||
catch (Exception ex) { Logger.Warning($"WriteAlarmResetM815: {ex.Message}"); }
|
||||
|
||||
try
|
||||
{
|
||||
using var scope = ServiceScopeFactory.CreateScope();
|
||||
var ik = scope.ServiceProvider.GetService<IInverseKinematics>();
|
||||
if (ik == null)
|
||||
{
|
||||
Logger.Warning("ApplyAlarmResetAndEnableMotorsLikeLock: IInverseKinematics không có");
|
||||
return;
|
||||
}
|
||||
|
||||
ik.FaultReset();
|
||||
Thread.Sleep(1500);
|
||||
ik.FaultReset();
|
||||
Thread.Sleep(800);
|
||||
ik.EnableAsync(CancellationToken.None).GetAwaiter().GetResult();
|
||||
if (ik.IsOperationEnabled)
|
||||
Logger.Info("ApplyAlarmResetAndEnableMotorsLikeLock: động cơ OperationEnabled");
|
||||
else
|
||||
Logger.Warning("ApplyAlarmResetAndEnableMotorsLikeLock: chưa OperationEnabled sau EnableAsync");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Logger.Warning($"ApplyAlarmResetAndEnableMotorsLikeLock: FaultReset/Enable — {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
// ===========================
|
||||
// HELPER METHODS
|
||||
// ===========================
|
||||
|
||||
/// <summary>
|
||||
/// Dừng robot bình thường - gửi zero velocity đến IInverseKinematics
|
||||
/// </summary>
|
||||
private void StopRobot()
|
||||
{
|
||||
try
|
||||
{
|
||||
using var scope = ServiceScopeFactory.CreateScope();
|
||||
var inverseKinematics = scope.ServiceProvider.GetService<IInverseKinematics>();
|
||||
|
||||
if (inverseKinematics == null)
|
||||
{
|
||||
Logger.Warning("IInverseKinematics not available, cannot stop robot");
|
||||
return;
|
||||
}
|
||||
|
||||
// Send zero velocity to stop the robot
|
||||
// SetVelocity not available - commented out
|
||||
// var zeroTwist = new Twist();
|
||||
// inverseKinematics.SetVelocity(zeroTwist);
|
||||
|
||||
Logger.Info("Robot stopped (zero velocity intended)");
|
||||
}
|
||||
catch (AggregateException ex)
|
||||
{
|
||||
Logger.Error($"Stop robot error: {ex.InnerException?.Message ?? ex.Message}");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Logger.Error($"Stop robot error: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Dừng khẩn cấp robot - gửi zero velocity và disable IInverseKinematics
|
||||
/// </summary>
|
||||
private void EmergencyStop()
|
||||
{
|
||||
try
|
||||
{
|
||||
using var scope = ServiceScopeFactory.CreateScope();
|
||||
var inverseKinematics = scope.ServiceProvider.GetService<IInverseKinematics>();
|
||||
|
||||
if (inverseKinematics == null)
|
||||
{
|
||||
Logger.Warning("IInverseKinematics not available, cannot emergency stop robot");
|
||||
return;
|
||||
}
|
||||
|
||||
// Send zero velocity first
|
||||
// SetVelocity not available - commented out
|
||||
// var zeroTwist = new Twist();
|
||||
// inverseKinematics.SetVelocity(zeroTwist);
|
||||
|
||||
// Disable the drive (if supported)
|
||||
try
|
||||
{
|
||||
inverseKinematics.Disable();
|
||||
}
|
||||
catch (Exception disableEx)
|
||||
{
|
||||
Logger.Warning($"Could not disable IInverseKinematics: {disableEx.Message}");
|
||||
}
|
||||
|
||||
Logger.Warning("EMERGENCY STOP! Robot stopped and disabled");
|
||||
}
|
||||
catch (AggregateException ex)
|
||||
{
|
||||
Logger.Error($"Emergency stop error: {ex.InnerException?.Message ?? ex.Message}");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Logger.Error($"Emergency stop error: {ex.Message}");
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,381 @@
|
||||
# H??ng d?n s? d?ng RobotStateMachineExecute
|
||||
|
||||
## Gi?i thi?u
|
||||
|
||||
`RobotStateMachineExecute` là class ch?a t?t c? các implementation methods cho Entry/Exit actions c?a State Machine. Class này ???c inject vào `RobotStateMachine` và t? ??ng ???c g?i khi state transitions x?y ra.
|
||||
|
||||
## Ki?n trúc
|
||||
|
||||
```
|
||||
RobotStateMachine (Qu?n lý state transitions)
|
||||
? s? d?ng
|
||||
RobotStateMachineExecute (Implementation c?a Entry/Exit actions)
|
||||
? s? d?ng
|
||||
IServiceProvider (?? inject các services khác khi c?n)
|
||||
```
|
||||
|
||||
## Danh sách các Entry/Exit Methods ?ã tri?n khai
|
||||
|
||||
### 1. System States
|
||||
|
||||
| State | Entry Method | Exit Method | Mô t? |
|
||||
|-------|-------------|-------------|-------|
|
||||
| **Initializing** | `EntryInitializing()` / `EntryInitializingAsync()` | - | Kh?i t?o các component c?a robot |
|
||||
| **Standby** | `EntryStandby()` | - | Robot s?n sàng ho?t ??ng |
|
||||
| **Shutting_Down** | `EntryShuttingDown()` / `EntryShuttingDownAsync()` | `ExitShuttingDown()` | T?t máy an toàn |
|
||||
|
||||
### 2. Auto Mode States
|
||||
|
||||
| State | Entry Method | Exit Method | Mô t? |
|
||||
|-------|-------------|-------------|-------|
|
||||
| **Idle** | `EntryIdle()` | - | S?n sàng nh?n nhi?m v? m?i |
|
||||
| **Executing** | `EntryExecuting()` | `ExitExecuting()` | ?ang th?c hi?n nhi?m v? |
|
||||
| **Paused** | `EntryPaused()` | `ExitPaused()` | T?m d?ng nhi?m v? |
|
||||
| **Canceling** | `EntryCanceling()` | `ExitCanceling()` | ?ang h?y nhi?m v? |
|
||||
| **Recovering** | `EntryRecovering()` / `EntryRecoveringAsync()` | - | Khôi ph?c t? l?i |
|
||||
| **Remote_Override** | `EntryRemoteOverride()` | `ExitRemoteOverride()` | ?i?u khi?n t? xa |
|
||||
|
||||
### 3. Moving Sub-States
|
||||
|
||||
| State | Entry Method | Exit Method | Mô t? |
|
||||
|-------|-------------|-------------|-------|
|
||||
| **Moving** | `EntryMoving()` | `ExitMoving()` | B?t ??u di chuy?n |
|
||||
| **Navigation** | `EntryNavigation()` | - | Navigation bình th??ng |
|
||||
| **Avoidance** | `EntryAvoidance()` | `ExitAvoidance()` | Tránh ch??ng ng?i v?t |
|
||||
| **Approach** | `EntryApproach()` | - | Ti?p c?n m?c tiêu |
|
||||
| **Tracking** | `EntryTracking()` | - | Theo dõi m?c tiêu ??ng |
|
||||
| **Repositioning** | `EntryRepositioning()` | - | ?i?u ch?nh v? trí |
|
||||
|
||||
### 4. ACT Sub-States
|
||||
|
||||
| State | Entry Method | Exit Method | Mô t? |
|
||||
|-------|-------------|-------------|-------|
|
||||
| **ACT** | `EntryACT()` | `ExitACT()` | B?t ??u action |
|
||||
| **Docking** | `EntryDocking()` / `EntryDockingAsync()` | - | ?ang docking |
|
||||
| **Docked** | `EntryDocked()` | - | ?ã docked |
|
||||
| **Charging** | `EntryCharging()` / `EntryChargingAsync()` | `ExitCharging()` | ?ang s?c pin |
|
||||
| **Undocking** | `EntryUndocking()` / `EntryUndockingAsync()` | - | ?ang undocking |
|
||||
| **Loading** | `EntryLoading()` / `EntryLoadingAsync()` | - | ?ang t?i hàng |
|
||||
| **Unloading** | `EntryUnloading()` / `EntryUnloadingAsync()` | - | ?ang d? hàng |
|
||||
| **TechAction** | `EntryTechAction()` / `EntryTechActionAsync()` | - | Action k? thu?t |
|
||||
|
||||
### 5. Mode States
|
||||
|
||||
| State | Entry Method | Exit Method | Mô t? |
|
||||
|-------|-------------|-------------|-------|
|
||||
| **Manual** | `EntryManual()` | `ExitManual()` | Ch? ?? th? công |
|
||||
| **Service** | `EntryService()` | `ExitService()` | Ch? ?? b?o trì |
|
||||
| **Stop** | `EntryStop()` | `ExitStop()` | Tr?ng thái d?ng |
|
||||
| **Fault** | `EntryFault()` | `ExitFault()` | Tr?ng thái l?i |
|
||||
|
||||
## Cách tùy ch?nh và m? r?ng
|
||||
|
||||
### 1. Inject thêm services
|
||||
|
||||
```csharp
|
||||
public class RobotStateMachineExecute
|
||||
{
|
||||
private readonly IServiceProvider _serviceProvider;
|
||||
private readonly Logger<RobotStateMachineExecute> _logger;
|
||||
|
||||
public void EntryDocking()
|
||||
{
|
||||
_logger.Info("==> Entry Docking State");
|
||||
|
||||
// L?y service t? IServiceProvider
|
||||
var driver = _serviceProvider.GetService<IDriver>();
|
||||
var sensors = _serviceProvider.GetService<ISensorService>();
|
||||
var dockingController = _serviceProvider.GetService<IDockingController>();
|
||||
|
||||
if (dockingController != null)
|
||||
{
|
||||
// Th?c hi?n docking
|
||||
dockingController.StartDocking();
|
||||
}
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
### 2. Thêm business logic c? th?
|
||||
|
||||
```csharp
|
||||
public void EntryCharging()
|
||||
{
|
||||
_logger.Info("==> Entry Charging State");
|
||||
|
||||
var batteryService = _serviceProvider.GetService<IBatteryService>();
|
||||
|
||||
if (batteryService != null)
|
||||
{
|
||||
// B?t ??u monitoring charging
|
||||
batteryService.StartCharging();
|
||||
|
||||
// Subscribe vào charging events
|
||||
batteryService.OnChargingProgress += (sender, progress) =>
|
||||
{
|
||||
_logger.Info($"Charging progress: {progress}%");
|
||||
};
|
||||
|
||||
batteryService.OnChargingCompleted += (sender, e) =>
|
||||
{
|
||||
_logger.Info("Charging completed!");
|
||||
// Có th? t? ??ng fire event ?? chuy?n state
|
||||
};
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
### 3. X? lý async operations
|
||||
|
||||
M?t s? methods ?ã ???c tri?n khai v?i c? sync và async versions:
|
||||
|
||||
```csharp
|
||||
// Async version - recommended
|
||||
public async Task EntryDockingAsync()
|
||||
{
|
||||
_logger.Info("==> Entry Docking State");
|
||||
|
||||
var dockingController = _serviceProvider.GetService<IDockingController>();
|
||||
|
||||
if (dockingController != null)
|
||||
{
|
||||
await dockingController.AlignWithDockAsync();
|
||||
await dockingController.ApproachDockAsync();
|
||||
await dockingController.ConnectToDockAsync();
|
||||
}
|
||||
}
|
||||
|
||||
// Sync version - wrapper
|
||||
public void EntryDocking() => EntryDockingAsync().GetAwaiter().GetResult();
|
||||
```
|
||||
|
||||
### 4. Thêm error handling
|
||||
|
||||
```csharp
|
||||
public void EntryExecuting()
|
||||
{
|
||||
_logger.Info("==> Entry Executing State");
|
||||
|
||||
try
|
||||
{
|
||||
var missionService = _serviceProvider.GetService<IMissionService>();
|
||||
|
||||
if (missionService != null)
|
||||
{
|
||||
var currentMission = missionService.GetCurrentMission();
|
||||
|
||||
if (currentMission == null)
|
||||
{
|
||||
_logger.Warning("No mission available");
|
||||
// Fire event to go back to Idle
|
||||
var stateMachine = _serviceProvider.GetService<RobotStateMachine>();
|
||||
stateMachine?.Fire(RobotEventType.CompleteExecution);
|
||||
return;
|
||||
}
|
||||
|
||||
// Start executing mission
|
||||
missionService.StartExecution(currentMission);
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.Error($"Error in EntryExecuting: {ex.Message}");
|
||||
|
||||
// Fire event to enter Fault state
|
||||
var stateMachine = _serviceProvider.GetService<RobotStateMachine>();
|
||||
stateMachine?.Fire(RobotEventType.EnterFault);
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
## Ví d? th?c t?: Lu?ng Docking & Charging
|
||||
|
||||
```csharp
|
||||
public async Task EntryDockingAsync()
|
||||
{
|
||||
_logger.Info("==> Entry Docking State");
|
||||
|
||||
try
|
||||
{
|
||||
var driver = _serviceProvider.GetService<IDriver>();
|
||||
var sensors = _serviceProvider.GetService<ISensorService>();
|
||||
var dockingService = _serviceProvider.GetService<IDockingService>();
|
||||
|
||||
if (dockingService == null)
|
||||
{
|
||||
_logger.Error("Docking service not available");
|
||||
throw new InvalidOperationException("Docking service not available");
|
||||
}
|
||||
|
||||
// 1. Tìm dock station
|
||||
_logger.Info("Searching for dock station...");
|
||||
var dockPosition = await dockingService.FindDockStationAsync();
|
||||
|
||||
if (dockPosition == null)
|
||||
{
|
||||
_logger.Error("Dock station not found");
|
||||
throw new InvalidOperationException("Dock station not found");
|
||||
}
|
||||
|
||||
// 2. Di chuy?n ??n v? trí dock
|
||||
_logger.Info("Moving to dock position...");
|
||||
await dockingService.MoveToDockPositionAsync(dockPosition);
|
||||
|
||||
// 3. Align v?i dock
|
||||
_logger.Info("Aligning with dock...");
|
||||
await dockingService.AlignWithDockAsync();
|
||||
|
||||
// 4. Di chuy?n vào dock ch?m rãi
|
||||
_logger.Info("Approaching dock...");
|
||||
await dockingService.SlowApproachAsync();
|
||||
|
||||
// 5. K?t n?i v?i dock
|
||||
_logger.Info("Connecting to dock...");
|
||||
var connected = await dockingService.ConnectToDockAsync();
|
||||
|
||||
if (!connected)
|
||||
{
|
||||
_logger.Error("Failed to connect to dock");
|
||||
throw new InvalidOperationException("Failed to connect to dock");
|
||||
}
|
||||
|
||||
_logger.Info("Docking completed successfully!");
|
||||
|
||||
// T? ??ng chuy?n sang state Docked
|
||||
var stateMachine = _serviceProvider.GetService<RobotStateMachine>();
|
||||
await stateMachine!.FireAsync(RobotEventType.CompleteDocking);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.Error($"Docking failed: {ex.Message}");
|
||||
|
||||
// Chuy?n sang fault state
|
||||
var stateMachine = _serviceProvider.GetService<RobotStateMachine>();
|
||||
await stateMachine!.FireAsync(RobotEventType.EnterFault);
|
||||
}
|
||||
}
|
||||
|
||||
public async Task EntryChargingAsync()
|
||||
{
|
||||
_logger.Info("==> Entry Charging State");
|
||||
|
||||
try
|
||||
{
|
||||
var batteryService = _serviceProvider.GetService<IBatteryService>();
|
||||
var dockingService = _serviceProvider.GetService<IDockingService>();
|
||||
|
||||
if (batteryService == null || dockingService == null)
|
||||
{
|
||||
_logger.Error("Required services not available");
|
||||
return;
|
||||
}
|
||||
|
||||
// 1. Ki?m tra k?t n?i dock
|
||||
if (!dockingService.IsConnectedToDock())
|
||||
{
|
||||
_logger.Error("Not connected to dock");
|
||||
return;
|
||||
}
|
||||
|
||||
// 2. B?t ??u s?c
|
||||
_logger.Info("Starting charging...");
|
||||
await batteryService.StartChargingAsync();
|
||||
|
||||
// 3. Monitor charging progress
|
||||
batteryService.OnChargingProgress += (sender, progress) =>
|
||||
{
|
||||
_logger.Info($"Battery level: {progress.CurrentLevel}% (Target: {progress.TargetLevel}%)");
|
||||
|
||||
// N?u ?ã ??t target level, hoàn thành charging
|
||||
if (progress.CurrentLevel >= progress.TargetLevel)
|
||||
{
|
||||
_logger.Info("Target battery level reached!");
|
||||
|
||||
var stateMachine = _serviceProvider.GetService<RobotStateMachine>();
|
||||
stateMachine?.Fire(RobotEventType.CompleteCharging);
|
||||
}
|
||||
};
|
||||
|
||||
// 4. Subscribe charging error events
|
||||
batteryService.OnChargingError += (sender, error) =>
|
||||
{
|
||||
_logger.Error($"Charging error: {error.Message}");
|
||||
|
||||
var stateMachine = _serviceProvider.GetService<RobotStateMachine>();
|
||||
stateMachine?.Fire(RobotEventType.EnterFault);
|
||||
};
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.Error($"Failed to start charging: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
public void ExitCharging()
|
||||
{
|
||||
_logger.Info("<== Exit Charging State");
|
||||
|
||||
var batteryService = _serviceProvider.GetService<IBatteryService>();
|
||||
|
||||
if (batteryService != null)
|
||||
{
|
||||
// D?ng charging
|
||||
batteryService.StopCharging();
|
||||
|
||||
// Unsubscribe events
|
||||
batteryService.OnChargingProgress = null;
|
||||
batteryService.OnChargingError = null;
|
||||
|
||||
// Log final battery level
|
||||
var batteryLevel = batteryService.GetCurrentLevel();
|
||||
_logger.Info($"Charging stopped. Final battery level: {batteryLevel}%");
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
## Best Practices
|
||||
|
||||
1. **Luôn log chi ti?t** trong m?i Entry/Exit method ?? d? debug
|
||||
2. **S? d?ng try-catch** ?? x? lý l?i và chuy?n sang Fault state n?u c?n
|
||||
3. **S? d?ng async/await** cho các operations t?n th?i gian
|
||||
4. **Cleanup resources** trong Exit methods
|
||||
5. **Inject services qua IServiceProvider** thay vì hard-code dependencies
|
||||
6. **Fire events** ?? t? ??ng chuy?n state khi hoàn thành action
|
||||
7. **Validate preconditions** tr??c khi th?c hi?n action
|
||||
8. **Update VDA5050 state** ?? ??ng b? v?i Fleet Manager
|
||||
|
||||
## Tích h?p v?i VDA5050
|
||||
|
||||
```csharp
|
||||
public void EntryExecuting()
|
||||
{
|
||||
_logger.Info("==> Entry Executing State");
|
||||
|
||||
var vda5050Service = _serviceProvider.GetService<IVDA5050Service>();
|
||||
|
||||
if (vda5050Service != null)
|
||||
{
|
||||
// C?p nh?t VDA5050 state
|
||||
vda5050Service.UpdateState(new StateMessage
|
||||
{
|
||||
OperatingMode = "AUTOMATIC",
|
||||
ActionStates = new List<ActionState>
|
||||
{
|
||||
new() { ActionStatus = "RUNNING" }
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
// ... rest of implementation
|
||||
}
|
||||
```
|
||||
|
||||
## K?t lu?n
|
||||
|
||||
`RobotStateMachineExecute` cung c?p m?t cách t? ch?c code rõ ràng và d? maintain cho t?t c? các state behaviors c?a robot. B?n có th? d? dàng:
|
||||
|
||||
- Thêm logic m?i vào các state
|
||||
- Inject và s? d?ng các services khác
|
||||
- X? lý async operations
|
||||
- Error handling và recovery
|
||||
- Testing t?ng state behavior ??c l?p
|
||||
@@ -0,0 +1,248 @@
|
||||
# H??ng d?n s? d?ng RobotStateMachine
|
||||
|
||||
## Gi?i thi?u
|
||||
|
||||
`RobotStateMachine` là m?t singleton service qu?n lý tr?ng thái c?a mobile robot s? d?ng Hierarchical State Machine pattern v?i th? vi?n Appccelerate.StateMachine.
|
||||
|
||||
## C?u trúc Hierarchical State Machine
|
||||
|
||||
### Root States (C?p 1)
|
||||
- **System**: Tr?ng thái h? th?ng
|
||||
- Initializing: ?ang kh?i t?o
|
||||
- Standby: Ch? s?n sàng
|
||||
- Shutting_Down: ?ang t?t
|
||||
|
||||
- **Auto**: Ch? ?? t? ??ng
|
||||
- Idle: R?nh r?i, s?n sàng nh?n l?nh
|
||||
- Executing: ?ang th?c hi?n nhi?m v?
|
||||
- Moving: ?ang di chuy?n
|
||||
- Navigation: ?i?u h??ng
|
||||
- Avoidance: Tránh ch??ng ng?i v?t
|
||||
- Approach: Ti?p c?n m?c tiêu
|
||||
- Tracking: Theo dõi m?c tiêu
|
||||
- Repositioning: ?i?u ch?nh v? trí
|
||||
- ACT: Th?c hi?n action
|
||||
- Docking: ?ang dock
|
||||
- Docked: ?ã dock
|
||||
- Charging: ?ang s?c
|
||||
- Undocking: ?ang undock
|
||||
- Loading: ?ang t?i hàng
|
||||
- Unloading: ?ang d? hàng
|
||||
- TechAction: Action k? thu?t
|
||||
- Paused: T?m d?ng
|
||||
- Canceling: ?ang h?y
|
||||
- Recovering: ?ang khôi ph?c
|
||||
- Remote_Override: ?i?u khi?n t? xa
|
||||
|
||||
- **Manual**: Ch? ?? th? công
|
||||
- **Service**: Ch? ?? b?o trì
|
||||
- **Stop**: Tr?ng thái d?ng
|
||||
- **Fault**: Tr?ng thái l?i
|
||||
|
||||
## Cách s? d?ng
|
||||
|
||||
### 1. ??ng ký Service (trong Program.cs)
|
||||
|
||||
```csharp
|
||||
// ??ng ký nh? Singleton
|
||||
builder.Services.AddSingleton<RobotStateMachine>();
|
||||
```
|
||||
|
||||
### 2. Kh?i t?o State Machine
|
||||
|
||||
```csharp
|
||||
public class RobotController
|
||||
{
|
||||
private readonly RobotStateMachine _stateMachine;
|
||||
|
||||
public RobotController(RobotStateMachine stateMachine)
|
||||
{
|
||||
_stateMachine = stateMachine;
|
||||
|
||||
// Kh?i t?o state machine
|
||||
_stateMachine.Initialize();
|
||||
|
||||
// Ho?c s? d?ng async
|
||||
await _stateMachine.InitializeAsync();
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
### 3. Subscribe vào State Change Events
|
||||
|
||||
```csharp
|
||||
_stateMachine.StateChanged += (sender, e) =>
|
||||
{
|
||||
Console.WriteLine($"State changed to: {e.NewState} via event: {e.EventType}");
|
||||
};
|
||||
```
|
||||
|
||||
### 4. Chuy?n ??i tr?ng thái
|
||||
|
||||
```csharp
|
||||
// Hoàn thành kh?i t?o -> chuy?n sang Standby
|
||||
await _stateMachine.FireAsync(RobotEventType.InitializeCompleted);
|
||||
|
||||
// Chuy?n sang ch? ?? Auto
|
||||
await _stateMachine.FireAsync(RobotEventType.EnterAuto);
|
||||
|
||||
// B?t ??u th?c hi?n nhi?m v?
|
||||
await _stateMachine.FireAsync(RobotEventType.StartExecution);
|
||||
|
||||
// Pause nhi?m v?
|
||||
await _stateMachine.FireAsync(RobotEventType.PauseExecution);
|
||||
|
||||
// Resume nhi?m v?
|
||||
await _stateMachine.FireAsync(RobotEventType.ResumeExecution);
|
||||
|
||||
// Hoàn thành nhi?m v?
|
||||
await _stateMachine.FireAsync(RobotEventType.CompleteExecution);
|
||||
```
|
||||
|
||||
### 5. Ki?m tra tr?ng thái hi?n t?i
|
||||
|
||||
```csharp
|
||||
var currentState = _stateMachine.CurrentState;
|
||||
var stateInfo = _stateMachine.GetStateInfo();
|
||||
var isInitialized = _stateMachine.IsInitialized;
|
||||
```
|
||||
|
||||
## Ví d? lu?ng ho?t ??ng
|
||||
|
||||
### Lu?ng kh?i ??ng robot
|
||||
|
||||
```csharp
|
||||
// 1. Kh?i t?o state machine
|
||||
await _stateMachine.InitializeAsync();
|
||||
// State: System -> Initializing
|
||||
|
||||
// 2. Hoàn thành kh?i t?o
|
||||
await _stateMachine.FireAsync(RobotEventType.InitializeCompleted);
|
||||
// State: System -> Standby
|
||||
|
||||
// 3. Chuy?n sang ch? ?? Auto
|
||||
await _stateMachine.FireAsync(RobotEventType.EnterAuto);
|
||||
// State: Auto -> Idle
|
||||
```
|
||||
|
||||
### Lu?ng th?c hi?n nhi?m v? di chuy?n
|
||||
|
||||
```csharp
|
||||
// 1. B?t ??u th?c hi?n
|
||||
await _stateMachine.FireAsync(RobotEventType.StartExecution);
|
||||
// State: Auto -> Executing -> Moving -> Navigation
|
||||
|
||||
// 2. Phát hi?n ch??ng ng?i v?t
|
||||
await _stateMachine.FireAsync(RobotEventType.StartAvoidance);
|
||||
// State: Auto -> Executing -> Moving -> Avoidance
|
||||
|
||||
// 3. Ti?p t?c navigation
|
||||
await _stateMachine.FireAsync(RobotEventType.StartNavigation);
|
||||
// State: Auto -> Executing -> Moving -> Navigation
|
||||
|
||||
// 4. Hoàn thành di chuy?n
|
||||
await _stateMachine.FireAsync(RobotEventType.CompleteMoving);
|
||||
// State: Auto -> Idle
|
||||
```
|
||||
|
||||
### Lu?ng th?c hi?n action (Docking & Charging)
|
||||
|
||||
```csharp
|
||||
// 1. B?t ??u th?c hi?n
|
||||
await _stateMachine.FireAsync(RobotEventType.StartExecution);
|
||||
// State: Auto -> Executing -> Moving
|
||||
|
||||
// 2. Chuy?n sang ACT
|
||||
await _stateMachine.FireAsync(RobotEventType.StartACT);
|
||||
// State: Auto -> Executing -> ACT -> Docking
|
||||
|
||||
// 3. Hoàn thành docking
|
||||
await _stateMachine.FireAsync(RobotEventType.CompleteDocking);
|
||||
// State: Auto -> Executing -> ACT -> Docked
|
||||
|
||||
// 4. B?t ??u s?c
|
||||
await _stateMachine.FireAsync(RobotEventType.StartCharging);
|
||||
// State: Auto -> Executing -> ACT -> Charging
|
||||
|
||||
// 5. Hoàn thành s?c
|
||||
await _stateMachine.FireAsync(RobotEventType.CompleteCharging);
|
||||
// State: Auto -> Executing -> ACT -> Docked
|
||||
|
||||
// 6. Undocking
|
||||
await _stateMachine.FireAsync(RobotEventType.StartUndocking);
|
||||
// State: Auto -> Executing -> ACT -> Undocking
|
||||
|
||||
// 7. Hoàn thành undocking
|
||||
await _stateMachine.FireAsync(RobotEventType.CompleteUndocking);
|
||||
// State: Auto -> Executing -> ACT -> Docking
|
||||
|
||||
// 8. Hoàn thành ACT
|
||||
await _stateMachine.FireAsync(RobotEventType.CompleteACT);
|
||||
// State: Auto -> Idle
|
||||
```
|
||||
|
||||
### X? lý Emergency Stop
|
||||
|
||||
```csharp
|
||||
// Khi ?ang th?c hi?n nhi?m v?
|
||||
await _stateMachine.FireAsync(RobotEventType.EnterStop);
|
||||
// State: Stop
|
||||
|
||||
// Gi?i phóng stop
|
||||
await _stateMachine.FireAsync(RobotEventType.ReleaseStop);
|
||||
// State: Auto (quay l?i tr?ng thái tr??c ?ó nh? History)
|
||||
```
|
||||
|
||||
## L?u ý quan tr?ng
|
||||
|
||||
1. **History Type**:
|
||||
- System hierarchy s? d?ng `HistoryType.None` - không l?u tr?ng thái con
|
||||
- Auto hierarchy s? d?ng `HistoryType.Deep` - l?u toàn b? tr?ng thái con khi quay l?i
|
||||
|
||||
2. **Thread Safety**: State machine là thread-safe, có th? g?i t? nhi?u thread
|
||||
|
||||
3. **Exception Handling**: T?t c? các l?i khi fire event ??u ???c log và không throw exception
|
||||
|
||||
4. **Async Support**: Nên s? d?ng các ph??ng th?c Async ?? tránh blocking thread
|
||||
|
||||
5. **Singleton Pattern**: Service này nên ???c ??ng ký nh? Singleton ?? ??m b?o ch? có m?t instance duy nh?t
|
||||
|
||||
## Tích h?p v?i VDA5050
|
||||
|
||||
```csharp
|
||||
public class VDA5050Handler
|
||||
{
|
||||
private readonly RobotStateMachine _stateMachine;
|
||||
|
||||
public void HandleOrder(Order order)
|
||||
{
|
||||
// Khi nh?n order m?i
|
||||
await _stateMachine.FireAsync(RobotEventType.StartExecution);
|
||||
}
|
||||
|
||||
public void HandleInstantAction(InstantAction action)
|
||||
{
|
||||
switch (action.ActionType)
|
||||
{
|
||||
case "cancelOrder":
|
||||
await _stateMachine.FireAsync(RobotEventType.CancelExecution);
|
||||
break;
|
||||
case "pause":
|
||||
await _stateMachine.FireAsync(RobotEventType.PauseExecution);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
public string GetOperatingMode()
|
||||
{
|
||||
// Map state machine state to VDA5050 operatingMode
|
||||
return _stateMachine.CurrentState switch
|
||||
{
|
||||
RobotStateType.Auto => "AUTOMATIC",
|
||||
RobotStateType.Manual => "MANUAL",
|
||||
RobotStateType.Service => "SERVICE",
|
||||
_ => "SEMIAUTOMATIC"
|
||||
};
|
||||
}
|
||||
}
|
||||
```
|
||||
@@ -0,0 +1,38 @@
|
||||
namespace RobotNet10.RobotApp.Services.State;
|
||||
|
||||
public enum RobotStateType
|
||||
{
|
||||
// Root
|
||||
System,
|
||||
Auto,
|
||||
Manual,
|
||||
Service,
|
||||
Remote_Override,
|
||||
Stop,
|
||||
Fault,
|
||||
|
||||
// System
|
||||
Initializing,
|
||||
Standby,
|
||||
Shutting_Down,
|
||||
|
||||
// Auto
|
||||
Idle,
|
||||
Executing,
|
||||
Paused,
|
||||
Canceling,
|
||||
Recovering,
|
||||
|
||||
// Executing
|
||||
Moving,
|
||||
ACT,
|
||||
|
||||
// ACT
|
||||
Docking,
|
||||
Docked,
|
||||
Charging,
|
||||
Undocking,
|
||||
Loading,
|
||||
Unloading,
|
||||
TechAction,
|
||||
}
|
||||
@@ -0,0 +1,323 @@
|
||||
# State Inheritance trong RobotStateMachine
|
||||
|
||||
## Tóm t?t
|
||||
|
||||
? **SUB-STATES T? ??NG K? TH?A CÁC EVENTS T? PARENT STATE** trong Hierarchical State Machine
|
||||
|
||||
## Ví d? chi ti?t
|
||||
|
||||
### Hierarchy trong RobotStateMachine
|
||||
|
||||
```
|
||||
System (Root)
|
||||
??? Initializing
|
||||
??? Standby
|
||||
??? Shutting_Down
|
||||
|
||||
Auto (Root)
|
||||
??? Idle
|
||||
??? Executing
|
||||
? ??? Moving
|
||||
? ? ??? Navigation
|
||||
? ? ??? Avoidance
|
||||
? ? ??? Approach
|
||||
? ? ??? Tracking
|
||||
? ? ??? Repositioning
|
||||
? ??? ACT
|
||||
? ??? Docking
|
||||
? ??? Docked
|
||||
? ??? Charging
|
||||
? ??? Undocking
|
||||
? ??? Loading
|
||||
? ??? Unloading
|
||||
? ??? TechAction
|
||||
??? Paused
|
||||
??? Canceling
|
||||
??? Recovering
|
||||
??? Remote_Override
|
||||
```
|
||||
|
||||
## 1. Inheritance c? b?n
|
||||
|
||||
### Ví d? 1: Idle k? th?a t? Auto
|
||||
|
||||
```csharp
|
||||
// Auto (parent) ??nh ngh?a các events
|
||||
builder.In(RobotStateType.Auto)
|
||||
.On(RobotEventType.EnterManual).Goto(RobotStateType.Manual)
|
||||
.On(RobotEventType.EnterService).Goto(RobotStateType.Service)
|
||||
.On(RobotEventType.EnterStop).Goto(RobotStateType.Stop)
|
||||
.On(RobotEventType.EnterFault).Goto(RobotStateType.Fault);
|
||||
|
||||
// Idle (sub-state) ch? ??nh ngh?a event riêng c?a nó
|
||||
builder.In(RobotStateType.Idle)
|
||||
.On(RobotEventType.StartExecution).Goto(RobotStateType.Executing);
|
||||
```
|
||||
|
||||
**K?t qu? khi ? state Idle:**
|
||||
|
||||
```csharp
|
||||
var stateMachine = serviceProvider.GetService<RobotStateMachine>();
|
||||
stateMachine.Initialize();
|
||||
|
||||
// Gi? s? ?ang ? Idle
|
||||
await stateMachine.FireAsync(RobotEventType.EnterAuto);
|
||||
|
||||
// Query transitions
|
||||
var transitions = stateMachine.GetPossibleTransitions(RobotStateType.Idle);
|
||||
|
||||
// K?t qu?:
|
||||
// 1. StartExecution -> Executing (??nh ngh?a riêng)
|
||||
// 2. EnterManual -> Manual (k? th?a t? Auto)
|
||||
// 3. EnterService -> Service (k? th?a t? Auto)
|
||||
// 4. EnterStop -> Stop (k? th?a t? Auto)
|
||||
// 5. EnterFault -> Fault (k? th?a t? Auto)
|
||||
|
||||
Console.WriteLine($"Idle has {transitions.Count} possible transitions");
|
||||
foreach (var t in transitions)
|
||||
{
|
||||
Console.WriteLine($" - {t.Event} -> {t.ToState}");
|
||||
}
|
||||
```
|
||||
|
||||
**Output:**
|
||||
```
|
||||
Idle has 5 possible transitions
|
||||
- StartExecution -> Executing
|
||||
- EnterManual -> Manual
|
||||
- EnterService -> Service
|
||||
- EnterStop -> Stop
|
||||
- EnterFault -> Fault
|
||||
```
|
||||
|
||||
## 2. Multi-level Inheritance
|
||||
|
||||
### Ví d? 2: Navigation k? th?a t? Moving và Executing
|
||||
|
||||
```csharp
|
||||
// Executing (c?p 1) ??nh ngh?a
|
||||
builder.In(RobotStateType.Executing)
|
||||
.On(RobotEventType.PauseExecution).Goto(RobotStateType.Paused)
|
||||
.On(RobotEventType.CancelExecution).Goto(RobotStateType.Canceling);
|
||||
|
||||
// Moving (c?p 2 - sub c?a Executing) ??nh ngh?a
|
||||
builder.In(RobotStateType.Moving)
|
||||
.On(RobotEventType.StartACT).Goto(RobotStateType.ACT);
|
||||
|
||||
// Navigation (c?p 3 - sub c?a Moving) ??nh ngh?a
|
||||
builder.In(RobotStateType.Navigation)
|
||||
.On(RobotEventType.StartAvoidance).Goto(RobotStateType.Avoidance);
|
||||
```
|
||||
|
||||
**K?t qu? khi ? Navigation:**
|
||||
|
||||
```csharp
|
||||
var transitions = stateMachine.GetPossibleTransitions(RobotStateType.Navigation);
|
||||
|
||||
// K?t qu? (k? th?a 3 c?p):
|
||||
// 1. StartAvoidance -> Avoidance (??nh ngh?a ? Navigation)
|
||||
// 2. StartACT -> ACT (k? th?a t? Moving)
|
||||
// 3. PauseExecution -> Paused (k? th?a t? Executing)
|
||||
// 4. CancelExecution -> Canceling (k? th?a t? Executing)
|
||||
// 5. EnterManual -> Manual (k? th?a t? Auto - parent c?a Executing)
|
||||
// 6. EnterService -> Service (k? th?a t? Auto)
|
||||
// 7. EnterStop -> Stop (k? th?a t? Auto)
|
||||
// 8. EnterFault -> Fault (k? th?a t? Auto)
|
||||
```
|
||||
|
||||
## 3. S? d?ng các method m?i
|
||||
|
||||
### Query Hierarchy
|
||||
|
||||
```csharp
|
||||
// L?y parent state
|
||||
var parent = stateMachine.GetParentState(RobotStateType.Navigation);
|
||||
Console.WriteLine($"Parent of Navigation: {parent}"); // Moving
|
||||
|
||||
// L?y toàn b? hierarchy chain
|
||||
var chain = stateMachine.GetStateHierarchyChain(RobotStateType.Navigation);
|
||||
Console.WriteLine("Hierarchy chain:");
|
||||
foreach (var state in chain)
|
||||
{
|
||||
Console.WriteLine($" - {state}");
|
||||
}
|
||||
// Output:
|
||||
// - Navigation
|
||||
// - Moving
|
||||
// - Executing
|
||||
// - Auto
|
||||
|
||||
// Ki?m tra sub-state relationship
|
||||
bool isSubState = stateMachine.IsSubStateOf(
|
||||
RobotStateType.Navigation,
|
||||
RobotStateType.Auto
|
||||
);
|
||||
Console.WriteLine($"Navigation is sub-state of Auto: {isSubState}"); // true
|
||||
|
||||
isSubState = stateMachine.IsSubStateOf(
|
||||
RobotStateType.Navigation,
|
||||
RobotStateType.Manual
|
||||
);
|
||||
Console.WriteLine($"Navigation is sub-state of Manual: {isSubState}"); // false
|
||||
```
|
||||
|
||||
### Check transitions v?i inheritance
|
||||
|
||||
```csharp
|
||||
// Khi ? Navigation state
|
||||
await stateMachine.FireAsync(RobotEventType.EnterAuto);
|
||||
await stateMachine.FireAsync(RobotEventType.StartExecution);
|
||||
|
||||
// Bây gi? ?ang ? Navigation (sub-state c?a Moving, Executing, Auto)
|
||||
|
||||
// Ki?m tra có th? fire các events k? th?a
|
||||
Console.WriteLine($"Can fire StartAvoidance: {stateMachine.CanFire(RobotEventType.StartAvoidance)}"); // true (riêng)
|
||||
Console.WriteLine($"Can fire StartACT: {stateMachine.CanFire(RobotEventType.StartACT)}"); // true (t? Moving)
|
||||
Console.WriteLine($"Can fire PauseExecution: {stateMachine.CanFire(RobotEventType.PauseExecution)}"); // true (t? Executing)
|
||||
Console.WriteLine($"Can fire EnterManual: {stateMachine.CanFire(RobotEventType.EnterManual)}"); // true (t? Auto)
|
||||
|
||||
// Ki?m tra có th? chuy?n sang các states
|
||||
Console.WriteLine($"Can go to Avoidance: {stateMachine.CanTransitionTo(RobotStateType.Avoidance)}"); // true
|
||||
Console.WriteLine($"Can go to ACT: {stateMachine.CanTransitionTo(RobotStateType.ACT)}"); // true
|
||||
Console.WriteLine($"Can go to Paused: {stateMachine.CanTransitionTo(RobotStateType.Paused)}"); // true
|
||||
Console.WriteLine($"Can go to Manual: {stateMachine.CanTransitionTo(RobotStateType.Manual)}"); // true
|
||||
|
||||
// L?y event ?? chuy?n sang state ?ích
|
||||
var evt = stateMachine.GetEventForTransition(RobotStateType.Manual);
|
||||
Console.WriteLine($"Event to go to Manual: {evt}"); // EnterManual
|
||||
```
|
||||
|
||||
## 4. Practical Use Cases
|
||||
|
||||
### Use Case 1: Emergency Stop t? b?t k? sub-state nào
|
||||
|
||||
```csharp
|
||||
public class SafetyController
|
||||
{
|
||||
private readonly RobotStateMachine _stateMachine;
|
||||
|
||||
public async Task EmergencyStopAsync()
|
||||
{
|
||||
// Có th? g?i t? B?T K? sub-state nào c?a Auto
|
||||
// vì EnterStop ???c ??nh ngh?a ? Auto (parent)
|
||||
|
||||
if (_stateMachine.CanFire(RobotEventType.EnterStop))
|
||||
{
|
||||
await _stateMachine.FireAsync(RobotEventType.EnterStop);
|
||||
Console.WriteLine("Emergency stop activated!");
|
||||
}
|
||||
else
|
||||
{
|
||||
Console.WriteLine("Cannot emergency stop from current state");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// G?i t? Navigation (deep sub-state)
|
||||
// ? V?n ho?t ??ng vì Navigation k? th?a EnterStop t? Auto
|
||||
```
|
||||
|
||||
### Use Case 2: Fault Handling t? m?i n?i
|
||||
|
||||
```csharp
|
||||
public class ErrorHandler
|
||||
{
|
||||
private readonly RobotStateMachine _stateMachine;
|
||||
|
||||
public async Task HandleErrorAsync(Exception error)
|
||||
{
|
||||
// EnterFault ???c ??nh ngh?a ? Auto
|
||||
// Có th? g?i t? m?i sub-state c?a Auto
|
||||
|
||||
Console.WriteLine($"Error occurred: {error.Message}");
|
||||
|
||||
if (_stateMachine.IsSubStateOf(_stateMachine.CurrentState, RobotStateType.Auto))
|
||||
{
|
||||
// ?ang ? trong Auto hierarchy, có th? fire EnterFault
|
||||
await _stateMachine.FireAsync(RobotEventType.EnterFault);
|
||||
}
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
### Use Case 3: Dynamic UI buttons d?a trên available transitions
|
||||
|
||||
```razor
|
||||
@* Blazor Component *@
|
||||
@inject RobotStateMachine StateMachine
|
||||
|
||||
<div class="control-panel">
|
||||
<h3>Current State: @StateMachine.CurrentState</h3>
|
||||
|
||||
<h4>Available Actions:</h4>
|
||||
@foreach (var transition in StateMachine.GetPossibleTransitions())
|
||||
{
|
||||
<button @onclick="() => FireEvent(transition.Event)">
|
||||
@transition.Event.ToString() ? @transition.ToState
|
||||
</button>
|
||||
}
|
||||
</div>
|
||||
|
||||
@code {
|
||||
private async Task FireEvent(RobotEventType eventType)
|
||||
{
|
||||
await StateMachine.FireAsync(eventType);
|
||||
StateHasChanged();
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
**Khi ? Navigation state, UI s? hi?n th? T?T C? các buttons bao g?m:**
|
||||
- StartAvoidance (riêng c?a Navigation)
|
||||
- StartACT (k? th?a t? Moving)
|
||||
- PauseExecution (k? th?a t? Executing)
|
||||
- CancelExecution (k? th?a t? Executing)
|
||||
- EnterManual (k? th?a t? Auto)
|
||||
- EnterService (k? th?a t? Auto)
|
||||
- EnterStop (k? th?a t? Auto)
|
||||
- EnterFault (k? th?a t? Auto)
|
||||
|
||||
## 5. Best Practices
|
||||
|
||||
### ? DO: ??nh ngh?a common events ? parent state
|
||||
|
||||
```csharp
|
||||
// Good: EnterFault ? Auto, t?t c? sub-states ??u có th? fault
|
||||
builder.In(RobotStateType.Auto)
|
||||
.On(RobotEventType.EnterFault).Goto(RobotStateType.Fault);
|
||||
```
|
||||
|
||||
### ? DON'T: L?p l?i events ? m?i sub-state
|
||||
|
||||
```csharp
|
||||
// Bad: Không c?n thi?t
|
||||
builder.In(RobotStateType.Idle)
|
||||
.On(RobotEventType.EnterFault).Goto(RobotStateType.Fault);
|
||||
|
||||
builder.In(RobotStateType.Executing)
|
||||
.On(RobotEventType.EnterFault).Goto(RobotStateType.Fault);
|
||||
|
||||
builder.In(RobotStateType.Paused)
|
||||
.On(RobotEventType.EnterFault).Goto(RobotStateType.Fault);
|
||||
// ... l?p l?i cho t?t c? sub-states
|
||||
```
|
||||
|
||||
### ? DO: Ki?m tra inherited transitions tr??c khi hi?n th? UI
|
||||
|
||||
```csharp
|
||||
public List<RobotEventType> GetAvailableActionsForCurrentState()
|
||||
{
|
||||
var transitions = _stateMachine.GetPossibleTransitions();
|
||||
return transitions.Select(t => t.Event).ToList();
|
||||
}
|
||||
```
|
||||
|
||||
## T?ng k?t
|
||||
|
||||
1. ? **Sub-states T? ??NG k? th?a** t?t c? events t? parent states
|
||||
2. ? **Multi-level inheritance** ho?t ??ng (Navigation k? th?a t? Moving ? Executing ? Auto)
|
||||
3. ? **GetPossibleTransitions()** ?ã ???c c?p nh?t ?? tr? v? c? inherited transitions
|
||||
4. ? **CanFire()**, **CanTransitionTo()**, **GetEventForTransition()** ??u h? tr? inheritance
|
||||
5. ? **Helper methods m?i**: GetParentState(), GetStateHierarchyChain(), IsSubStateOf()
|
||||
|
||||
?i?u này giúp code c?a b?n **DRY** (Don't Repeat Yourself) và d? maintain h?n!
|
||||
Reference in New Issue
Block a user