Initial commit
This commit is contained in:
@@ -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}");
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user