699 lines
22 KiB
C#
699 lines
22 KiB
C#
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
|
|
}
|
|
|