JackM323
9c31de3c38
config into dataclass and enums new Gui that includes settings deleted GlobalVariables small fixes (import, names...)
101 lines
3.5 KiB
Python
101 lines
3.5 KiB
Python
"""
|
|
inputs/PygameController.py - Headless Pygame Joystick Provider
|
|
"""
|
|
import math
|
|
from multiprocessing import Queue
|
|
import pygame
|
|
|
|
from inputs.InputProvider import InputProvider, CommandFrame
|
|
|
|
|
|
class PygameController(InputProvider):
|
|
def __init__(self, deadzone: float = 0.2):
|
|
pygame.init()
|
|
pygame.joystick.init()
|
|
|
|
self.deadzone = deadzone
|
|
self.joysticks = {}
|
|
self.clock = pygame.time.Clock()
|
|
self.current_command = CommandFrame(state="idle", vector_dirmov=[0.0, 0.0, 0.0])
|
|
|
|
def _normalize_input(self, x: float, y: float) -> tuple[float, float]:
|
|
magnitude = math.sqrt(x**2 + y**2)
|
|
if magnitude > 1.0:
|
|
x /= magnitude
|
|
y /= magnitude
|
|
return x, y
|
|
|
|
def update(self) -> CommandFrame:
|
|
vx, vy, omega = 0.0, 0.0, 0.0
|
|
emote = ""
|
|
|
|
# Pump Pygame events in headless mode
|
|
for event in pygame.event.get():
|
|
if event.type == pygame.JOYDEVICEADDED:
|
|
joy = pygame.joystick.Joystick(event.device_index)
|
|
self.joysticks[joy.get_instance_id()] = joy
|
|
print(f"[Controller] Joystick {joy.get_instance_id()} connected: {joy.get_name()}")
|
|
elif event.type == pygame.JOYDEVICEREMOVED:
|
|
if event.instance_id in self.joysticks:
|
|
print(f"[Controller] Joystick {event.instance_id} disconnected")
|
|
del self.joysticks[event.instance_id]
|
|
elif event.type == pygame.JOYBUTTONDOWN:
|
|
if event.button == 3:
|
|
emote = "wave"
|
|
|
|
# Read stick values if joystick is connected
|
|
if self.joysticks:
|
|
for joystick in self.joysticks.values():
|
|
raw_vx = -joystick.get_axis(1) # Forward (+) / Backward (-)
|
|
raw_vy = joystick.get_axis(0) # Right (+) / Left (-)
|
|
|
|
num_axes = joystick.get_numaxes()
|
|
raw_omega = joystick.get_axis(3) if num_axes > 3 else joystick.get_axis(2)
|
|
|
|
norm_vx, norm_vy = self._normalize_input(raw_vx, raw_vy)
|
|
|
|
if abs(raw_omega) >= self.deadzone:
|
|
omega = raw_omega
|
|
if abs(norm_vx) >= self.deadzone or abs(norm_vy) >= self.deadzone:
|
|
vx, vy = norm_vx, norm_vy
|
|
|
|
# Cardinal snapping
|
|
if 0.8 < vx and -0.2 < vy < 0.2:
|
|
vx, vy = 1.0, 0.0
|
|
elif vx < -0.8 and -0.2 < vy < 0.2:
|
|
vx, vy = -1.0, 0.0
|
|
elif -0.2 < vx < 0.2 and 0.8 < vy:
|
|
vx, vy = 0.0, 1.0
|
|
elif -0.2 < vx < 0.2 and vy < -0.8:
|
|
vx, vy = 0.0, -1.0
|
|
|
|
state = "walking" if (vx != 0.0 or vy != 0.0 or omega != 0.0) else "idle"
|
|
|
|
self.current_command = CommandFrame(
|
|
state=state,
|
|
vector_dirmov=[vx, vy, omega],
|
|
emote=emote
|
|
)
|
|
return self.current_command
|
|
|
|
def get_command(self) -> CommandFrame:
|
|
return self.update()
|
|
|
|
def stop(self) -> None:
|
|
pygame.quit()
|
|
|
|
|
|
def controller_loop(control_queue: Queue):
|
|
provider = PygameController()
|
|
try:
|
|
while True:
|
|
cmd = provider.get_command()
|
|
control_queue.put({
|
|
"state": cmd.state,
|
|
"dirmov": cmd.vector_dirmov,
|
|
"emote": cmd.emote,
|
|
"joysticks_count": len(provider.joysticks)
|
|
})
|
|
provider.clock.tick(30)
|
|
finally:
|
|
provider.stop() |