Initial commit
This commit is contained in:
@@ -0,0 +1,698 @@
|
||||
using Appccelerate.StateMachine;
|
||||
using Appccelerate.StateMachine.Machine;
|
||||
using Microsoft.Extensions.Configuration;
|
||||
using Microsoft.Extensions.Hosting;
|
||||
using Microsoft.Extensions.Logging;
|
||||
using RobotNet10.RobotApp.Motion;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
// using RobotNet10.Shared.Numbers;
|
||||
using SDL2;
|
||||
using System;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace RobotNet10.RobotApp.Motion;
|
||||
|
||||
/// <summary>
|
||||
/// States cho PS5ControllerService state machine
|
||||
/// </summary>
|
||||
public enum PS5ControllerState
|
||||
{
|
||||
Disabled,
|
||||
Active
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Triggers cho PS5ControllerService state machine
|
||||
/// </summary>
|
||||
public enum PS5ControllerTrigger
|
||||
{
|
||||
Enable,
|
||||
Disable,
|
||||
SafetyStop
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Service điều khiển robot từ tay cầm PS5
|
||||
/// - R1, R2, L1, L2: Chọn mức vận tốc (0.3, 0.6, 0.9, 1.2 m/s)
|
||||
/// - Left stick (núm trái): Điều khiển lên/xuống (linear velocity)
|
||||
/// - Right stick (núm phải): Điều khiển trái/phải (angular velocity)
|
||||
/// </summary>
|
||||
public class PS5ControllerService : IHostedService, IDisposable
|
||||
{
|
||||
private readonly PassiveStateMachine<PS5ControllerState, PS5ControllerTrigger> _stateMachine;
|
||||
private readonly PS5ControllerConfiguration _config;
|
||||
private readonly IInverseKinematics? _inverseKinematics;
|
||||
private readonly ILogger<PS5ControllerService> _logger;
|
||||
private readonly object _lock = new();
|
||||
|
||||
private IntPtr _gamepad = IntPtr.Zero;
|
||||
private int _gamepadIndex = -1;
|
||||
private CancellationTokenSource? _updateCts;
|
||||
private Task? _updateTask;
|
||||
private PS5ControllerState _currentState = PS5ControllerState.Disabled;
|
||||
private bool _disposed = false;
|
||||
private bool _sdlInitialized = false;
|
||||
private bool _wasGamepadConnected = false;
|
||||
// Current velocity being sent to robot
|
||||
private Twist _currentTwist = new();
|
||||
|
||||
// Current selected velocity level (determined by trigger buttons)
|
||||
private double _currentMaxVelocity = 0.0;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the current state of the PS5ControllerService
|
||||
/// </summary>
|
||||
public PS5ControllerState State => _currentState;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the current twist being sent to robot
|
||||
/// </summary>
|
||||
public Twist CurrentTwist
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
return _currentTwist;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the current selected velocity level (m/s)
|
||||
/// </summary>
|
||||
public double CurrentMaxVelocity
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
return _currentMaxVelocity;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public PS5ControllerService(
|
||||
IConfiguration configuration,
|
||||
IInverseKinematics inverseKinematics,
|
||||
ILogger<PS5ControllerService> logger)
|
||||
{
|
||||
_logger = logger ?? throw new ArgumentNullException(nameof(logger));
|
||||
|
||||
// Try to get IInverseKinematics (optional - may not be available)
|
||||
_inverseKinematics = inverseKinematics;
|
||||
|
||||
// Load configuration
|
||||
var configSection = configuration.GetSection("Motion:PS5Controller");
|
||||
if (!configSection.Exists())
|
||||
{
|
||||
throw new InvalidOperationException("Configuration section 'Motion:PS5Controller' not found in appsettings.json");
|
||||
}
|
||||
|
||||
_config = new PS5ControllerConfiguration();
|
||||
configSection.Bind(_config);
|
||||
|
||||
// Validate configuration
|
||||
ValidateConfiguration();
|
||||
|
||||
// Build state machine
|
||||
_stateMachine = BuildStateMachine();
|
||||
_stateMachine.Start();
|
||||
}
|
||||
|
||||
private void ValidateConfiguration()
|
||||
{
|
||||
if (_config.R1Velocity <= 0)
|
||||
throw new InvalidOperationException("R1Velocity must be > 0");
|
||||
|
||||
if (_config.R2Velocity <= 0)
|
||||
throw new InvalidOperationException("R2Velocity must be > 0");
|
||||
|
||||
if (_config.L1Velocity <= 0)
|
||||
throw new InvalidOperationException("L1Velocity must be > 0");
|
||||
|
||||
if (_config.L2Velocity <= 0)
|
||||
throw new InvalidOperationException("L2Velocity must be > 0");
|
||||
|
||||
if (_config.MaxAngularVelocity <= 0)
|
||||
throw new InvalidOperationException("MaxAngularVelocity must be > 0");
|
||||
|
||||
if (_config.UpdateRate <= 0)
|
||||
throw new InvalidOperationException("UpdateRate must be > 0");
|
||||
}
|
||||
|
||||
private PassiveStateMachine<PS5ControllerState, PS5ControllerTrigger> BuildStateMachine()
|
||||
{
|
||||
var builder = new StateMachineDefinitionBuilder<PS5ControllerState, PS5ControllerTrigger>();
|
||||
|
||||
// Disabled state
|
||||
builder.In(PS5ControllerState.Disabled)
|
||||
.ExecuteOnEntry(() =>
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_currentState = PS5ControllerState.Disabled;
|
||||
}
|
||||
StopUpdateLoop();
|
||||
StopRobot();
|
||||
_logger.LogInformation("PS5ControllerService state: Disabled");
|
||||
})
|
||||
.On(PS5ControllerTrigger.Enable)
|
||||
.Goto(PS5ControllerState.Active);
|
||||
|
||||
// Active state
|
||||
builder.In(PS5ControllerState.Active)
|
||||
.ExecuteOnEntry(() =>
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_currentState = PS5ControllerState.Active;
|
||||
}
|
||||
_logger.LogInformation("PS5ControllerService state: Active");
|
||||
StartUpdateLoop();
|
||||
})
|
||||
.On(PS5ControllerTrigger.Disable)
|
||||
.Goto(PS5ControllerState.Disabled)
|
||||
.On(PS5ControllerTrigger.SafetyStop)
|
||||
.Goto(PS5ControllerState.Disabled)
|
||||
.Execute(() =>
|
||||
{
|
||||
_logger.LogWarning("PS5ControllerService: Safety stop triggered");
|
||||
});
|
||||
|
||||
return builder
|
||||
.WithInitialState(PS5ControllerState.Disabled)
|
||||
.Build()
|
||||
.CreatePassiveStateMachine();
|
||||
}
|
||||
|
||||
private void StartUpdateLoop()
|
||||
{
|
||||
StopUpdateLoop(); // Stop existing loop if any
|
||||
|
||||
if (_gamepad == IntPtr.Zero || !IsTrue(SDL.SDL_GameControllerGetAttached(_gamepad)))
|
||||
{
|
||||
if (!TryReconnectGamepad(false))
|
||||
{
|
||||
_logger.LogWarning("PS5ControllerService: No gamepad connected, cannot start update loop");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
lock (_lock)
|
||||
{
|
||||
_updateCts = new CancellationTokenSource();
|
||||
var token = _updateCts.Token;
|
||||
_updateTask = Task.Run(async () => await UpdateLoopAsync(token), token);
|
||||
}
|
||||
}
|
||||
|
||||
private void StopUpdateLoop()
|
||||
{
|
||||
Task? taskToWait = null;
|
||||
CancellationTokenSource? ctsToCancel = null;
|
||||
|
||||
lock (_lock)
|
||||
{
|
||||
ctsToCancel = _updateCts;
|
||||
taskToWait = _updateTask;
|
||||
_updateCts = null;
|
||||
_updateTask = null;
|
||||
}
|
||||
|
||||
if (ctsToCancel != null)
|
||||
{
|
||||
try
|
||||
{
|
||||
ctsToCancel.Cancel();
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
// CTS already disposed, ignore
|
||||
}
|
||||
}
|
||||
|
||||
if (taskToWait != null)
|
||||
{
|
||||
try
|
||||
{
|
||||
taskToWait.Wait(TimeSpan.FromSeconds(1));
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error stopping update loop");
|
||||
}
|
||||
}
|
||||
|
||||
ctsToCancel?.Dispose();
|
||||
}
|
||||
|
||||
private async Task UpdateLoopAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
var updateInterval = TimeSpan.FromMilliseconds(1000.0 / _config.UpdateRate);
|
||||
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
try
|
||||
{
|
||||
// Check if gamepad is still connected
|
||||
if (_gamepad == IntPtr.Zero || !IsTrue(SDL.SDL_GameControllerGetAttached(_gamepad)))
|
||||
{
|
||||
StopRobot();
|
||||
|
||||
if (!TryReconnectGamepad(false))
|
||||
{
|
||||
await Task.Delay(updateInterval, cancellationToken);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
// Update velocity from gamepad
|
||||
await UpdateVelocityFromGamepadAsync(cancellationToken);
|
||||
|
||||
await Task.Delay(updateInterval, cancellationToken);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
break;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error in update loop");
|
||||
await Task.Delay(updateInterval, cancellationToken);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private async Task UpdateVelocityFromGamepadAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
if (_gamepad == IntPtr.Zero)
|
||||
return;
|
||||
|
||||
// Update SDL events (required for gamepad state)
|
||||
SDL.SDL_PumpEvents();
|
||||
|
||||
// Get button states
|
||||
byte r1Pressed = SDL.SDL_GameControllerGetButton(_gamepad, SDL.SDL_GameControllerButton.SDL_CONTROLLER_BUTTON_RIGHTSHOULDER);
|
||||
byte r2Pressed = SDL.SDL_GameControllerGetAxis(_gamepad, SDL.SDL_GameControllerAxis.SDL_CONTROLLER_AXIS_TRIGGERRIGHT) > 0 ? (byte)1 : (byte)0;
|
||||
byte l1Pressed = SDL.SDL_GameControllerGetButton(_gamepad, SDL.SDL_GameControllerButton.SDL_CONTROLLER_BUTTON_LEFTSHOULDER);
|
||||
byte l2Pressed = SDL.SDL_GameControllerGetAxis(_gamepad, SDL.SDL_GameControllerAxis.SDL_CONTROLLER_AXIS_TRIGGERLEFT) > 0 ? (byte)1 : (byte)0;
|
||||
|
||||
// Determine velocity level based on trigger buttons (priority: L2 > L1 > R2 > R1)
|
||||
double maxVelocity = 0.0;
|
||||
//Velocity
|
||||
double linearVelocity = 0.0;
|
||||
double angularVelocity = 0.0;
|
||||
if (l2Pressed != 0)
|
||||
maxVelocity = _config.L2Velocity;
|
||||
else if (l1Pressed != 0)
|
||||
maxVelocity = _config.L1Velocity;
|
||||
else if (r2Pressed != 0)
|
||||
maxVelocity = _config.R2Velocity;
|
||||
else if (r1Pressed != 0)
|
||||
maxVelocity = _config.R1Velocity;
|
||||
else if( l2Pressed == 0 && l1Pressed == 0 && r2Pressed == 0 && r1Pressed == 0)
|
||||
StopRobot();
|
||||
// Console.WriteLine($"l2: {l2Pressed}, l1: {l1Pressed}, r2: {r2Pressed}, r1: {r1Pressed}, maxVelocity: {maxVelocity}, linearVelocity: {linearVelocity}");
|
||||
lock (_lock)
|
||||
{
|
||||
_currentMaxVelocity = maxVelocity;
|
||||
}
|
||||
|
||||
// Get stick values (left stick for linear, right stick for angular)
|
||||
short leftStickY = SDL.SDL_GameControllerGetAxis(_gamepad, SDL.SDL_GameControllerAxis.SDL_CONTROLLER_AXIS_LEFTY);
|
||||
short rightStickX = SDL.SDL_GameControllerGetAxis(_gamepad, SDL.SDL_GameControllerAxis.SDL_CONTROLLER_AXIS_RIGHTX);
|
||||
|
||||
// Normalize stick values from [-32768, 32767] to [-1.0, 1.0]
|
||||
// Left stick Y: up is negative, down is positive (invert for forward/backward)
|
||||
// Right stick X: left is negative, right is positive
|
||||
double leftStickYNormalized = -leftStickY / 32768.0; // Invert so up = positive (forward)
|
||||
double rightStickXNormalized = -rightStickX / 32768.0;
|
||||
|
||||
// Apply deadzone (5% deadzone)
|
||||
const double deadzone = 0.1;
|
||||
if (Math.Abs(leftStickYNormalized) < deadzone)
|
||||
leftStickYNormalized = 0.0;
|
||||
else
|
||||
{
|
||||
// Rescale after deadzone
|
||||
var sign = Math.Sign(leftStickYNormalized);
|
||||
leftStickYNormalized = sign * ((Math.Abs(leftStickYNormalized) - deadzone) / (1.0 - deadzone));
|
||||
}
|
||||
|
||||
if (Math.Abs(rightStickXNormalized) < deadzone)
|
||||
rightStickXNormalized = 0.0;
|
||||
else
|
||||
{
|
||||
// Rescale after deadzone
|
||||
var sign = Math.Sign(rightStickXNormalized);
|
||||
rightStickXNormalized = sign * ((Math.Abs(rightStickXNormalized) - deadzone) / (1.0 - deadzone));
|
||||
}
|
||||
|
||||
// Calculate velocities
|
||||
linearVelocity = leftStickYNormalized * maxVelocity;
|
||||
angularVelocity = rightStickXNormalized * _config.MaxAngularVelocity;
|
||||
// Console.WriteLine($"linearVelocity: {linearVelocity}, angularVelocity: {angularVelocity}");
|
||||
// Console.WriteLine($"leftStickYNormalized: {leftStickYNormalized}, maxVelocity: {maxVelocity}, linearVelocity: {linearVelocity}");
|
||||
// if(leftStickYNormalized == 0 || rightStickXNormalized == 0)
|
||||
// {
|
||||
// linearVelocity = 0;
|
||||
// angularVelocity = 0;
|
||||
// }
|
||||
// Build twist
|
||||
var twist = new Twist
|
||||
{
|
||||
Linear = new Vector3(linearVelocity, 0, 0),
|
||||
Angular = new Vector3(0, 0, angularVelocity)
|
||||
};
|
||||
|
||||
// Update current twist
|
||||
lock (_lock)
|
||||
{
|
||||
_currentTwist = twist;
|
||||
}
|
||||
|
||||
// Send to IInverseKinematics if available
|
||||
if (_inverseKinematics != null && maxVelocity > 0.0)
|
||||
{
|
||||
try
|
||||
{
|
||||
await _inverseKinematics.SetVelocityAsync(twist, cancellationToken);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error sending velocity to IInverseKinematics");
|
||||
}
|
||||
}
|
||||
|
||||
// Log occasionally for debugging
|
||||
if (DateTime.Now.Millisecond % 500 < 50) // Log roughly every 500ms
|
||||
{
|
||||
_logger.LogDebug("PS5Controller: MaxVel={MaxVel:F2}, Linear={Linear:F2}, Angular={Angular:F2}",
|
||||
maxVelocity, linearVelocity, angularVelocity);
|
||||
}
|
||||
}
|
||||
|
||||
private void StopRobot()
|
||||
{
|
||||
var zeroTwist = new Twist();
|
||||
lock (_lock)
|
||||
{
|
||||
_currentTwist = zeroTwist;
|
||||
_currentMaxVelocity = 0.0;
|
||||
}
|
||||
|
||||
// Send zero velocity to IInverseKinematics if available
|
||||
if (_inverseKinematics != null)
|
||||
{
|
||||
try
|
||||
{
|
||||
_ = Task.Run(async () => await _inverseKinematics.SetVelocityAsync(zeroTwist));
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error stopping robot");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private bool InitializeSDL()
|
||||
{
|
||||
if (_sdlInitialized)
|
||||
return true;
|
||||
|
||||
try
|
||||
{
|
||||
if (SDL.SDL_Init(SDL.SDL_INIT_GAMECONTROLLER) < 0)
|
||||
{
|
||||
_logger.LogError("Failed to initialize SDL: {Error}", SDL.SDL_GetError());
|
||||
return false;
|
||||
}
|
||||
|
||||
SDL.SDL_GameControllerEventState(SDL.SDL_ENABLE);
|
||||
_sdlInitialized = true;
|
||||
_logger.LogInformation("SDL initialized successfully for gamepad support");
|
||||
return true;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error initializing SDL");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
private bool OpenGamepad()
|
||||
{
|
||||
if (_gamepad != IntPtr.Zero)
|
||||
{
|
||||
if (IsTrue(SDL.SDL_GameControllerGetAttached(_gamepad)))
|
||||
return true;
|
||||
|
||||
SDL.SDL_GameControllerClose(_gamepad);
|
||||
_gamepad = IntPtr.Zero;
|
||||
_gamepadIndex = -1;
|
||||
}
|
||||
|
||||
SDL.SDL_PumpEvents();
|
||||
|
||||
int numJoysticks = SDL.SDL_NumJoysticks();
|
||||
_logger.LogDebug("Found {Count} joystick(s)", numJoysticks);
|
||||
|
||||
if (numJoysticks <= 0)
|
||||
return false;
|
||||
|
||||
// If GamepadIndex is configured, always try that exact index.
|
||||
if (_config.GamepadIndex.HasValue)
|
||||
{
|
||||
return TryOpenGamepadAtIndex(_config.GamepadIndex.Value);
|
||||
}
|
||||
|
||||
// Otherwise auto-detect first available game controller.
|
||||
for (int i = 0; i < numJoysticks; i++)
|
||||
{
|
||||
if (TryOpenGamepadAtIndex(i))
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
private bool TryOpenGamepadAtIndex(int index)
|
||||
{
|
||||
if (!IsTrue(SDL.SDL_IsGameController(index)))
|
||||
return false;
|
||||
|
||||
_gamepad = SDL.SDL_GameControllerOpen(index);
|
||||
if (_gamepad == IntPtr.Zero)
|
||||
{
|
||||
_logger.LogWarning("Failed to open game controller {Index}: {Error}", index, SDL.SDL_GetError());
|
||||
return false;
|
||||
}
|
||||
|
||||
_gamepadIndex = index;
|
||||
var name = SDL.SDL_GameControllerName(_gamepad);
|
||||
_logger.LogInformation("Opened game controller: {Name} (index {Index})", name, index);
|
||||
return true;
|
||||
}
|
||||
|
||||
private bool TryReconnectGamepad(bool logWhenUnavailable)
|
||||
{
|
||||
var reconnected = OpenGamepad();
|
||||
|
||||
if (reconnected)
|
||||
{
|
||||
if (!_wasGamepadConnected)
|
||||
_logger.LogInformation("PS5ControllerService: Gamepad connected/reconnected");
|
||||
|
||||
_wasGamepadConnected = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
if (_wasGamepadConnected)
|
||||
{
|
||||
_logger.LogWarning("PS5ControllerService: Gamepad disconnected, waiting for reconnect");
|
||||
}
|
||||
else if (logWhenUnavailable)
|
||||
{
|
||||
_logger.LogWarning("PS5ControllerService: No gamepad connected");
|
||||
}
|
||||
|
||||
_wasGamepadConnected = false;
|
||||
return false;
|
||||
}
|
||||
|
||||
private static bool IsTrue(SDL.SDL_bool value) => value == SDL.SDL_bool.SDL_TRUE;
|
||||
|
||||
#region IHostedService
|
||||
|
||||
public async Task StartAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
_logger.LogInformation("Starting PS5ControllerService...");
|
||||
|
||||
try
|
||||
{
|
||||
if (!InitializeSDL())
|
||||
{
|
||||
_logger.LogWarning("Failed to initialize SDL. PS5ControllerService will not be available.");
|
||||
return;
|
||||
}
|
||||
|
||||
if (!TryReconnectGamepad(true))
|
||||
{
|
||||
_logger.LogWarning("No gamepad found at startup. PS5ControllerService will auto-reconnect when gamepad is plugged in.");
|
||||
}
|
||||
|
||||
_logger.LogInformation("PS5ControllerService initialized successfully");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error starting PS5ControllerService");
|
||||
}
|
||||
|
||||
await Task.CompletedTask;
|
||||
}
|
||||
|
||||
public async Task StopAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
_logger.LogInformation("Stopping PS5ControllerService...");
|
||||
|
||||
try
|
||||
{
|
||||
Disable();
|
||||
StopUpdateLoop();
|
||||
|
||||
if (_gamepad != IntPtr.Zero)
|
||||
{
|
||||
SDL.SDL_GameControllerClose(_gamepad);
|
||||
_gamepad = IntPtr.Zero;
|
||||
_gamepadIndex = -1;
|
||||
}
|
||||
|
||||
if (_sdlInitialized)
|
||||
{
|
||||
SDL.SDL_QuitSubSystem(SDL.SDL_INIT_GAMECONTROLLER);
|
||||
_sdlInitialized = false;
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error stopping PS5ControllerService");
|
||||
}
|
||||
|
||||
await Task.CompletedTask;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Public Control Methods
|
||||
|
||||
/// <summary>
|
||||
/// Enable PS5 controller control
|
||||
/// </summary>
|
||||
public void Enable()
|
||||
{
|
||||
// Try to open gamepad if not already open
|
||||
if (_gamepad == IntPtr.Zero)
|
||||
{
|
||||
if (!TryReconnectGamepad(true))
|
||||
{
|
||||
_logger.LogWarning("Cannot enable PS5 controller: No gamepad connected");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// Check if gamepad is still connected
|
||||
if (!IsTrue(SDL.SDL_GameControllerGetAttached(_gamepad)))
|
||||
{
|
||||
_logger.LogWarning("Cannot enable PS5 controller: Gamepad not connected");
|
||||
return;
|
||||
}
|
||||
|
||||
// Check if IInverseKinematics (DifferentialDrive) is ready
|
||||
if (_inverseKinematics is DifferentialDrive differentialDrive)
|
||||
{
|
||||
if (differentialDrive.State != DifferentialDriveState.OperationEnabled)
|
||||
{
|
||||
_logger.LogInformation("DifferentialDrive is not in OperationEnabled state (current: {State}). Enabling it...", differentialDrive.State);
|
||||
|
||||
int maxAttempts = 10;
|
||||
int attemptDelay = 300; // ms
|
||||
|
||||
for (int i = 0; i < maxAttempts && differentialDrive.State != DifferentialDriveState.OperationEnabled; i++)
|
||||
{
|
||||
var currentState = differentialDrive.State;
|
||||
differentialDrive.Enable();
|
||||
|
||||
System.Threading.Thread.Sleep(attemptDelay);
|
||||
|
||||
if (differentialDrive.State == currentState && i > 0)
|
||||
{
|
||||
_logger.LogWarning("DifferentialDrive state did not change after Enable() call. State: {State}", currentState);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (differentialDrive.State != DifferentialDriveState.OperationEnabled)
|
||||
{
|
||||
_logger.LogWarning("Cannot enable PS5 controller: DifferentialDrive is not ready. Current state: {State}", differentialDrive.State);
|
||||
return;
|
||||
}
|
||||
|
||||
_logger.LogInformation("DifferentialDrive is now in OperationEnabled state");
|
||||
}
|
||||
}
|
||||
else if (_inverseKinematics == null)
|
||||
{
|
||||
_logger.LogWarning("Cannot enable PS5 controller: IInverseKinematics is not available");
|
||||
return;
|
||||
}
|
||||
|
||||
_stateMachine.Fire(PS5ControllerTrigger.Enable);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Disable PS5 controller control
|
||||
/// </summary>
|
||||
public void Disable()
|
||||
{
|
||||
_stateMachine.Fire(PS5ControllerTrigger.Disable);
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region IDisposable
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (_disposed)
|
||||
return;
|
||||
|
||||
_disposed = true;
|
||||
|
||||
try
|
||||
{
|
||||
Disable();
|
||||
StopUpdateLoop();
|
||||
_stateMachine.Stop();
|
||||
|
||||
if (_gamepad != IntPtr.Zero)
|
||||
{
|
||||
SDL.SDL_GameControllerClose(_gamepad);
|
||||
_gamepad = IntPtr.Zero;
|
||||
}
|
||||
|
||||
if (_sdlInitialized)
|
||||
{
|
||||
SDL.SDL_QuitSubSystem(SDL.SDL_INIT_GAMECONTROLLER);
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error disposing PS5ControllerService");
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user