732 lines
28 KiB
C#
732 lines
28 KiB
C#
using RobotNet.VDA5050.Type;
|
|
using RobotNet10.Common;
|
|
using RobotNet10.RobotApp.Devices;
|
|
using RobotNet10.RobotApp.Events;
|
|
using RobotNet10.RobotApp.Events.Events;
|
|
using RobotNet10.RobotApp.Interfaces;
|
|
using RobotNet10.RobotApp.Modules;
|
|
using RobotNet10.RobotApp.Motion;
|
|
using RobotNet10.RobotApp.Services.ConfigManager;
|
|
using RobotNet10.RobotApp.Services.Exceptions;
|
|
using RobotNet10.RobotApp.Services.Robot.Connection;
|
|
using RobotNet10.RobotApp.Services.State;
|
|
|
|
namespace RobotNet10.RobotApp.Services.Robot;
|
|
|
|
public partial class RobotController(IOrder OrderManager,
|
|
INavigation NavigationManager,
|
|
IAction ActionManager,
|
|
IPlcController PlcController,
|
|
IDeviceProvider DeviceProvider,
|
|
IConfiguration Configuration,
|
|
IError ErrorManager,
|
|
Logger<RobotController> Logger,
|
|
IRobotConnectionsService RobotConnectionsService,
|
|
IRobotEventBus RobotEventBus,
|
|
RobotStateMachine StateManager,
|
|
ManualControlService RFControl,
|
|
PS5ControllerService Ps5Controller,
|
|
IRobotConfiguration RobotConfiguration,
|
|
RobotStates StateService,
|
|
RobotVisualization VisualizationService,
|
|
ILiftModule LiftModule,
|
|
ILocalization Localization,
|
|
IRotationModule RotateModule,
|
|
IInverseKinematics? InverseKinematics = null) : BackgroundService, IRobotController
|
|
{
|
|
private readonly Mutex NewOrderMutex = new();
|
|
private readonly Mutex NewInstanceMutex = new();
|
|
private readonly Lock _stateTransitionLock = new();
|
|
private WatchThread<RobotController>? _watchTimer;
|
|
private bool _rfHandleHasPriority = false;
|
|
private IBattery? Battery;
|
|
private OperatingMode _previousPlcMode = OperatingMode.SERVICE;
|
|
private double _batteryLowThresholdPercent = 20.0;
|
|
|
|
protected override async Task ExecuteAsync(CancellationToken stoppingToken)
|
|
{
|
|
await Task.Yield();
|
|
await StateManager.InitializeAsync();
|
|
while (!stoppingToken.IsCancellationRequested)
|
|
{
|
|
if (StateManager.CurrentState == RobotStateType.Standby) break;
|
|
await Task.Delay(1000, stoppingToken);
|
|
}
|
|
|
|
// Subscribe to PLC events
|
|
PlcController.OnPeripheralModeChanged += OnPlcModeChanged;
|
|
PlcController.OnStop += OnStop;
|
|
PlcController.OnButtonPressed += OnButtonPressed;
|
|
|
|
// Subscribe to RF Handle mode changes (via RobotController for PLC sync)
|
|
RFControl.OnRfModeChanged += OnRfModeChanged;
|
|
|
|
// Subscribe to fatal errors
|
|
ErrorManager.OnNewFatalError += OnNewFatalError;
|
|
|
|
// Start WatchThread at 5Hz (200ms)
|
|
_watchTimer = new WatchThread<RobotController>(200, WatchThreadCallback, null);
|
|
_watchTimer.Start();
|
|
|
|
var deviceBattery = DeviceProvider.GetDeviceByType(Client.Shared.Devices.DeviceType.Battery);
|
|
if(deviceBattery is IBattery battery) Battery = battery;
|
|
_batteryLowThresholdPercent = ResolveBatteryLowThresholdPercent();
|
|
|
|
// Initial mode switch based on current PLC mode
|
|
_previousPlcMode = PlcController.PeripheralMode;
|
|
SwitchModeChanged(PlcController.PeripheralMode);
|
|
PlcController.SetRFMode(RFMode.None);
|
|
while (!stoppingToken.IsCancellationRequested)
|
|
{
|
|
if(RFControl.IsRunning)
|
|
{
|
|
RFControl.Start();
|
|
break;
|
|
}
|
|
await Task.Delay(2000);
|
|
}
|
|
}
|
|
|
|
public override Task StopAsync(CancellationToken cancellationToken)
|
|
{
|
|
StopHandler();
|
|
return base.StopAsync(cancellationToken);
|
|
}
|
|
|
|
public async Task ModuleInitializeAsync()
|
|
{
|
|
while (true)
|
|
{
|
|
if (StateManager.IsInitialized) break;
|
|
await Task.Delay(500);
|
|
}
|
|
|
|
// Start MQTT independently so connection topics are available even if hardware init is slow.
|
|
_ = RobotConnectionsService.StartAsync(CancellationToken.None);
|
|
// Start VDA5050 publishers early so state/visualization topics keep updating.
|
|
StateService.Start();
|
|
VisualizationService.Start();
|
|
|
|
while (true)
|
|
{
|
|
try
|
|
{
|
|
if (!RobotConfiguration.GetSimulationConfig().IsEnable)
|
|
{
|
|
Logger.Info("Checking hardware...");
|
|
|
|
await PlcController.Start(CancellationToken.None);
|
|
|
|
while (!PlcController.IsReady || !DeviceProvider.AreDevicesConnected)
|
|
{
|
|
// (!DeviceProvider.AreDevicesConnected) Logger.Info(" - Devices service not ready");
|
|
//if (!PlcController.IsReady) Logger.Info(" - Peripheral service not ready");
|
|
if (PlcController.IsReady) PlcController.SetSystemState(SystemState.INIT);
|
|
// if (!LiftModule.IsReady) Logger.Info(" - LiftModule service not ready");
|
|
// if (!RotateModule.IsReady) Logger.Info(" - RotateModule service not ready");
|
|
await Task.Delay(3000);
|
|
}
|
|
}
|
|
Logger.Info("Hardware modules ready");
|
|
|
|
// Start software modules independently
|
|
NavigationManager.Start();
|
|
|
|
StateManager.Fire(RobotEventType.InitializeCompleted);
|
|
Logger.Info("Initialization completed");
|
|
break;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Logger.Warning($"Robot initialize failed: {ex.Message}");
|
|
await Task.Delay(2000);
|
|
}
|
|
}
|
|
}
|
|
|
|
public void StopHandler()
|
|
{
|
|
_watchTimer?.Dispose();
|
|
_watchTimer = null;
|
|
|
|
if (RobotConnectionsService.IsConnected)
|
|
{
|
|
var pubOffline = RobotConnectionsService.PublishConnectionStateAsync(ConnectionState.OFFLINE);
|
|
pubOffline.Wait();
|
|
}
|
|
|
|
var stopConnection = RobotConnectionsService.StopAsync();
|
|
stopConnection.Wait();
|
|
RobotEventBus.OrderMessageReceived -= NewOrderUpdated;
|
|
RobotEventBus.InstantActionReceived -= NewInstantActionUpdated;
|
|
NavigationManager.Stop();
|
|
PlcController.Stop();
|
|
PlcController.OnPeripheralModeChanged -= OnPlcModeChanged;
|
|
PlcController.OnStop -= OnStop;
|
|
PlcController.OnButtonPressed -= OnButtonPressed;
|
|
RFControl.OnRfModeChanged -= OnRfModeChanged;
|
|
ErrorManager.OnNewFatalError -= OnNewFatalError;
|
|
}
|
|
|
|
public void NewOrderUpdated(object? sender, OrderChangedEvent e)
|
|
{
|
|
if (NewOrderMutex.WaitOne(2000))
|
|
{
|
|
try
|
|
{
|
|
var orderMsg = e.OrderMessage;
|
|
if (!StateManager.IsInState(RobotStateType.Auto)) throw new OrderException(RobotErrors.Error1006(StateManager.CurrentState.ToString()));
|
|
if (!Localization.IsReady) throw new OrderException(RobotErrors.Error3001());
|
|
OrderManager.UpdateOrder(orderMsg);
|
|
}
|
|
catch (RobotException orEx)
|
|
{
|
|
if (orEx.Error is not null)
|
|
{
|
|
ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10));
|
|
Logger.Warning($"New order error: {orEx.Error.ErrorDescription}");
|
|
}
|
|
else Logger.Warning($"New order error: {orEx.Message}");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Logger.Warning($"Order processing error: {ex.Message}");
|
|
}
|
|
finally
|
|
{
|
|
NewOrderMutex.ReleaseMutex();
|
|
}
|
|
}
|
|
}
|
|
|
|
public void NewInstantActionUpdated(object? sender, InstantActionChangedEvent e)
|
|
{
|
|
if (NewInstanceMutex.WaitOne(2000))
|
|
{
|
|
try
|
|
{
|
|
var instantAction = e.InstantActionMessage;
|
|
|
|
// VDA5050: Filter instant actions based on current robot state
|
|
var filteredActions = FilterInstantActionsByState(instantAction.Actions);
|
|
|
|
if (filteredActions.Length > 0)
|
|
{
|
|
ActionManager.AddInstantAction(filteredActions);
|
|
}
|
|
}
|
|
catch (RobotException acEx)
|
|
{
|
|
if (acEx.Error is not null)
|
|
{
|
|
ErrorManager.AddError(acEx.Error, TimeSpan.FromSeconds(10));
|
|
Logger.Warning($"InstantAction error: {acEx.Error.ErrorDescription}");
|
|
}
|
|
else Logger.Warning($"InstantAction error: {acEx.Message}");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Logger.Warning($"InstantAction processing error: {ex.Message}");
|
|
}
|
|
finally
|
|
{
|
|
NewInstanceMutex.ReleaseMutex();
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Filter instant actions based on current robot state for security and safety
|
|
/// </summary>
|
|
private RobotNet.VDA5050.InstantAction.Action[] FilterInstantActionsByState(RobotNet.VDA5050.InstantAction.Action[] actions)
|
|
{
|
|
var currentState = StateManager.CurrentState;
|
|
var allowedActions = new List<RobotNet.VDA5050.InstantAction.Action>();
|
|
|
|
foreach (var action in actions)
|
|
{
|
|
bool isAllowed = IsActionAllowedInState(action.ActionType, currentState);
|
|
|
|
if (isAllowed)
|
|
{
|
|
allowedActions.Add(action);
|
|
}
|
|
else
|
|
{
|
|
// VDA5050: Report rejected instant action as error
|
|
var error = new RobotError
|
|
{
|
|
ErrorType = "instantActionRejected",
|
|
ErrorLevel = RobotNet.VDA5050.Type.ErrorLevel.WARNING,
|
|
ErrorDescription = $"Instant action '{action.ActionType}' rejected - not allowed in state '{currentState}'",
|
|
ErrorReferences = [
|
|
new() { ReferenceKey = "actionId", ReferenceValue = action.ActionId },
|
|
new() { ReferenceKey = "actionType", ReferenceValue = action.ActionType },
|
|
new() { ReferenceKey = "robotState", ReferenceValue = currentState.ToString() }
|
|
]
|
|
};
|
|
ErrorManager.AddError(error, TimeSpan.FromSeconds(10));
|
|
Logger.Warning($"Instant action {action.ActionId} (type: {action.ActionType}) rejected - not allowed in state {currentState}");
|
|
}
|
|
}
|
|
|
|
return [.. allowedActions];
|
|
}
|
|
|
|
/// <summary>
|
|
/// Define which instant actions are allowed in each robot state
|
|
/// </summary>
|
|
private static bool IsActionAllowedInState(string actionType, RobotStateType state)
|
|
{
|
|
// Actions allowed in ALL states (read-only or critical control)
|
|
var alwaysAllowedActions = new HashSet<string>
|
|
{
|
|
"cancelOrder", // VDA5050: Must work in all states
|
|
"stateRequest", // Read-only
|
|
"factsheetRequest", // Read-only
|
|
};
|
|
|
|
if (alwaysAllowedActions.Contains(actionType)) return true;
|
|
|
|
// Actions allowed only in Auto state
|
|
if (state == RobotStateType.Auto ||
|
|
state == RobotStateType.Idle ||
|
|
state == RobotStateType.Executing ||
|
|
state == RobotStateType.Paused ||
|
|
state == RobotStateType.Canceling)
|
|
{
|
|
return true; // All actions allowed in Auto mode
|
|
}
|
|
|
|
// Shared set of maintenance/setup actions (used in Service, Manual, System, Standby)
|
|
var maintenanceAllowedActions = new HashSet<string>
|
|
{
|
|
"initPosition",
|
|
"pick",
|
|
"drop",
|
|
"rotate",
|
|
"liftRotate",
|
|
"homingCamera",
|
|
"liftCameraByHeight",
|
|
"controlLight",
|
|
"cameraLightOn",
|
|
"cameraLightOff",
|
|
"mutedBaseOn",
|
|
"mutedBaseOff",
|
|
"mutedLoadOn",
|
|
"mutedLoadOff",
|
|
"dockTo",
|
|
"moveStraightToCoor",
|
|
"moveStraightWithDistance"
|
|
};
|
|
|
|
// Actions allowed in Service/Override/Manual states (maintenance/manual control)
|
|
if (state == RobotStateType.Service ||
|
|
state == RobotStateType.Remote_Override ||
|
|
state == RobotStateType.Manual)
|
|
{
|
|
return maintenanceAllowedActions.Contains(actionType);
|
|
}
|
|
|
|
// After ReleaseStop robot goes to System/Standby - allow maintenance actions so operator can e.g. lift camera before switching mode
|
|
if (state == RobotStateType.System || state == RobotStateType.Standby)
|
|
{
|
|
return maintenanceAllowedActions.Contains(actionType);
|
|
}
|
|
|
|
// Stop and Fault states: only critical control actions
|
|
if (state == RobotStateType.Stop || state == RobotStateType.Fault)
|
|
{
|
|
// Already handled by alwaysAllowedActions above
|
|
return false;
|
|
}
|
|
|
|
// Default: reject
|
|
return false;
|
|
}
|
|
|
|
public void Pause()
|
|
{
|
|
OrderManager.PauseOrder();
|
|
ActionManager.PauseActions();
|
|
}
|
|
|
|
public void Resume()
|
|
{
|
|
OrderManager.ResumeOrder();
|
|
ActionManager.ResumeActions();
|
|
}
|
|
|
|
public bool TryClearFault()
|
|
{
|
|
lock (_stateTransitionLock)
|
|
{
|
|
if (!StateManager.IsInState(RobotStateType.Fault)) return false;
|
|
|
|
if (PlcController.IsReady && !PlcController.IsDisconected)
|
|
ErrorManager.DeleteErrorId(2003);
|
|
|
|
ErrorManager.ClearFatalErrors();
|
|
|
|
if (!ErrorManager.HasFatalError)
|
|
{
|
|
Logger.Info("TryClearFault: Exiting Fault");
|
|
StateManager.Fire(RobotEventType.ExitFault);
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
}
|
|
|
|
private void OnPlcModeChanged(OperatingMode mode)
|
|
{
|
|
lock (_stateTransitionLock)
|
|
{
|
|
if (_rfHandleHasPriority)
|
|
{
|
|
Logger.Info($"PLC mode change to {mode} ignored - RF Handle has priority");
|
|
return;
|
|
}
|
|
// Khi chuyển từ Lock (SERVICE) sang Auto hoặc Manual: ghi M815 xuống PLC, reset fault, enable động cơ
|
|
if (_previousPlcMode == OperatingMode.SERVICE && (mode == OperatingMode.AUTOMATIC || mode == OperatingMode.MANUAL))
|
|
{
|
|
Logger.Info($"PLC Lock -> {mode}: áp dụng ApplyResetFromPlc (M815 + fault reset + enable)");
|
|
ApplyResetFromPlc();
|
|
}
|
|
_previousPlcMode = mode;
|
|
SwitchModeChanged(mode);
|
|
}
|
|
}
|
|
|
|
/// <summary>Reset theo PLC (M815): ghi M815 xuống PLC (pulse), clear fault robot, reset fault động cơ, enable lại động cơ (retry đến khi OperationEnabled).</summary>
|
|
private void ApplyResetFromPlc()
|
|
{
|
|
Logger.Info("ApplyResetFromPlc: bắt đầu (M815 pulse, clear fault, fault reset + enable drive)");
|
|
try { PlcController.WriteAlarmResetM815(); } catch (Exception ex) { Logger.Warning($"WriteAlarmResetM815: {ex.Message}"); }
|
|
TryClearFault();
|
|
try
|
|
{
|
|
InverseKinematics?.FaultReset();
|
|
// Đợi servo thoát Fault (CiA402 có thể cần >1s để cập nhật statusword)
|
|
Thread.Sleep(1500);
|
|
InverseKinematics?.FaultReset();
|
|
Thread.Sleep(800);
|
|
// Enable 2 động cơ giống enable bằng tay trên device: gửi lệnh trực tiếp, await từng bước
|
|
if (InverseKinematics != null)
|
|
{
|
|
InverseKinematics.EnableAsync(CancellationToken.None).GetAwaiter().GetResult();
|
|
if (InverseKinematics.IsOperationEnabled)
|
|
Logger.Info("ApplyResetFromPlc: 2 động cơ đã enable (OperationEnabled)");
|
|
else
|
|
Logger.Warning("ApplyResetFromPlc: động cơ chưa lên OperationEnabled sau EnableAsync");
|
|
}
|
|
}
|
|
catch (Exception ex) { Logger.Warning($"FaultReset/Enable drive: {ex.Message}"); }
|
|
}
|
|
|
|
private void SwitchModeChanged(OperatingMode mode)
|
|
{
|
|
// Pause order when leaving Auto mode
|
|
if (StateManager.IsInState(RobotStateType.Auto) && mode != OperatingMode.AUTOMATIC)
|
|
{
|
|
Pause();
|
|
RobotEventBus.OrderMessageReceived -= NewOrderUpdated;
|
|
// Keep InstantActionReceived subscription - instant actions (e.g. cancelOrder) must work in all states
|
|
}
|
|
|
|
switch (mode)
|
|
{
|
|
case OperatingMode.AUTOMATIC:
|
|
Ps5Controller.Disable();
|
|
StateManager.Fire(RobotEventType.EnterAuto);
|
|
// Prevent duplicate subscriptions
|
|
RobotEventBus.OrderMessageReceived -= NewOrderUpdated;
|
|
RobotEventBus.OrderMessageReceived += NewOrderUpdated;
|
|
RobotEventBus.InstantActionReceived -= NewInstantActionUpdated;
|
|
RobotEventBus.InstantActionReceived += NewInstantActionUpdated;
|
|
Resume();
|
|
break;
|
|
case OperatingMode.MANUAL:
|
|
Ps5Controller.Enable();
|
|
StateManager.Fire(RobotEventType.EnterManual);
|
|
break;
|
|
case OperatingMode.SERVICE:
|
|
Ps5Controller.Disable();
|
|
StateManager.Fire(RobotEventType.EnterService);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
private void OnStop(StopStateType state)
|
|
{
|
|
lock (_stateTransitionLock)
|
|
{
|
|
if (state != StopStateType.None)
|
|
{
|
|
_rfHandleHasPriority = false; // Safety overrides RF Handle
|
|
if (!StateManager.IsInState(RobotStateType.Stop))
|
|
{
|
|
Pause();
|
|
StateManager.Fire(RobotEventType.EnterStop);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// No physical Start button: leave Stop as soon as PLC reports all safety inputs clear.
|
|
TryReleaseStopAfterSafetyClear();
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Exit Stop when EMC/bumper are released. Previously required a Start button; this robot has none.
|
|
/// </summary>
|
|
private void TryReleaseStopAfterSafetyClear()
|
|
{
|
|
if (!StateManager.IsInState(RobotStateType.Stop))
|
|
return;
|
|
if (PlcController.Emergency || PlcController.Bumper)
|
|
return;
|
|
|
|
Logger.Info("Robot Controller: Safety cleared; releasing Stop (auto, no Start button)");
|
|
StateManager.Fire(RobotEventType.ReleaseStop);
|
|
}
|
|
|
|
private void OnButtonPressed(PeripheralButton button)
|
|
{
|
|
lock (_stateTransitionLock)
|
|
{
|
|
if (button == PeripheralButton.Reset)
|
|
{
|
|
// M815 Reset: clear robot fault + reset fault động cơ
|
|
ApplyResetFromPlc();
|
|
}
|
|
}
|
|
}
|
|
|
|
private void OnNewFatalError()
|
|
{
|
|
lock (_stateTransitionLock)
|
|
{
|
|
if (!StateManager.IsInState(RobotStateType.Fault))
|
|
{
|
|
_rfHandleHasPriority = false; // Fault overrides RF Handle
|
|
Pause();
|
|
StateManager.Fire(RobotEventType.EnterFault);
|
|
}
|
|
}
|
|
}
|
|
|
|
private void OnRfModeChanged(RFMode rfMode)
|
|
{
|
|
lock (_stateTransitionLock)
|
|
{
|
|
// Ignore RF mode changes while in Stop or Fault — safety overrides everything
|
|
// RF mode will be re-evaluated when returning to Standby via WatchThread
|
|
if (StateManager.IsInState(RobotStateType.Stop) || StateManager.IsInState(RobotStateType.Fault))
|
|
{
|
|
Logger.Info($"RF mode change to {rfMode} ignored - robot in {StateManager.CurrentState}");
|
|
return;
|
|
}
|
|
|
|
switch (rfMode)
|
|
{
|
|
case RFMode.Maintenance:
|
|
// RF Handle requests Service mode
|
|
_rfHandleHasPriority = true;
|
|
if (StateManager.IsInState(RobotStateType.Auto))
|
|
{
|
|
Pause();
|
|
RobotEventBus.OrderMessageReceived -= NewOrderUpdated;
|
|
// Keep InstantActionReceived subscription - instant actions must work in Service mode
|
|
}
|
|
StateManager.Fire(RobotEventType.EnterService);
|
|
break;
|
|
|
|
case RFMode.Override:
|
|
// RF Handle requests Remote Override
|
|
_rfHandleHasPriority = true;
|
|
if (StateManager.IsInState(RobotStateType.Auto))
|
|
{
|
|
Pause();
|
|
RobotEventBus.OrderMessageReceived -= NewOrderUpdated;
|
|
// Keep InstantActionReceived subscription - instant actions must work in Override mode
|
|
}
|
|
StateManager.Fire(RobotEventType.RemoteOverride);
|
|
break;
|
|
|
|
case RFMode.Default:
|
|
case RFMode.None:
|
|
// RF Handle released control or disconnected - return to PLC-determined mode
|
|
_rfHandleHasPriority = false;
|
|
PlcController.SetRFEStop(false);
|
|
if (StateManager.IsInState(RobotStateType.Service) || StateManager.IsInState(RobotStateType.Remote_Override))
|
|
{
|
|
SwitchModeChanged(PlcController.PeripheralMode);
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
private void WatchThreadCallback()
|
|
{
|
|
lock (_stateTransitionLock)
|
|
{
|
|
// 1. Fatal error detection
|
|
if (ErrorManager.HasFatalError && !StateManager.IsInState(RobotStateType.Fault))
|
|
{
|
|
Logger.Warning("Robot Controller: Fatal error detected, transitioning to Fault state");
|
|
Pause();
|
|
StateManager.Fire(RobotEventType.EnterFault);
|
|
return;
|
|
}
|
|
|
|
// 2. In Stop: release automatically when PLC shows safety clear (backup if OnStop edge was missed)
|
|
if (StateManager.IsInState(RobotStateType.Stop))
|
|
{
|
|
TryReleaseStopAfterSafetyClear();
|
|
return;
|
|
}
|
|
|
|
// 2b. Fault auto-recovery
|
|
if (StateManager.IsInState(RobotStateType.Fault))
|
|
{
|
|
if (PlcController.IsReady && !PlcController.IsDisconected)
|
|
ErrorManager.DeleteErrorId(2003);
|
|
|
|
if (!ErrorManager.HasFatalError)
|
|
{
|
|
Logger.Info("Robot Controller: Fatal errors resolved, auto-recovering from Fault");
|
|
StateManager.Fire(RobotEventType.ExitFault);
|
|
}
|
|
return;
|
|
}
|
|
|
|
var plcMode = PlcController.PeripheralMode;
|
|
|
|
// 3. If in Standby, trigger mode switch (e.g., after ReleaseStop or initialization)
|
|
if (StateManager.CurrentState == RobotStateType.Standby)
|
|
{
|
|
// Check if RF Handle has an active mode that should take priority
|
|
// (RF mode preserved on PLC during Stop/Fault, re-evaluated here after release)
|
|
var rfMode = PlcController.CurrentRFMode;
|
|
if (rfMode == RFMode.Maintenance)
|
|
{
|
|
Logger.Info("Robot Controller: Standby → RF Handle Maintenance detected, entering Service");
|
|
_rfHandleHasPriority = true;
|
|
StateManager.Fire(RobotEventType.EnterService);
|
|
}
|
|
else if (rfMode == RFMode.Override)
|
|
{
|
|
Logger.Info("Robot Controller: Standby → RF Handle Override detected, entering Remote_Override");
|
|
_rfHandleHasPriority = true;
|
|
StateManager.Fire(RobotEventType.RemoteOverride);
|
|
}
|
|
else
|
|
{
|
|
SwitchModeChanged(plcMode);
|
|
}
|
|
return;
|
|
}
|
|
|
|
// 4. PLC mode mismatch check — ONLY when RF Handle does NOT have priority
|
|
if (!_rfHandleHasPriority)
|
|
{
|
|
var currentModeMatch = plcMode switch
|
|
{
|
|
OperatingMode.AUTOMATIC => StateManager.IsInState(RobotStateType.Auto),
|
|
OperatingMode.MANUAL => StateManager.IsInState(RobotStateType.Manual),
|
|
OperatingMode.SERVICE => StateManager.IsInState(RobotStateType.Service),
|
|
_ => true
|
|
};
|
|
|
|
if (!currentModeMatch)
|
|
{
|
|
Logger.Warning($"Robot Controller: PLC mode mismatch. PLC: {plcMode}, State: {StateManager.CurrentState}");
|
|
SwitchModeChanged(plcMode);
|
|
}
|
|
}
|
|
|
|
// 5. Backup stop detection (ALWAYS runs, even when RF Handle has priority)
|
|
bool hasSafetyStop = PlcController.Emergency || PlcController.Bumper;
|
|
|
|
if (hasSafetyStop && !StateManager.IsInState(RobotStateType.Stop))
|
|
{
|
|
Logger.Warning("Robot Controller: Safety stop detected from PLC properties");
|
|
_rfHandleHasPriority = false; // Safety overrides RF Handle
|
|
Pause();
|
|
StateManager.Fire(RobotEventType.EnterStop);
|
|
}
|
|
|
|
// 6. Check has load
|
|
if (LiftModule.IsReady && LiftModule.Position == LiftPosition.Top) PlcController.SetHasLoad(true);
|
|
else PlcController.SetHasLoad(false);
|
|
|
|
// 7. Check Pin: set M929 when battery percentage is below configured threshold.
|
|
if(Battery != null
|
|
&& Battery.CurrentBatteryState.HasValue
|
|
&& !double.IsNaN(Battery.CurrentBatteryState.Value.Percentage)
|
|
&& Battery.CurrentBatteryState.Value.Percentage < _batteryLowThresholdPercent)
|
|
{
|
|
PlcController.SetBatteryLow(true);
|
|
}
|
|
else PlcController.SetBatteryLow(false);
|
|
|
|
// 8. Check PLC connection
|
|
if (PlcController.IsDisconected)
|
|
{
|
|
ErrorManager.AddError(RobotErrors.Error2003());
|
|
}
|
|
else
|
|
{
|
|
ErrorManager.DeleteErrorId(2003);
|
|
}
|
|
}
|
|
}
|
|
|
|
private double ResolveBatteryLowThresholdPercent()
|
|
{
|
|
const double defaultThreshold = 20.0;
|
|
try
|
|
{
|
|
if (Battery is not DeviceBase batteryDevice)
|
|
{
|
|
return defaultThreshold;
|
|
}
|
|
|
|
var devicesSection = Configuration.GetSection("Devices");
|
|
foreach (var section in devicesSection.GetChildren())
|
|
{
|
|
var deviceId = section.GetValue<string>("DeviceId");
|
|
if (!string.Equals(deviceId, batteryDevice.DeviceId, StringComparison.OrdinalIgnoreCase))
|
|
{
|
|
continue;
|
|
}
|
|
|
|
var threshold = section.GetSection("Connection").GetValue<double?>("LowBatteryThresholdPercent");
|
|
if (!threshold.HasValue)
|
|
{
|
|
return defaultThreshold;
|
|
}
|
|
|
|
var clamped = Math.Clamp(threshold.Value, 0.0, 100.0);
|
|
if (Math.Abs(clamped - threshold.Value) > double.Epsilon)
|
|
{
|
|
Logger.Warning($"Battery low threshold {threshold.Value} out of range [0..100], clamped to {clamped}");
|
|
}
|
|
Logger.Info($"Battery low threshold loaded from config: {clamped}%");
|
|
return clamped;
|
|
}
|
|
|
|
return defaultThreshold;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Logger.Warning($"Failed to resolve battery low threshold from config, fallback {defaultThreshold}%: {ex.Message}");
|
|
return defaultThreshold;
|
|
}
|
|
}
|
|
} |