111 lines
4.0 KiB
C#
111 lines
4.0 KiB
C#
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);
|
|
}
|
|
}
|