1969 lines
73 KiB
C#
1969 lines
73 KiB
C#
using System.Runtime.InteropServices;
|
|
using System.Text;
|
|
using Appccelerate.StateMachine;
|
|
using Appccelerate.StateMachine.Machine;
|
|
using RobotNet10.CANOpen.CiA402.Enums;
|
|
using RobotNet10.RobotApp.Devices;
|
|
using RobotNet10.RobotApp.Interfaces;
|
|
using RobotNet10.RobotApp.Modules;
|
|
using RobotNet10.RobotApp.Natives;
|
|
using RobotNet10.Shared.Geometry;
|
|
// using RobotNet10.Shared.Numbers;
|
|
|
|
namespace RobotNet10.RobotApp.Motion;
|
|
|
|
/// <summary>
|
|
/// States cho ManualControlService state machine
|
|
/// State được xác định tự động dựa trên dữ liệu từ RF Handle
|
|
/// </summary>
|
|
public enum ManualControlState
|
|
{
|
|
/// <summary>Initial state, waiting for device connection</summary>
|
|
Initialization,
|
|
/// <summary>No signal from RF Handle (RemoteReady == false)</summary>
|
|
Disabled,
|
|
/// <summary>EStop is pressed (has signal but emergency stop active)</summary>
|
|
SafeStop,
|
|
/// <summary>Has signal + no EStop + mode is Unknown</summary>
|
|
Active,
|
|
/// <summary>Has signal + no EStop + Mode == "Maintenance" - allows robot control</summary>
|
|
Maintenance,
|
|
/// <summary>Has signal + no EStop + Mode == "Override" - allows robot control</summary>
|
|
Override,
|
|
/// <summary>Has signal + no EStop + Mode == "Default" - reserved for future</summary>
|
|
Default
|
|
}
|
|
|
|
/// <summary>
|
|
/// Service điều khiển robot từ tay điều khiển RF Handle
|
|
/// Chuyển đổi dữ liệu từ IRfHandle thành Twist và điều khiển IInverseKinematics
|
|
/// </summary>
|
|
public class ManualControlService : IHostedService, IDisposable
|
|
{
|
|
private readonly ManualControlConfiguration _config;
|
|
private readonly IServiceProvider _serviceProvider;
|
|
private readonly IDeviceProvider _deviceProvider;
|
|
private readonly IInverseKinematics _inverseKinematics;
|
|
private readonly ILiftModule _liftModule;
|
|
private readonly IRotationModule _rotationModule;
|
|
private readonly IPlcController _plcController;
|
|
private readonly ILogger<ManualControlService> _logger;
|
|
private readonly Lock _lock = new();
|
|
|
|
private IRfHandle? _rfHandle;
|
|
private CancellationTokenSource? _updateCts;
|
|
private Thread? _updateThread;
|
|
private ManualControlState _currentState = ManualControlState.Initialization;
|
|
private ManualControlState _previousState = ManualControlState.Initialization;
|
|
private ManualControlState? _externallySetState = null; // State set by RobotStateMachine
|
|
private bool _wasInControlState = false; // Track state transitions for Maintenance/Override
|
|
private bool _disposed = false;
|
|
private volatile bool _isRunning = false; // Thread-safe running state
|
|
private int _updateLoopCounter = 0;
|
|
|
|
// ModeSelect button hold tracking
|
|
private DateTime? _modeSelectPressedTime = null;
|
|
private const int ModeSelectHoldDurationMs = 2000; // 2 seconds hold required
|
|
|
|
// RF Mode change tracking - notifies RobotController when RF Mode changes
|
|
private RFMode _previousRfMode = RFMode.None;
|
|
|
|
/// <summary>
|
|
/// Event raised when RF Handle mode changes.
|
|
/// RobotController subscribes to this to handle state transitions with PLC sync.
|
|
/// </summary>
|
|
public event Action<RFMode>? OnRfModeChanged;
|
|
|
|
// Keyboard input state (khi UsingKeyboard = true)
|
|
private Thread? _keyboardThread;
|
|
private readonly Dictionary<ConsoleKey, bool> _keyStates = new();
|
|
private int _keyboardSpeed = 50; // Default 50%
|
|
private bool _keyboardReady = false;
|
|
|
|
// Linux evdev keyboard state
|
|
private int _evdevFd = -1;
|
|
private const int INPUT_DEVICE_MAX = 32; // Max /dev/input/event* devices to check
|
|
|
|
// Current velocity being sent to robot
|
|
private Twist _currentTwist = new();
|
|
|
|
/// <summary>
|
|
/// Gets the current state of the ManualControlService
|
|
/// </summary>
|
|
public ManualControlState State => _currentState;
|
|
|
|
/// <summary>
|
|
/// Gets the current twist being sent to robot
|
|
/// </summary>
|
|
public Twist CurrentTwist
|
|
{
|
|
get
|
|
{
|
|
lock (_lock)
|
|
{
|
|
return _currentTwist;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the current RF Handle status (if available)
|
|
/// </summary>
|
|
public RfHandleStatus? CurrentRfHandleStatus
|
|
{
|
|
get
|
|
{
|
|
if (_rfHandle == null)
|
|
return null;
|
|
|
|
lock (_lock)
|
|
{
|
|
return new RfHandleStatus
|
|
{
|
|
Heartbeat = _rfHandle.Heartbeat,
|
|
Ready = _rfHandle.RemoteReady,
|
|
EStop = _rfHandle.EStop,
|
|
Enable = _rfHandle.Enable,
|
|
Speed = _rfHandle.Speed,
|
|
Mode = _plcController.CurrentRFMode.ToString(),
|
|
LastUpdateTime = _rfHandle.LastUpdateTime
|
|
};
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Maps Linux evdev key codes to ConsoleKey enum
|
|
/// </summary>
|
|
private static readonly Dictionary<ushort, ConsoleKey> LinuxKeyToConsoleKey = new()
|
|
{
|
|
{ EvdevNative.KEY_W, ConsoleKey.W },
|
|
{ EvdevNative.KEY_A, ConsoleKey.A },
|
|
{ EvdevNative.KEY_S, ConsoleKey.S },
|
|
{ EvdevNative.KEY_D, ConsoleKey.D },
|
|
{ EvdevNative.KEY_Q, ConsoleKey.Q },
|
|
{ EvdevNative.KEY_E, ConsoleKey.E },
|
|
{ EvdevNative.KEY_Z, ConsoleKey.Z },
|
|
{ EvdevNative.KEY_C, ConsoleKey.C },
|
|
{ EvdevNative.KEY_SPACE, ConsoleKey.Spacebar },
|
|
{ EvdevNative.KEY_MINUS, ConsoleKey.Subtract },
|
|
{ EvdevNative.KEY_EQUAL, ConsoleKey.Add },
|
|
{ EvdevNative.KEY_KPMINUS, ConsoleKey.Subtract },
|
|
{ EvdevNative.KEY_KPPLUS, ConsoleKey.Add },
|
|
{ EvdevNative.KEY_ESC, ConsoleKey.Escape }
|
|
};
|
|
|
|
public ManualControlService(
|
|
IConfiguration configuration,
|
|
IServiceProvider serviceProvider,
|
|
IDeviceProvider deviceProvider,
|
|
IInverseKinematics inverseKinematics,
|
|
ILiftModule liftModule,
|
|
IRotationModule rotationModule,
|
|
IPlcController plcController,
|
|
ILogger<ManualControlService> logger)
|
|
{
|
|
_serviceProvider = serviceProvider ?? throw new ArgumentNullException(nameof(serviceProvider));
|
|
_deviceProvider = deviceProvider ?? throw new ArgumentNullException(nameof(deviceProvider));
|
|
_logger = logger ?? throw new ArgumentNullException(nameof(logger));
|
|
|
|
// Try to get IInverseKinematics (optional - may not be available)
|
|
_inverseKinematics = inverseKinematics;
|
|
|
|
// Try to get ILiftModule (optional - may not be available)
|
|
_liftModule = liftModule;
|
|
|
|
// Try to get IRotationModule (optional - may not be available)
|
|
_rotationModule = rotationModule;
|
|
|
|
// PLC Controller for system state changes
|
|
_plcController = plcController;
|
|
|
|
// Load configuration
|
|
var configSection = configuration.GetSection("Motion:ManualControl");
|
|
if (!configSection.Exists())
|
|
{
|
|
throw new InvalidOperationException("Configuration section 'Motion:ManualControl' not found in appsettings.json");
|
|
}
|
|
|
|
_config = new ManualControlConfiguration();
|
|
configSection.Bind(_config);
|
|
|
|
// Validate configuration
|
|
ValidateConfiguration();
|
|
|
|
// Initialize state
|
|
_currentState = ManualControlState.Initialization;
|
|
_previousState = ManualControlState.Initialization;
|
|
}
|
|
|
|
private void ValidateConfiguration()
|
|
{
|
|
if (!_config.Enable) return;
|
|
|
|
if (string.IsNullOrWhiteSpace(_config.RfHandleDeviceId))
|
|
{
|
|
if (!_config.UsingKeyboard)
|
|
throw new InvalidOperationException("RfHandleDeviceId is required");
|
|
}
|
|
|
|
if (_config.MinLinearVelocity < 0)
|
|
throw new InvalidOperationException("MinLinearVelocity must be >= 0");
|
|
|
|
if (_config.MaxLinearVelocity <= _config.MinLinearVelocity)
|
|
throw new InvalidOperationException("MaxLinearVelocity must be > MinLinearVelocity");
|
|
|
|
if (_config.MinAngularVelocity < 0)
|
|
throw new InvalidOperationException("MinAngularVelocity must be >= 0");
|
|
|
|
if (_config.MaxAngularVelocity <= _config.MinAngularVelocity)
|
|
throw new InvalidOperationException("MaxAngularVelocity must be > MinAngularVelocity");
|
|
|
|
if (_config.UpdateRate <= 0)
|
|
throw new InvalidOperationException("UpdateRate must be > 0");
|
|
|
|
if (_config.Acceleration <= 0)
|
|
throw new InvalidOperationException("Acceleration must be > 0");
|
|
|
|
if (_config.Deceleration <= 0)
|
|
throw new InvalidOperationException("Deceleration must be > 0");
|
|
}
|
|
|
|
/// <summary>
|
|
/// Determines the current state based on RF Handle data and externally set state
|
|
/// Called every loop cycle to determine the appropriate state
|
|
///
|
|
/// State priority:
|
|
/// 1. If RF Handle not connected -> Initialization
|
|
/// 2. If RemoteReady == false -> Disabled (resets external state)
|
|
/// 3. If EStop pressed -> SafeStop (resets external state)
|
|
/// 4. If external state set (Maintenance/Override) -> use external state
|
|
/// 5. Otherwise -> Active (waiting for RobotStateMachine to set state)
|
|
/// </summary>
|
|
private ManualControlState DetermineStateFromRfHandle()
|
|
{
|
|
if (_rfHandle == null)
|
|
return ManualControlState.Initialization;
|
|
|
|
// No signal from RF Handle - reset external state
|
|
if (!_rfHandle.RemoteReady)
|
|
{
|
|
_externallySetState = null;
|
|
return ManualControlState.Disabled;
|
|
}
|
|
|
|
// Has signal + EStop pressed - keep external state for recovery when EStop released
|
|
if (_rfHandle.EStop)
|
|
{
|
|
_plcController.SetRFEStop(true); // Ensure PLC knows EStop is active
|
|
return ManualControlState.SafeStop;
|
|
}
|
|
else _plcController.SetRFEStop(false);
|
|
|
|
// If external state is set (by RobotStateMachine), use it
|
|
if (_externallySetState.HasValue)
|
|
{
|
|
return _externallySetState.Value;
|
|
}
|
|
|
|
// Has signal + No EStop + No external state -> Active
|
|
// Note: We do NOT auto-switch to Maintenance/Override based on RF Handle Mode
|
|
// State transitions to Maintenance/Override are controlled by RobotStateMachine
|
|
return ManualControlState.Active;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Checks if the current state allows robot control actions
|
|
/// </summary>
|
|
private static bool IsControlAllowedState(ManualControlState state)
|
|
=> state == ManualControlState.Maintenance || state == ManualControlState.Override;
|
|
|
|
/// <summary>
|
|
/// Called when entering Maintenance or Override state
|
|
/// Sets acceleration/deceleration for IInverseKinematics
|
|
/// </summary>
|
|
private void OnEnterControlState()
|
|
{
|
|
try
|
|
{
|
|
// SetAcceleration/SetDeceleration not available - commented out
|
|
// _inverseKinematics.SetAcceleration(_config.Acceleration);
|
|
// _inverseKinematics.SetDeceleration(_config.Deceleration);
|
|
_logger.LogInformation("Entered control state, Accel={Accel}, Decel={Decel}",
|
|
_config.Acceleration, _config.Deceleration);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error setting acceleration/deceleration on entering control state");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Called when exiting Maintenance or Override state
|
|
/// Stops the robot
|
|
/// </summary>
|
|
private void OnExitControlState()
|
|
{
|
|
StopRobot();
|
|
_logger.LogInformation("Exited control state, robot stopped");
|
|
}
|
|
|
|
/// <summary>
|
|
/// Handles ModeSelect button hold detection for PLC system state changes
|
|
/// - In Active state: Hold 2 seconds → PlcController.SetSystemState(MAINTENANCE)
|
|
/// - In Maintenance state: Hold 2 seconds → PlcController.SetSystemState(OVERRIDE)
|
|
///
|
|
/// Note: After PLC changes mode, RF Handle will read new mode, then HandleRfModeChange() fires RobotStateMachine
|
|
/// </summary>
|
|
private void HandleModeSelectButtonHold()
|
|
{
|
|
if (_rfHandle == null || !_plcController.IsReady)
|
|
return;
|
|
|
|
bool modeSelectPressed = _rfHandle.ModeSelect;
|
|
|
|
if (modeSelectPressed)
|
|
{
|
|
// Button is pressed
|
|
if (_modeSelectPressedTime == null)
|
|
{
|
|
// Start tracking press time
|
|
_modeSelectPressedTime = DateTime.UtcNow;
|
|
_logger.LogDebug("ModeSelect button pressed, starting hold timer");
|
|
}
|
|
else
|
|
{
|
|
// Check if held long enough (2 seconds)
|
|
var holdDuration = (DateTime.UtcNow - _modeSelectPressedTime.Value).TotalMilliseconds;
|
|
|
|
if (holdDuration >= ModeSelectHoldDurationMs)
|
|
{
|
|
// Determine action based on current state
|
|
switch (_currentState)
|
|
{
|
|
case ManualControlState.Active:
|
|
// Active → SetSystemState(MAINTENANCE) → PLC changes → RF reads new mode → Fire RobotStateMachine
|
|
_logger.LogInformation("ModeSelect held for 2s in Active state, setting PLC to MAINTENANCE");
|
|
try
|
|
{
|
|
_plcController.SetRFMode(RFMode.Maintenance);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error setting system state to MAINTENANCE");
|
|
}
|
|
break;
|
|
|
|
case ManualControlState.Maintenance:
|
|
// Maintenance → SetSystemState(OVERRIDE) → PLC changes → RF reads new mode → Fire RobotStateMachine
|
|
_logger.LogInformation("ModeSelect held for 2s in Maintenance state, setting PLC to OVERRIDE");
|
|
try
|
|
{
|
|
_plcController.SetRFMode(RFMode.Override);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error setting system state to OVERRIDE");
|
|
}
|
|
break;
|
|
|
|
default:
|
|
// Other states - no action
|
|
break;
|
|
}
|
|
|
|
// Reset timer to prevent repeated triggers
|
|
_modeSelectPressedTime = null;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Button released - reset timer
|
|
if (_modeSelectPressedTime != null)
|
|
{
|
|
_logger.LogDebug("ModeSelect button released before 2s hold");
|
|
_modeSelectPressedTime = null;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Handles RF Mode change detection and notifies RobotController via event
|
|
/// Called every loop cycle after reading RF Handle data
|
|
/// Reads RFMode from PlcController (not from RF Handle device, which doesn't expose Mode)
|
|
///
|
|
/// Flow: RF Mode changes on PLC → detected here → OnRfModeChanged event → RobotController handles state transition
|
|
/// </summary>
|
|
private void HandleRfModeChange()
|
|
{
|
|
if (_plcController == null || !_plcController.IsReady)
|
|
return;
|
|
|
|
var currentRfMode = _plcController.CurrentRFMode;
|
|
|
|
// Check if mode changed
|
|
if (currentRfMode != _previousRfMode)
|
|
{
|
|
_logger.LogInformation("RF Mode changed from {Previous} to {Current}", _previousRfMode, currentRfMode);
|
|
|
|
// Notify RobotController via event (RobotController handles Pause/Resume + state transitions)
|
|
try
|
|
{
|
|
OnRfModeChanged?.Invoke(currentRfMode);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error in OnRfModeChanged handler");
|
|
}
|
|
|
|
// Update previous mode
|
|
_previousRfMode = currentRfMode;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Start high-priority update thread for real-time velocity control
|
|
/// </summary>
|
|
private void StartUpdateLoop()
|
|
{
|
|
StopUpdateLoop(); // Stop existing loop if any
|
|
|
|
// Check if input device is available
|
|
if (_config.UsingKeyboard)
|
|
{
|
|
if (!_keyboardReady)
|
|
{
|
|
_logger.LogWarning("Cannot start update loop: Keyboard input not ready");
|
|
return;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (_rfHandle == null)
|
|
{
|
|
_logger.LogWarning("Cannot start update loop: RF Handle not available");
|
|
return;
|
|
}
|
|
}
|
|
|
|
lock (_lock)
|
|
{
|
|
_updateCts = new CancellationTokenSource();
|
|
var token = _updateCts.Token; // Store token in local variable to avoid race condition
|
|
|
|
if (_config.UsingKeyboard)
|
|
{
|
|
_updateThread = new Thread(() => UpdateFromKeyboardLoop(token))
|
|
{
|
|
Name = "ManualControl-Update-Keyboard",
|
|
IsBackground = false, // Không phải background thread để đảm bảo chạy liên tục
|
|
Priority = ThreadPriority.Highest // Priority cao để đảm bảo real-time control
|
|
};
|
|
}
|
|
else
|
|
{
|
|
_updateThread = new Thread(() => UpdateFromDeviceLoop(token))
|
|
{
|
|
Name = $"ManualControl-Update-{_config.RfHandleDeviceId}",
|
|
IsBackground = false, // Không phải background thread để đảm bảo chạy liên tục
|
|
Priority = ThreadPriority.Highest // Priority cao để đảm bảo real-time control
|
|
};
|
|
}
|
|
|
|
_updateThread.Start();
|
|
_logger.LogDebug("ManualControlService: Started high-priority update thread at {UpdateRate}Hz", _config.UpdateRate);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Stop update thread gracefully
|
|
/// </summary>
|
|
private void StopUpdateLoop()
|
|
{
|
|
CancellationTokenSource? ctsToCancel;
|
|
Thread? threadToWait;
|
|
lock (_lock)
|
|
{
|
|
ctsToCancel = _updateCts;
|
|
threadToWait = _updateThread;
|
|
_updateCts = null;
|
|
_updateThread = null;
|
|
}
|
|
|
|
// Cancel and wait outside the lock to avoid blocking
|
|
if (ctsToCancel != null)
|
|
{
|
|
try
|
|
{
|
|
ctsToCancel.Cancel();
|
|
}
|
|
catch (ObjectDisposedException)
|
|
{
|
|
// CTS already disposed, ignore
|
|
}
|
|
}
|
|
|
|
if (threadToWait != null)
|
|
{
|
|
try
|
|
{
|
|
// Wait for thread to finish gracefully
|
|
if (!threadToWait.Join(TimeSpan.FromSeconds(3)))
|
|
{
|
|
_logger.LogWarning("Timeout waiting for update thread to stop");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error stopping update thread");
|
|
}
|
|
}
|
|
|
|
// Dispose CTS after thread is done
|
|
ctsToCancel?.Dispose();
|
|
}
|
|
|
|
/// <summary>
|
|
/// High-priority synchronous update loop for keyboard input - runs at configured UpdateRate
|
|
/// Similar to UpdateFromDeviceLoop but reads from keyboard instead of RF Handle
|
|
/// </summary>
|
|
private void UpdateFromKeyboardLoop(CancellationToken cancellationToken)
|
|
{
|
|
var updateIntervalMs = (int)(1000.0 / _config.UpdateRate);
|
|
var spinWait = new SpinWait();
|
|
|
|
Thread.BeginThreadAffinity();
|
|
try
|
|
{
|
|
// Ensure IInverseKinematics is in OperationEnabled state and ProfileVelocity mode
|
|
while (!EnsureInverseKinematicsReady(cancellationToken))
|
|
{
|
|
// IInverseKinematics is not ready, stop robot and continue (will retry next cycle)
|
|
StopRobot();
|
|
PreciseDelay(updateIntervalMs, cancellationToken);
|
|
// Use synchronous delay instead of async Task.Delay to avoid thread pool starvation
|
|
PreciseDelay(500, cancellationToken);
|
|
}
|
|
|
|
while (!cancellationToken.IsCancellationRequested)
|
|
{
|
|
var startTime = DateTime.UtcNow;
|
|
|
|
try
|
|
{
|
|
// Increment update loop counter
|
|
_updateLoopCounter++;
|
|
|
|
// Check if keyboard is ready
|
|
if (!_keyboardReady)
|
|
{
|
|
// Log warning every 10 cycles to avoid spam
|
|
if (_updateLoopCounter % 10 == 0)
|
|
{
|
|
_logger.LogWarning("Cannot send velocity: Keyboard input not ready");
|
|
}
|
|
|
|
// Stop robot but continue loop
|
|
StopRobot();
|
|
PreciseDelay(updateIntervalMs, cancellationToken);
|
|
continue;
|
|
}
|
|
|
|
// Check if Stop key is pressed (Space)
|
|
bool stopPressed = false;
|
|
lock (_lock)
|
|
{
|
|
stopPressed = _keyStates.GetValueOrDefault(ConsoleKey.Spacebar, false);
|
|
}
|
|
|
|
if (stopPressed)
|
|
{
|
|
StopRobot();
|
|
PreciseDelay(updateIntervalMs, cancellationToken);
|
|
continue;
|
|
}
|
|
|
|
// Update velocity from keyboard
|
|
UpdateVelocityFromKeyboard(cancellationToken);
|
|
|
|
// Handle lift module control
|
|
HandleLiftModuleControlFromKeyboard(cancellationToken);
|
|
|
|
// Handle rotation module control
|
|
HandleRotationModuleControlFromKeyboard(cancellationToken);
|
|
|
|
// Calculate elapsed time and sleep for remaining interval
|
|
var elapsedMs = (DateTime.UtcNow - startTime).TotalMilliseconds;
|
|
var remainingMs = updateIntervalMs - elapsedMs;
|
|
|
|
if (remainingMs > 0)
|
|
{
|
|
PreciseDelay((int)remainingMs, cancellationToken);
|
|
}
|
|
else
|
|
{
|
|
// If update took longer than interval, log warning and continue immediately
|
|
if (elapsedMs > updateIntervalMs * 1.5) // Only warn if significantly over
|
|
{
|
|
_logger.LogWarning(
|
|
"ManualControlService: Update loop took {ElapsedMs:F1}ms (exceeds {IntervalMs}ms interval)",
|
|
elapsedMs, updateIntervalMs);
|
|
}
|
|
|
|
// Use minimal delay to prevent CPU spinning
|
|
spinWait.Reset();
|
|
for (int i = 0; i < 10; i++)
|
|
{
|
|
if (cancellationToken.IsCancellationRequested)
|
|
break;
|
|
spinWait.SpinOnce();
|
|
}
|
|
}
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
// Loop cancelled, exit gracefully
|
|
break;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error in keyboard update loop");
|
|
|
|
// Stop robot on error but continue loop
|
|
StopRobot();
|
|
PreciseDelay(updateIntervalMs, cancellationToken);
|
|
}
|
|
}
|
|
}
|
|
finally
|
|
{
|
|
Thread.EndThreadAffinity();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// High-priority synchronous update loop - runs at configured UpdateRate
|
|
/// This method runs directly in a high-priority thread for optimal real-time performance
|
|
/// State is determined each cycle based on RF Handle data
|
|
/// Control actions only execute in Maintenance or Override states
|
|
/// </summary>
|
|
private void UpdateFromDeviceLoop(CancellationToken cancellationToken)
|
|
{
|
|
var updateIntervalMs = (int)(1000.0 / _config.UpdateRate);
|
|
var spinWait = new SpinWait();
|
|
|
|
Thread.BeginThreadAffinity();
|
|
try
|
|
{
|
|
while (!cancellationToken.IsCancellationRequested)
|
|
{
|
|
var startTime = DateTime.UtcNow;
|
|
|
|
try
|
|
{
|
|
// Increment update loop counter
|
|
_updateLoopCounter++;
|
|
|
|
// STEP 1: Determine current state from RF Handle data
|
|
var newState = DetermineStateFromRfHandle();
|
|
|
|
// STEP 2: Handle state transitions
|
|
bool isInControlState = IsControlAllowedState(newState);
|
|
bool stateChanged = newState != _currentState;
|
|
|
|
if (isInControlState && !_wasInControlState)
|
|
{
|
|
// Entering control state (Maintenance or Override)
|
|
_logger.LogInformation("Entering control state: {State}", newState);
|
|
OnEnterControlState();
|
|
|
|
// Ensure IK is ready when entering control state
|
|
if (!EnsureInverseKinematicsReady(cancellationToken))
|
|
{
|
|
StopRobot();
|
|
PreciseDelay(updateIntervalMs, cancellationToken);
|
|
continue;
|
|
}
|
|
}
|
|
else if (!isInControlState && _wasInControlState)
|
|
{
|
|
// Exiting control state to non-control state
|
|
_logger.LogInformation("Exiting control state, new state: {State}", newState);
|
|
OnExitControlState(); // This calls StopRobot() once
|
|
}
|
|
else if (!isInControlState && stateChanged)
|
|
{
|
|
// Transitioning between non-control states (e.g., Disabled → SafeStop)
|
|
// Ensure robot is stopped when changing states
|
|
_logger.LogInformation("State changed to: {State}", newState);
|
|
StopRobot();
|
|
}
|
|
|
|
// STEP 3: Update state tracking
|
|
lock (_lock)
|
|
{
|
|
_previousState = _currentState;
|
|
_currentState = newState;
|
|
}
|
|
_wasInControlState = isInControlState;
|
|
|
|
// STEP 3.4: Detect RF Handle disconnection → notify RobotController to release RF priority
|
|
if (newState == ManualControlState.Disabled && stateChanged
|
|
&& _previousState != ManualControlState.Initialization)
|
|
{
|
|
// Clear PLC RF Mode to prevent HandleRfModeChange() from re-detecting old mode
|
|
try { _plcController.SetRFMode(RFMode.None); }
|
|
catch (Exception ex) { _logger.LogError(ex, "Error clearing PLC RF mode on disconnect"); }
|
|
|
|
try
|
|
{
|
|
OnRfModeChanged?.Invoke(RFMode.None);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error in OnRfModeChanged handler (disconnect)");
|
|
}
|
|
_previousRfMode = RFMode.None;
|
|
}
|
|
|
|
// STEP 3.5: Handle ModeSelect button hold for PLC system state changes
|
|
// - Active + ModeSelect held 2s → PlcController.SetSystemState(MAINTENANCE)
|
|
// - Maintenance + ModeSelect held 2s → PlcController.SetSystemState(OVERRIDE)
|
|
HandleModeSelectButtonHold();
|
|
|
|
// STEP 3.6: Handle RF Mode change - Fire RobotStateMachine when RF Mode changes
|
|
// This happens after PLC changes mode and RF Handle reads new mode
|
|
HandleRfModeChange();
|
|
|
|
// STEP 4: Execute actions based on state
|
|
if (isInControlState)
|
|
{
|
|
// In Maintenance or Override: Execute full control
|
|
// Ensure IK is ready (may have been disabled during operation)
|
|
if (!EnsureInverseKinematicsReady(cancellationToken))
|
|
{
|
|
StopRobot();
|
|
PreciseDelay(updateIntervalMs, cancellationToken);
|
|
continue;
|
|
}
|
|
|
|
UpdateVelocityFromRfHandle(cancellationToken);
|
|
HandleLiftModuleControl(cancellationToken);
|
|
HandleRotationModuleControl(cancellationToken);
|
|
}
|
|
else
|
|
{
|
|
// In non-control states (Disabled, SafeStop, Active without external state, etc.)
|
|
// StopRobot() is already called ONCE in OnExitControlState() when transitioning
|
|
// No need to call it repeatedly every cycle
|
|
|
|
// Log state periodically (every 100 cycles = ~5 seconds at 20Hz)
|
|
if (_updateLoopCounter % 100 == 0)
|
|
{
|
|
_logger.LogDebug("ManualControl state: {State}", newState);
|
|
}
|
|
}
|
|
|
|
// STEP 5: Timing control
|
|
var elapsedMs = (DateTime.UtcNow - startTime).TotalMilliseconds;
|
|
var remainingMs = updateIntervalMs - elapsedMs;
|
|
|
|
if (remainingMs > 0)
|
|
{
|
|
PreciseDelay((int)remainingMs, cancellationToken);
|
|
}
|
|
else
|
|
{
|
|
// If update took longer than interval, log warning and continue immediately
|
|
if (elapsedMs > updateIntervalMs * 1.5) // Only warn if significantly over
|
|
{
|
|
_logger.LogWarning(
|
|
"ManualControlService: Update loop took {ElapsedMs:F1}ms (exceeds {IntervalMs}ms interval)",
|
|
elapsedMs, updateIntervalMs);
|
|
}
|
|
|
|
// Use minimal delay to prevent CPU spinning
|
|
spinWait.Reset();
|
|
for (int i = 0; i < 10; i++)
|
|
{
|
|
if (cancellationToken.IsCancellationRequested)
|
|
break;
|
|
spinWait.SpinOnce();
|
|
}
|
|
}
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
// Loop cancelled, exit gracefully
|
|
break;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error in update loop");
|
|
|
|
// Stop robot on error but continue loop
|
|
StopRobot();
|
|
PreciseDelay(updateIntervalMs, cancellationToken);
|
|
}
|
|
}
|
|
}
|
|
finally
|
|
{
|
|
Thread.EndThreadAffinity();
|
|
// Always stop robot when loop exits
|
|
StopRobot();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Precise synchronous delay using Thread.Sleep for longer delays and SpinWait for short delays
|
|
/// This ensures accurate timing for the update loop without async overhead
|
|
/// </summary>
|
|
private static void PreciseDelay(int milliseconds, CancellationToken cancellationToken)
|
|
{
|
|
if (milliseconds <= 0)
|
|
return;
|
|
|
|
if (milliseconds > 1)
|
|
{
|
|
// Use Thread.Sleep for longer delays (synchronous, more precise in dedicated thread)
|
|
// Check cancellation periodically during sleep
|
|
var sleepStart = DateTime.UtcNow;
|
|
while ((DateTime.UtcNow - sleepStart).TotalMilliseconds < milliseconds)
|
|
{
|
|
if (cancellationToken.IsCancellationRequested)
|
|
return;
|
|
|
|
var remaining = milliseconds - (int)(DateTime.UtcNow - sleepStart).TotalMilliseconds;
|
|
if (remaining > 0)
|
|
{
|
|
Thread.Sleep(Math.Min(remaining, 10)); // Sleep in 10ms chunks to check cancellation
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Use SpinWait for very short delays to maintain precise timing
|
|
var spinWait = new SpinWait();
|
|
for (int i = 0; i < 10; i++)
|
|
{
|
|
if (cancellationToken.IsCancellationRequested)
|
|
break;
|
|
spinWait.SpinOnce();
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Synchronous version of UpdateVelocityFromRfHandleAsync for use in high-priority thread
|
|
/// Uses ConfigureAwait(false) and timeout to prevent blocking when CPU load is high
|
|
/// </summary>
|
|
private void UpdateVelocityFromRfHandle(CancellationToken cancellationToken)
|
|
{
|
|
if (_rfHandle == null)
|
|
return;
|
|
|
|
// Read joy state to update RF Handle internal state (even though we use button properties directly)
|
|
// This ensures properties like Forward, Backward, Left, Right, Speed are up-to-date
|
|
var joyState = _rfHandle.CurrentJoyState;
|
|
if (joyState == null)
|
|
{
|
|
// Try to read if not cached with timeout to prevent blocking
|
|
// Direct async call with timeout since we're in a dedicated high-priority thread
|
|
try
|
|
{
|
|
var readTask = _rfHandle.ReadJoyStateAsync(cancellationToken);
|
|
|
|
if (!readTask.Wait(TimeSpan.FromMilliseconds(50), CancellationToken.None))
|
|
{
|
|
_logger.LogWarning("Timeout reading joy state (50ms)");
|
|
StopRobot();
|
|
return;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogWarning(ex, "Failed to read joy state");
|
|
StopRobot();
|
|
return;
|
|
}
|
|
}
|
|
|
|
// Calculate velocity from RF Handle buttons (Forward, Backward, Left, Right)
|
|
var twist = CalculateTwistFromRfHandle();
|
|
|
|
// Update current twist (minimal lock time)
|
|
lock (_lock)
|
|
{
|
|
_currentTwist = twist;
|
|
}
|
|
|
|
// Send to IInverseKinematics (synchronous)
|
|
// Note: IInverseKinematics state is already ensured in UpdateLoop
|
|
try
|
|
{
|
|
var task = _inverseKinematics.SetVelocityAsync(twist, cancellationToken);
|
|
if (!task.Wait(TimeSpan.FromMilliseconds(50), CancellationToken.None))
|
|
{
|
|
_logger.LogWarning("Timeout sending velocity to IInverseKinematics (50ms) - CPU may be overloaded");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error sending velocity to IInverseKinematics");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Synchronous version of EnsureInverseKinematicsReadyAsync for use in high-priority thread
|
|
/// Đảm bảo IInverseKinematics ở trạng thái OperationEnabled và mode ProfileVelocity
|
|
/// Tự động enable nếu cần, reset fault nếu có
|
|
/// Uses ConfigureAwait(false) and timeout to prevent blocking when CPU load is high
|
|
/// </summary>
|
|
/// <returns>True nếu IInverseKinematics ready, False nếu không</returns>
|
|
private bool EnsureInverseKinematicsReady(CancellationToken cancellationToken)
|
|
{
|
|
try
|
|
{
|
|
// Check if need to reset fault first
|
|
// Note: DifferentialDrive doesn't expose IsFaulted, so we try FaultReset if not enabled
|
|
if (!_inverseKinematics.IsOperationEnabled)
|
|
{
|
|
// Try fault reset first (in case it's in fault state)
|
|
_inverseKinematics.FaultReset();
|
|
PreciseDelay(200, cancellationToken);
|
|
}
|
|
|
|
// Check if IInverseKinematics is in OperationEnabled state
|
|
if (!_inverseKinematics.IsOperationEnabled)
|
|
{
|
|
// Auto-enable IInverseKinematics through state transitions
|
|
// Enable() is a convenience method that automatically transitions through all states
|
|
int maxAttempts = 3; // Reduced attempts since this runs in loop
|
|
int attemptDelay = 300; // ms
|
|
|
|
for (int i = 0; i < maxAttempts && !_inverseKinematics.IsOperationEnabled; i++)
|
|
{
|
|
_inverseKinematics.Enable();
|
|
PreciseDelay(attemptDelay, cancellationToken);
|
|
}
|
|
|
|
// Check if enabled successfully
|
|
if (!_inverseKinematics.IsOperationEnabled)
|
|
{
|
|
// Log only once every 10 times to avoid spam
|
|
if (_updateLoopCounter % 10 == 0)
|
|
{
|
|
_logger.LogWarning("IInverseKinematics is not in OperationEnabled state. Cannot send velocity.");
|
|
}
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// Check and set operation mode to ProfileVelocity (synchronous)
|
|
try
|
|
{
|
|
var modeTask = _inverseKinematics.GetOperationModeAsync(cancellationToken);
|
|
if (!modeTask.Wait(TimeSpan.FromMilliseconds(50), CancellationToken.None))
|
|
{
|
|
_logger.LogWarning("Timeout getting operation mode (50ms) - CPU may be overloaded");
|
|
return true; // Continue anyway
|
|
}
|
|
|
|
OperationMode currentMode = modeTask.Result;
|
|
|
|
if (currentMode != OperationMode.ProfileVelocity)
|
|
{
|
|
var setTask = _inverseKinematics.SetOperationModeAsync(OperationMode.ProfileVelocity, cancellationToken);
|
|
if (!setTask.Wait(TimeSpan.FromMilliseconds(50), CancellationToken.None))
|
|
{
|
|
_logger.LogWarning("Timeout setting operation mode (50ms) - CPU may be overloaded");
|
|
return true; // Continue anyway
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Log only once every 10 times to avoid spam
|
|
if (_updateLoopCounter % 10 == 0)
|
|
{
|
|
_logger.LogWarning(ex, "Error checking/setting operation mode");
|
|
}
|
|
// Continue anyway to prevent blocking
|
|
}
|
|
|
|
return true;
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
throw;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Log only once every 10 times to avoid spam
|
|
if (_updateLoopCounter % 10 == 0)
|
|
{
|
|
_logger.LogError(ex, "Error ensuring IInverseKinematics ready");
|
|
}
|
|
return false;
|
|
}
|
|
}
|
|
|
|
private Twist CalculateTwistFromRfHandle()
|
|
{
|
|
var twist = new Twist();
|
|
|
|
// Get speed percentage (0-100)
|
|
var speedPercent = _rfHandle?.Speed ?? 0;
|
|
if (speedPercent < 0) speedPercent = 0;
|
|
if (speedPercent > 100) speedPercent = 100;
|
|
|
|
// Calculate velocity range based on speed percentage
|
|
var linearVelocityRange = _config.MaxLinearVelocity - _config.MinLinearVelocity;
|
|
var angularVelocityRange = _config.MaxAngularVelocity - _config.MinAngularVelocity;
|
|
var speedFactor = speedPercent / 100.0;
|
|
|
|
// Use buttons from RF Handle: Forward/Backward for linear, Left/Right for angular
|
|
double linearInput = _rfHandle?.Linear ?? 0;
|
|
double angularInput = _rfHandle?.Angular ?? 0;
|
|
|
|
// Apply deadzone (min velocity threshold)
|
|
var linearDeadzone = _config.MinLinearVelocity / _config.MaxLinearVelocity;
|
|
var angularDeadzone = _config.MinAngularVelocity / _config.MaxAngularVelocity;
|
|
|
|
if (Math.Abs(linearInput) < linearDeadzone)
|
|
linearInput = 0.0;
|
|
else
|
|
{
|
|
// Normalize after deadzone
|
|
var sign = Math.Sign(linearInput);
|
|
linearInput = (sign * ((Math.Abs(linearInput) - linearDeadzone) / (1.0 - linearDeadzone)));
|
|
}
|
|
|
|
if (Math.Abs(angularInput) < angularDeadzone)
|
|
angularInput = 0.0;
|
|
else
|
|
{
|
|
// Normalize after deadzone
|
|
var sign = Math.Sign(angularInput);
|
|
angularInput = (sign * ((Math.Abs(angularInput) - angularDeadzone) / (1.0 - angularDeadzone)));
|
|
}
|
|
|
|
// Calculate final velocities
|
|
var linearVelocity = _config.MinLinearVelocity + linearInput * linearVelocityRange * speedFactor;
|
|
var angularVelocity = _config.MinAngularVelocity + angularInput * angularVelocityRange * speedFactor;
|
|
|
|
twist.Linear = new Vector3(linearVelocity, 0, 0);
|
|
twist.Angular = new Vector3(0, 0, angularVelocity);
|
|
|
|
return twist;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Synchronous version of HandleLiftModuleControlAsync for use in high-priority thread
|
|
/// Uses timeout to prevent blocking when CPU load is high
|
|
/// </summary>
|
|
private void HandleLiftModuleControl(CancellationToken cancellationToken)
|
|
{
|
|
// Check if lift module is ready
|
|
if (_rfHandle == null || _liftModule.State != LiftModuleState.Ready)
|
|
return;
|
|
|
|
try
|
|
{
|
|
// Handle lift up
|
|
if (_rfHandle.LiftUp)
|
|
{
|
|
var task = _liftModule.LiftUpAsync(cancellationToken);
|
|
|
|
if (!task.Wait(TimeSpan.FromMilliseconds(200), CancellationToken.None))
|
|
{
|
|
_logger.LogWarning("Timeout handling lift up (200ms) - CPU may be overloaded");
|
|
}
|
|
}
|
|
// Handle lift down
|
|
else if (_rfHandle.LiftDown)
|
|
{
|
|
var task = _liftModule.LiftDownAsync(cancellationToken);
|
|
|
|
if (!task.Wait(TimeSpan.FromMilliseconds(200), CancellationToken.None))
|
|
{
|
|
_logger.LogWarning("Timeout handling lift down (200ms) - CPU may be overloaded");
|
|
}
|
|
}
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
throw;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error handling lift module control");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Synchronous version of HandleRotationModuleControlAsync for use in high-priority thread
|
|
/// Uses timeout to prevent blocking when CPU load is high
|
|
/// </summary>
|
|
private void HandleRotationModuleControl(CancellationToken cancellationToken)
|
|
{
|
|
// Check if rotation module is ready
|
|
if (_rfHandle == null || _rotationModule.State != RotationModuleState.Ready)
|
|
return;
|
|
|
|
try
|
|
{
|
|
// Handle rotate left (90 degrees)
|
|
if (_rfHandle.RotateLeft)
|
|
{
|
|
var task = _rotationModule.RotateOffsetAsync(90.0, cancellationToken);
|
|
|
|
if (!task.Wait(TimeSpan.FromMilliseconds(200), CancellationToken.None))
|
|
{
|
|
_logger.LogWarning("Timeout handling rotate left (200ms) - CPU may be overloaded");
|
|
}
|
|
}
|
|
// Handle rotate right (-90 degrees)
|
|
else if (_rfHandle.RotateRight)
|
|
{
|
|
var task = _rotationModule.RotateOffsetAsync(-90.0, cancellationToken);
|
|
|
|
if (!task.Wait(TimeSpan.FromMilliseconds(200), CancellationToken.None))
|
|
{
|
|
_logger.LogWarning("Timeout handling rotate right (200ms) - CPU may be overloaded");
|
|
}
|
|
}
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
throw;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error handling rotation module control");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Update velocity from keyboard input
|
|
/// </summary>
|
|
private void UpdateVelocityFromKeyboard(CancellationToken cancellationToken)
|
|
{
|
|
if (!_keyboardReady)
|
|
return;
|
|
|
|
// Calculate velocity from keyboard
|
|
var twist = CalculateTwistFromKeyboard();
|
|
|
|
// Update current twist (minimal lock time)
|
|
lock (_lock)
|
|
{
|
|
_currentTwist = twist;
|
|
}
|
|
|
|
// Send to IInverseKinematics (synchronous)
|
|
try
|
|
{
|
|
var task = _inverseKinematics.SetVelocityAsync(twist, cancellationToken);
|
|
if (!task.Wait(TimeSpan.FromMilliseconds(50), CancellationToken.None))
|
|
{
|
|
_logger.LogWarning("Timeout sending velocity to IInverseKinematics (50ms) - CPU may be overloaded");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error sending velocity to IInverseKinematics");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculate Twist from keyboard input
|
|
/// W/S: Linear forward/backward
|
|
/// A/D: Angular left/right
|
|
/// </summary>
|
|
private Twist CalculateTwistFromKeyboard()
|
|
{
|
|
var twist = new Twist();
|
|
|
|
// Get key states
|
|
bool forward, backward, left, right;
|
|
int speedPercent;
|
|
|
|
lock (_lock)
|
|
{
|
|
forward = _keyStates.GetValueOrDefault(ConsoleKey.W, false);
|
|
backward = _keyStates.GetValueOrDefault(ConsoleKey.S, false);
|
|
left = _keyStates.GetValueOrDefault(ConsoleKey.A, false);
|
|
right = _keyStates.GetValueOrDefault(ConsoleKey.D, false);
|
|
speedPercent = _keyboardSpeed;
|
|
}
|
|
|
|
// Calculate linear input (-1.0 to 1.0)
|
|
double linearInput = 0.0;
|
|
if (forward && !backward) linearInput = 1.0;
|
|
else if (backward && !forward) linearInput = -1.0;
|
|
|
|
// Calculate angular input (-1.0 to 1.0)
|
|
double angularInput = 0.0;
|
|
if (right && !left) angularInput = 1.0;
|
|
else if (left && !right) angularInput = -1.0;
|
|
|
|
// Get speed percentage (0-100)
|
|
if (speedPercent < 0) speedPercent = 0;
|
|
if (speedPercent > 100) speedPercent = 100;
|
|
|
|
// Calculate velocity range based on speed percentage
|
|
var linearVelocityRange = _config.MaxLinearVelocity - _config.MinLinearVelocity;
|
|
var angularVelocityRange = _config.MaxAngularVelocity - _config.MinAngularVelocity;
|
|
var speedFactor = speedPercent / 100.0;
|
|
|
|
// Apply deadzone (min velocity threshold)
|
|
var linearDeadzone = _config.MinLinearVelocity / _config.MaxLinearVelocity;
|
|
var angularDeadzone = _config.MinAngularVelocity / _config.MaxAngularVelocity;
|
|
|
|
if (Math.Abs(linearInput) < linearDeadzone)
|
|
linearInput = 0.0;
|
|
else
|
|
{
|
|
// Normalize after deadzone
|
|
var sign = Math.Sign(linearInput);
|
|
linearInput = (sign * ((Math.Abs(linearInput) - linearDeadzone) / (1.0 - linearDeadzone)));
|
|
}
|
|
|
|
if (Math.Abs(angularInput) < angularDeadzone)
|
|
angularInput = 0.0;
|
|
else
|
|
{
|
|
// Normalize after deadzone
|
|
var sign = Math.Sign(angularInput);
|
|
angularInput = (sign * ((Math.Abs(angularInput) - angularDeadzone) / (1.0 - angularDeadzone)));
|
|
}
|
|
|
|
// Calculate final velocities
|
|
var linearVelocity = _config.MinLinearVelocity + linearInput * linearVelocityRange * speedFactor;
|
|
var angularVelocity = _config.MinAngularVelocity + angularInput * angularVelocityRange * speedFactor;
|
|
|
|
twist.Linear = new Vector3(linearVelocity, 0, 0);
|
|
twist.Angular = new Vector3(0, 0, angularVelocity);
|
|
|
|
return twist;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Handle lift module control from keyboard
|
|
/// Q: Lift up, Z: Lift down
|
|
/// </summary>
|
|
private void HandleLiftModuleControlFromKeyboard(CancellationToken cancellationToken)
|
|
{
|
|
// Check if lift module is ready
|
|
if (_liftModule.State != LiftModuleState.Ready)
|
|
return;
|
|
|
|
bool liftUp, liftDown;
|
|
lock (_lock)
|
|
{
|
|
liftUp = _keyStates.GetValueOrDefault(ConsoleKey.Q, false);
|
|
liftDown = _keyStates.GetValueOrDefault(ConsoleKey.Z, false);
|
|
}
|
|
|
|
try
|
|
{
|
|
// Handle lift up
|
|
if (liftUp)
|
|
{
|
|
var task = _liftModule.LiftUpAsync(cancellationToken);
|
|
|
|
if (!task.Wait(TimeSpan.FromMilliseconds(200), CancellationToken.None))
|
|
{
|
|
_logger.LogWarning("Timeout handling lift up (200ms) - CPU may be overloaded");
|
|
}
|
|
}
|
|
// Handle lift down
|
|
else if (liftDown)
|
|
{
|
|
var task = _liftModule.LiftDownAsync(cancellationToken);
|
|
|
|
if (!task.Wait(TimeSpan.FromMilliseconds(200), CancellationToken.None))
|
|
{
|
|
_logger.LogWarning("Timeout handling lift down (200ms) - CPU may be overloaded");
|
|
}
|
|
}
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
throw;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error handling lift module control from keyboard");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Handle rotation module control from keyboard
|
|
/// E: Rotate left, C: Rotate right
|
|
/// </summary>
|
|
private void HandleRotationModuleControlFromKeyboard(CancellationToken cancellationToken)
|
|
{
|
|
// Check if rotation module is ready
|
|
if (_rotationModule.State != RotationModuleState.Ready)
|
|
return;
|
|
|
|
bool rotateLeft, rotateRight;
|
|
lock (_lock)
|
|
{
|
|
rotateLeft = _keyStates.GetValueOrDefault(ConsoleKey.E, false);
|
|
rotateRight = _keyStates.GetValueOrDefault(ConsoleKey.C, false);
|
|
}
|
|
|
|
try
|
|
{
|
|
// Handle rotate left (90 degrees)
|
|
if (rotateLeft)
|
|
{
|
|
var task = _rotationModule.RotateOffsetAsync(90.0, cancellationToken);
|
|
|
|
if (!task.Wait(TimeSpan.FromMilliseconds(200), CancellationToken.None))
|
|
{
|
|
_logger.LogWarning("Timeout handling rotate left (200ms) - CPU may be overloaded");
|
|
}
|
|
}
|
|
// Handle rotate right (-90 degrees)
|
|
else if (rotateRight)
|
|
{
|
|
var task = _rotationModule.RotateOffsetAsync(-90.0, cancellationToken);
|
|
|
|
if (!task.Wait(TimeSpan.FromMilliseconds(200), CancellationToken.None))
|
|
{
|
|
_logger.LogWarning("Timeout handling rotate right (200ms) - CPU may be overloaded");
|
|
}
|
|
}
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
throw;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error handling rotation module control from keyboard");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Start keyboard listener thread
|
|
/// Đọc keyboard input và cập nhật _keyStates
|
|
/// </summary>
|
|
private void StartKeyboardListener(CancellationToken cancellationToken)
|
|
{
|
|
if (_keyboardThread != null && _keyboardThread.IsAlive)
|
|
{
|
|
_logger.LogWarning("Keyboard listener already running");
|
|
return;
|
|
}
|
|
|
|
_keyboardReady = false;
|
|
|
|
// Create keyboard listener thread based on OS
|
|
if (OperatingSystem.IsWindows())
|
|
{
|
|
StartWindowsKeyboardListener(cancellationToken);
|
|
}
|
|
else if (OperatingSystem.IsLinux())
|
|
{
|
|
StartLinuxKeyboardListener(cancellationToken);
|
|
}
|
|
else
|
|
{
|
|
_logger.LogError("Keyboard input not supported on {OS}", Environment.OSVersion.Platform);
|
|
throw new PlatformNotSupportedException($"Keyboard input not supported on {Environment.OSVersion.Platform}");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Start Windows keyboard listener using Console.ReadKey
|
|
/// TODO: Replace with low-level keyboard hook (SetWindowsHookEx) for better performance
|
|
/// </summary>
|
|
private void StartWindowsKeyboardListener(CancellationToken cancellationToken)
|
|
{
|
|
_keyboardThread = new Thread(() =>
|
|
{
|
|
try
|
|
{
|
|
_keyboardReady = true;
|
|
_logger.LogInformation("Windows keyboard listener started");
|
|
|
|
while (!cancellationToken.IsCancellationRequested)
|
|
{
|
|
if (Console.KeyAvailable)
|
|
{
|
|
var keyInfo = Console.ReadKey(intercept: true);
|
|
|
|
lock (_lock)
|
|
{
|
|
// Set key state to true
|
|
_keyStates[keyInfo.Key] = true;
|
|
|
|
// Handle speed adjustment
|
|
if (keyInfo.Key == ConsoleKey.Add || keyInfo.KeyChar == '+')
|
|
{
|
|
_keyboardSpeed = Math.Min(100, _keyboardSpeed + 10);
|
|
_logger.LogDebug("Speed increased to {Speed}%", _keyboardSpeed);
|
|
}
|
|
else if (keyInfo.Key == ConsoleKey.Subtract || keyInfo.KeyChar == '-')
|
|
{
|
|
_keyboardSpeed = Math.Max(0, _keyboardSpeed - 10);
|
|
_logger.LogDebug("Speed decreased to {Speed}%", _keyboardSpeed);
|
|
}
|
|
}
|
|
|
|
// Clear key state after short delay (simulate key release)
|
|
Task.Delay(100, cancellationToken).ContinueWith(t =>
|
|
{
|
|
if (!t.IsCanceled)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
_keyStates[keyInfo.Key] = false;
|
|
}
|
|
}
|
|
}, cancellationToken);
|
|
}
|
|
else
|
|
{
|
|
Thread.Sleep(10);
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error in Windows keyboard listener");
|
|
_keyboardReady = false;
|
|
}
|
|
})
|
|
{
|
|
Name = "KeyboardListener-Windows",
|
|
IsBackground = true
|
|
};
|
|
|
|
_keyboardThread.Start();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Find keyboard device among /dev/input/event* devices
|
|
/// Returns file descriptor or -1 if not found
|
|
/// </summary>
|
|
private int FindKeyboardDevice()
|
|
{
|
|
for (int i = 0; i < INPUT_DEVICE_MAX; i++)
|
|
{
|
|
string devicePath = $"/dev/input/event{i}";
|
|
|
|
int fd = EvdevNative.open(devicePath, EvdevNative.O_RDONLY | EvdevNative.O_NONBLOCK);
|
|
if (fd < 0)
|
|
continue;
|
|
|
|
try
|
|
{
|
|
byte[] nameBuffer = new byte[256];
|
|
int result = EvdevNative.ioctl(fd, EvdevNative.EVIOCGNAME_256, nameBuffer);
|
|
|
|
if (result >= 0)
|
|
{
|
|
string deviceName = Encoding.UTF8.GetString(nameBuffer, 0, result).TrimEnd('\0');
|
|
_logger.LogDebug("Found input device {Device}: {Name}", devicePath, deviceName);
|
|
|
|
if (deviceName.Contains("keyboard", StringComparison.OrdinalIgnoreCase) ||
|
|
deviceName.Contains("kbd", StringComparison.OrdinalIgnoreCase))
|
|
{
|
|
_logger.LogInformation("Selected keyboard device: {Device} ({Name})", devicePath, deviceName);
|
|
return fd;
|
|
}
|
|
}
|
|
|
|
EvdevNative.close(fd);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogDebug(ex, "Error querying device {Device}", devicePath);
|
|
EvdevNative.close(fd);
|
|
}
|
|
}
|
|
|
|
return -1;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Process a key event from evdev
|
|
/// </summary>
|
|
private void ProcessKeyEvent(ushort keyCode, int value)
|
|
{
|
|
if (!LinuxKeyToConsoleKey.TryGetValue(keyCode, out ConsoleKey consoleKey))
|
|
return;
|
|
|
|
lock (_lock)
|
|
{
|
|
if (value == EvdevNative.KEY_PRESS || value == EvdevNative.KEY_REPEAT)
|
|
{
|
|
bool wasAlreadyPressed = _keyStates.GetValueOrDefault(consoleKey, false);
|
|
_keyStates[consoleKey] = true;
|
|
|
|
if (!wasAlreadyPressed)
|
|
{
|
|
if (consoleKey == ConsoleKey.Add || keyCode == EvdevNative.KEY_EQUAL)
|
|
{
|
|
_keyboardSpeed = Math.Min(100, _keyboardSpeed + 10);
|
|
_logger.LogDebug("Speed increased to {Speed}%", _keyboardSpeed);
|
|
}
|
|
else if (consoleKey == ConsoleKey.Subtract || keyCode == EvdevNative.KEY_MINUS)
|
|
{
|
|
_keyboardSpeed = Math.Max(0, _keyboardSpeed - 10);
|
|
_logger.LogDebug("Speed decreased to {Speed}%", _keyboardSpeed);
|
|
}
|
|
}
|
|
}
|
|
else if (value == EvdevNative.KEY_RELEASE)
|
|
{
|
|
_keyStates[consoleKey] = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Fallback keyboard listener using Console.ReadKey
|
|
/// </summary>
|
|
private void FallbackToConsoleReadKey(CancellationToken cancellationToken)
|
|
{
|
|
_keyboardReady = true;
|
|
|
|
while (!cancellationToken.IsCancellationRequested)
|
|
{
|
|
if (Console.KeyAvailable)
|
|
{
|
|
var keyInfo = Console.ReadKey(intercept: true);
|
|
|
|
lock (_lock)
|
|
{
|
|
_keyStates[keyInfo.Key] = true;
|
|
|
|
if (keyInfo.Key == ConsoleKey.Add || keyInfo.KeyChar == '+')
|
|
{
|
|
_keyboardSpeed = Math.Min(100, _keyboardSpeed + 10);
|
|
_logger.LogDebug("Speed increased to {Speed}%", _keyboardSpeed);
|
|
}
|
|
else if (keyInfo.Key == ConsoleKey.Subtract || keyInfo.KeyChar == '-')
|
|
{
|
|
_keyboardSpeed = Math.Max(0, _keyboardSpeed - 10);
|
|
_logger.LogDebug("Speed decreased to {Speed}%", _keyboardSpeed);
|
|
}
|
|
}
|
|
|
|
Task.Delay(100, cancellationToken).ContinueWith(t =>
|
|
{
|
|
if (!t.IsCanceled)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
_keyStates[keyInfo.Key] = false;
|
|
}
|
|
}
|
|
}, cancellationToken);
|
|
}
|
|
else
|
|
{
|
|
Thread.Sleep(10);
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Start Linux keyboard listener reading from /dev/input/event*
|
|
/// Falls back to Console.ReadKey if evdev unavailable
|
|
/// </summary>
|
|
private void StartLinuxKeyboardListener(CancellationToken cancellationToken)
|
|
{
|
|
_keyboardThread = new Thread(() =>
|
|
{
|
|
try
|
|
{
|
|
_evdevFd = FindKeyboardDevice();
|
|
|
|
if (_evdevFd >= 0)
|
|
{
|
|
_logger.LogInformation("Linux evdev keyboard listener started (fd={Fd})", _evdevFd);
|
|
_keyboardReady = true;
|
|
|
|
var inputEvent = new EvdevNative.input_event();
|
|
int eventSize = Marshal.SizeOf<EvdevNative.input_event>();
|
|
|
|
while (!cancellationToken.IsCancellationRequested)
|
|
{
|
|
int bytesRead = EvdevNative.read(_evdevFd, ref inputEvent, eventSize);
|
|
|
|
if (bytesRead < 0)
|
|
{
|
|
int errno = EvdevNative.GetLastError();
|
|
|
|
if (errno == EvdevNative.EAGAIN)
|
|
{
|
|
Thread.Sleep(10);
|
|
continue;
|
|
}
|
|
|
|
_logger.LogError("Error reading evdev (errno={Errno})", errno);
|
|
break;
|
|
}
|
|
|
|
if (bytesRead != eventSize)
|
|
{
|
|
_logger.LogWarning("Partial read: {Bytes}/{Expected} bytes", bytesRead, eventSize);
|
|
continue;
|
|
}
|
|
|
|
if (inputEvent.type == EvdevNative.EV_KEY)
|
|
{
|
|
ProcessKeyEvent(inputEvent.code, inputEvent.value);
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
_logger.LogWarning(
|
|
"Linux evdev not available (no keyboard found or permission denied). " +
|
|
"Using Console.ReadKey fallback. " +
|
|
"To use evdev: sudo usermod -aG input $USER && reboot");
|
|
|
|
FallbackToConsoleReadKey(cancellationToken);
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error in Linux keyboard listener");
|
|
_keyboardReady = false;
|
|
}
|
|
finally
|
|
{
|
|
if (_evdevFd >= 0)
|
|
{
|
|
EvdevNative.close(_evdevFd);
|
|
_evdevFd = -1;
|
|
}
|
|
}
|
|
})
|
|
{
|
|
Name = "KeyboardListener-Linux-Evdev",
|
|
IsBackground = true
|
|
};
|
|
|
|
_keyboardThread.Start();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Stop keyboard listener thread
|
|
/// </summary>
|
|
private void StopKeyboardListener()
|
|
{
|
|
_keyboardReady = false;
|
|
|
|
if (_keyboardThread != null && _keyboardThread.IsAlive)
|
|
{
|
|
try
|
|
{
|
|
if (!_keyboardThread.Join(TimeSpan.FromSeconds(2)))
|
|
{
|
|
_logger.LogWarning("Timeout waiting for keyboard listener thread to stop");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error stopping keyboard listener thread");
|
|
}
|
|
|
|
_keyboardThread = null;
|
|
}
|
|
|
|
lock (_lock)
|
|
{
|
|
_keyStates.Clear();
|
|
}
|
|
}
|
|
|
|
private void StopRobot()
|
|
{
|
|
var zeroTwist = new Twist();
|
|
lock (_lock)
|
|
{
|
|
_currentTwist = zeroTwist;
|
|
}
|
|
|
|
// Send zero velocity to IInverseKinematics if available
|
|
try
|
|
{
|
|
var task = _inverseKinematics.SetVelocityAsync(zeroTwist);
|
|
if (!task.Wait(TimeSpan.FromMilliseconds(50), CancellationToken.None))
|
|
{
|
|
_logger.LogWarning("Timeout stopping robot (50ms) - CPU may be overloaded");
|
|
}
|
|
}
|
|
catch (AggregateException ex)
|
|
{
|
|
_logger.LogError(ex.InnerException ?? ex, "Error stopping robot");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error stopping robot");
|
|
}
|
|
}
|
|
|
|
#region IHostedService
|
|
|
|
/// <summary>
|
|
/// IHostedService.StartAsync - Chỉ khởi tạo phần cứng (device, keyboard)
|
|
/// KHÔNG start update loop - module manager sẽ gọi Start() khi sẵn sàng
|
|
/// </summary>
|
|
public async Task StartAsync(CancellationToken cancellationToken)
|
|
{
|
|
try
|
|
{
|
|
if (!_config.Enable) return;
|
|
|
|
if (_config.UsingKeyboard)
|
|
{
|
|
// Start keyboard listener thread
|
|
StartKeyboardListener(cancellationToken);
|
|
_logger.LogInformation("Keyboard input initialized successfully. Keys: W/S (linear), A/D (angular), Q/Z (lift), E/C (rotate), +/- (speed), Space (stop)");
|
|
|
|
// Wait for modules to be ready
|
|
if(_liftModule.Enable)
|
|
{
|
|
while (!_liftModule.IsReady && !cancellationToken.IsCancellationRequested)
|
|
{
|
|
await Task.Delay(500, CancellationToken.None);
|
|
}
|
|
}
|
|
|
|
if(_rotationModule.Enable)
|
|
{
|
|
while (!_rotationModule.IsReady && !cancellationToken.IsCancellationRequested)
|
|
{
|
|
await Task.Delay(500, CancellationToken.None);
|
|
}
|
|
}
|
|
_currentState = ManualControlState.Active;
|
|
Start();
|
|
/*if (_liftModule.IsReady && _rotationModule.IsReady && !cancellationToken.IsCancellationRequested)
|
|
{
|
|
Start();
|
|
}*/
|
|
}
|
|
else
|
|
{
|
|
// Đợi DeviceProvider kết nối xong tất cả devices
|
|
var connected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromMinutes(5), cancellationToken);
|
|
|
|
if (!connected)
|
|
{
|
|
_logger.LogWarning("Timeout waiting for devices to connect. ManualControlService will not be initialized.");
|
|
return;
|
|
}
|
|
|
|
// Get RF Handle device
|
|
var device = _deviceProvider.GetDevice(_config.RfHandleDeviceId);
|
|
if (device is not IRfHandle rfHandle)
|
|
{
|
|
_logger.LogError("RF Handle device '{DeviceId}' is not an IRfHandle", _config.RfHandleDeviceId);
|
|
return;
|
|
}
|
|
|
|
_rfHandle = rfHandle;
|
|
_rfHandle.Updated += OnRfHandleUpdated;
|
|
}
|
|
_isRunning = true;
|
|
_logger.LogInformation("ManualControlService hardware initialized. Waiting for Start() to begin update loop.");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error initializing ManualControlService hardware");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// IHostedService.StopAsync - Cleanup khi ứng dụng tắt
|
|
/// </summary>
|
|
public Task StopAsync(CancellationToken cancellationToken)
|
|
{
|
|
try
|
|
{
|
|
if (!_config.Enable) return Task.CompletedTask;
|
|
|
|
Stop();
|
|
|
|
if (_config.UsingKeyboard)
|
|
{
|
|
StopKeyboardListener();
|
|
}
|
|
else if (_rfHandle != null)
|
|
{
|
|
_rfHandle.Updated -= OnRfHandleUpdated;
|
|
}
|
|
|
|
// Stop update thread and wait for it to finish
|
|
StopUpdateLoop();
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "ManualControlService: Error during StopAsync()");
|
|
throw;
|
|
}
|
|
|
|
return Task.CompletedTask;
|
|
}
|
|
|
|
private void OnRfHandleUpdated()
|
|
{
|
|
// State is now determined in UpdateLoop based on RF Handle data
|
|
// This event can be used for external notifications if needed
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Public Control Methods
|
|
|
|
/// <summary>
|
|
/// Bắt đầu update loop - được gọi bởi module manager khi hệ thống sẵn sàng
|
|
/// </summary>
|
|
public void Start()
|
|
{
|
|
if (!_config.Enable) return;
|
|
|
|
if (_updateThread != null && _updateThread.IsAlive)
|
|
{
|
|
_logger.LogDebug("ManualControlService: Update loop already running");
|
|
return;
|
|
}
|
|
|
|
StartUpdateLoop();
|
|
_logger.LogInformation("ManualControlService: Update loop started");
|
|
}
|
|
|
|
/// <summary>
|
|
/// Dừng update loop và stop robot - được gọi bởi module manager
|
|
/// </summary>
|
|
public void Stop()
|
|
{
|
|
if (!_config.Enable) return;
|
|
|
|
_logger.LogInformation("ManualControlService: Stopping update loop");
|
|
StopUpdateLoop();
|
|
StopRobot();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Kiểm tra update loop có đang chạy không (thread-safe)
|
|
/// </summary>
|
|
public bool IsRunning => _isRunning;
|
|
|
|
/// <summary>
|
|
/// Sets the ManualControl state externally (called by RobotStateMachine)
|
|
/// Only Maintenance and Override states can be set externally
|
|
/// To exit these states, call with null or let RF Handle conditions reset it
|
|
/// </summary>
|
|
public void SetState(ManualControlState? state)
|
|
{
|
|
if (state.HasValue && state.Value != ManualControlState.Maintenance && state.Value != ManualControlState.Override)
|
|
{
|
|
_logger.LogWarning("SetState called with invalid state {State}. Only Maintenance and Override can be set externally.", state);
|
|
return;
|
|
}
|
|
|
|
lock (_lock)
|
|
{
|
|
var previousExternalState = _externallySetState;
|
|
_externallySetState = state;
|
|
|
|
if (state.HasValue)
|
|
{
|
|
_logger.LogInformation("ManualControlService state externally set to {State}", state.Value);
|
|
}
|
|
else if (previousExternalState.HasValue)
|
|
{
|
|
_logger.LogInformation("ManualControlService external state cleared (was {PreviousState})", previousExternalState.Value);
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Clears externally set state, allowing state to be determined by RF Handle data
|
|
/// Called by RobotStateMachine when exiting Service or Remote_Override states
|
|
/// </summary>
|
|
public void ClearExternalState()
|
|
{
|
|
SetState(null);
|
|
// Reset _previousRfMode so next UpdateFromDeviceLoop cycle detects current RF mode as "changed"
|
|
// This ensures re-entry to Service/Remote_Override after Stop release or mode exit
|
|
lock (_lock)
|
|
{
|
|
_previousRfMode = RFMode.None;
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region IDisposable
|
|
|
|
public void Dispose()
|
|
{
|
|
if (_disposed)
|
|
return;
|
|
|
|
_disposed = true;
|
|
|
|
try
|
|
{
|
|
if (_config.Enable)
|
|
{
|
|
StopUpdateLoop();
|
|
StopRobot();
|
|
|
|
// Stop keyboard listener if using keyboard
|
|
if (_config.UsingKeyboard)
|
|
{
|
|
StopKeyboardListener();
|
|
}
|
|
|
|
// Unsubscribe from RF Handle events
|
|
if (_rfHandle != null)
|
|
{
|
|
_rfHandle.Updated -= OnRfHandleUpdated;
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error disposing ManualControlService");
|
|
}
|
|
GC.SuppressFinalize(this);
|
|
}
|
|
|
|
#endregion
|
|
}
|
|
|
|
/// <summary>
|
|
/// Status của RF Handle (for SignalR)
|
|
/// </summary>
|
|
public class RfHandleStatus
|
|
{
|
|
public int Heartbeat { get; set; }
|
|
public bool Ready { get; set; }
|
|
public bool Locked { get; set; }
|
|
public bool EStop { get; set; }
|
|
public bool Enable { get; set; }
|
|
public int Speed { get; set; }
|
|
public double Linear { get; set; }
|
|
public double Angular { get; set; }
|
|
public string Mode { get; set; } = string.Empty;
|
|
public DateTime LastUpdateTime { get; set; }
|
|
}
|
|
|