merge conflict and readme update
old code was uploaded in the initial upload changed everything with working programm and updated the readme
This commit is contained in:
+179
-72
@@ -1,93 +1,200 @@
|
||||
import pygame
|
||||
import pygame
|
||||
import math
|
||||
import time
|
||||
import GlobalVariables as gv
|
||||
|
||||
import DataTypes as dt
|
||||
|
||||
def normalize(x, y, deadzone=0.15):
|
||||
if abs(x) < deadzone:
|
||||
x = 0.0
|
||||
if abs(y) < deadzone:
|
||||
y = 0.0
|
||||
|
||||
mag = math.hypot(x, y)
|
||||
if mag > 1.0:
|
||||
x /= mag
|
||||
y /= mag
|
||||
|
||||
return x, y
|
||||
|
||||
|
||||
def controller_loop(shared_intent: dt.ControlIntent, lock):
|
||||
def controller():
|
||||
pygame.init()
|
||||
pygame.joystick.init()
|
||||
|
||||
# This is a simple class that will help us print to the screen.
|
||||
# It has nothing to do with the joysticks, just outputting the
|
||||
# information.
|
||||
class TextPrint:
|
||||
def __init__(self):
|
||||
self.reset()
|
||||
self.font = pygame.font.Font(None, 25)
|
||||
|
||||
joysticks = {}
|
||||
prev_buttons = {}
|
||||
def tprint(self, screen, text):
|
||||
text_bitmap = self.font.render(text, True, (0, 0, 0))
|
||||
screen.blit(text_bitmap, (self.x, self.y))
|
||||
self.y += self.line_height
|
||||
|
||||
clock = pygame.time.Clock()
|
||||
def reset(self):
|
||||
self.x = 10
|
||||
self.y = 10
|
||||
self.line_height = 15
|
||||
|
||||
while True:
|
||||
# 🔑 REQUIRED: keeps joystick state updating
|
||||
pygame.event.pump()
|
||||
def indent(self):
|
||||
self.x += 10
|
||||
|
||||
for event in pygame.event.get():
|
||||
if event.type == pygame.JOYDEVICEADDED:
|
||||
joy = pygame.joystick.Joystick(event.device_index)
|
||||
joysticks[joy.get_instance_id()] = joy
|
||||
prev_buttons[joy.get_instance_id()] = [0] * joy.get_numbuttons()
|
||||
print(f"[Controller] Joystick connected: {joy.get_name()}")
|
||||
def unindent(self):
|
||||
self.x -= 10
|
||||
|
||||
elif event.type == pygame.JOYDEVICEREMOVED:
|
||||
joysticks.pop(event.instance_id, None)
|
||||
prev_buttons.pop(event.instance_id, None)
|
||||
print("[Controller] Joystick disconnected")
|
||||
|
||||
if not joysticks:
|
||||
time.sleep(0.1)
|
||||
continue
|
||||
def main():
|
||||
|
||||
def normalize_controller_input(x, y):
|
||||
# Berechne die Laenge des Vektors
|
||||
magnitude = math.sqrt(x**2 + y**2)
|
||||
|
||||
# Wenn die Laenge groesser als 1 ist, normalisiere sie
|
||||
if magnitude > 1.0:
|
||||
x /= magnitude
|
||||
y /= magnitude
|
||||
|
||||
return x, y
|
||||
|
||||
# Set the width and height of the screen (width, height), and name the window.
|
||||
screen = pygame.display.set_mode((500, 700))
|
||||
pygame.display.set_caption("Controller Inputs")
|
||||
|
||||
# Use first joystick
|
||||
joy = next(iter(joysticks.values()))
|
||||
jid = joy.get_instance_id()
|
||||
# Used to manage how fast the screen updates.
|
||||
clock = pygame.time.Clock()
|
||||
|
||||
# ---- AXES ----
|
||||
forward = -joy.get_axis(1) # usually inverted
|
||||
sideways = joy.get_axis(0)
|
||||
# Get ready to print.
|
||||
text_print = TextPrint()
|
||||
|
||||
forward, sideways = normalize(forward, sideways)
|
||||
# This dict can be left as-is, since pygame will generate a
|
||||
# pygame.JOYDEVICEADDED event for every joystick connected
|
||||
# at the start of the program.
|
||||
joysticks = {}
|
||||
|
||||
walk = abs(forward) > 0 or abs(sideways) > 0
|
||||
done = False
|
||||
while not done:
|
||||
# Event processing step.
|
||||
# Possible joystick events: JOYAXISMOTION, JOYBALLMOTION, JOYBUTTONDOWN,
|
||||
# JOYBUTTONUP, JOYHATMOTION, JOYDEVICEADDED, JOYDEVICEREMOVED
|
||||
for event in pygame.event.get():
|
||||
if event.type == pygame.QUIT:
|
||||
done = True # Flag that we are done so we exit this loop.
|
||||
|
||||
# ---- BUTTONS ----
|
||||
buttons = joy.get_numbuttons()
|
||||
current_buttons = [joy.get_button(i) for i in range(buttons)]
|
||||
if event.type == pygame.JOYBUTTONDOWN:
|
||||
print("Joystick button pressed.")
|
||||
if event.button == 3 and gv.emote is None:
|
||||
gv.emote = "wave"
|
||||
if event.button == 2 and gv.emote is None:
|
||||
gv.robotCommunication.send_text("Huhrensohn")
|
||||
if event.button == 0:
|
||||
joystick = joysticks[event.instance_id]
|
||||
gv
|
||||
if joystick.rumble(0, 0.7, 500):
|
||||
print(f"Rumble effect played on joystick {event.instance_id}")
|
||||
|
||||
emote = None
|
||||
quit_flag = False
|
||||
if event.type == pygame.JOYBUTTONUP:
|
||||
print("Joystick button released.")
|
||||
|
||||
for i in range(buttons):
|
||||
if current_buttons[i] and not prev_buttons[jid][i]:
|
||||
# Button DOWN (edge)
|
||||
if i == 0:
|
||||
emote = "HELLO"
|
||||
elif i == 1:
|
||||
emote = "SAD"
|
||||
elif i == 9:
|
||||
quit_flag = True
|
||||
# Handle hotplugging
|
||||
if event.type == pygame.JOYDEVICEADDED:
|
||||
# This event will be generated when the program starts for every
|
||||
# joystick, filling up the list without needing to create them manually.
|
||||
joy = pygame.joystick.Joystick(event.device_index)
|
||||
joysticks[joy.get_instance_id()] = joy
|
||||
print(f"Joystick {joy.get_instance_id()} connencted")
|
||||
|
||||
prev_buttons[jid] = current_buttons
|
||||
if event.type == pygame.JOYDEVICEREMOVED:
|
||||
del joysticks[event.instance_id]
|
||||
print(f"Joystick {event.instance_id} disconnected")
|
||||
|
||||
# ---- UPDATE INTENT (atomic) ----
|
||||
with lock:
|
||||
shared_intent.move_x = forward
|
||||
shared_intent.move_y = sideways
|
||||
shared_intent.walk = walk
|
||||
# Drawing step
|
||||
# First, clear the screen to white. Don't put other drawing commands
|
||||
# above this, or they will be erased with this command.
|
||||
screen.fill((255, 255, 255))
|
||||
text_print.reset()
|
||||
|
||||
if emote:
|
||||
shared_intent.emote = emote
|
||||
# Get count of joysticks.
|
||||
joystick_count = pygame.joystick.get_count()
|
||||
|
||||
if quit_flag:
|
||||
shared_intent.quit = True
|
||||
text_print.tprint(screen, f"Number of joysticks: {joystick_count}")
|
||||
text_print.indent()
|
||||
|
||||
clock.tick(60)
|
||||
# For each joystick:
|
||||
for joystick in joysticks.values():
|
||||
jid = joystick.get_instance_id()
|
||||
|
||||
text_print.tprint(screen, f"Joystick {jid}")
|
||||
text_print.indent()
|
||||
|
||||
# Get the name from the OS for the controller/joystick.
|
||||
name = joystick.get_name()
|
||||
text_print.tprint(screen, f"Joystick name: {name}")
|
||||
|
||||
guid = joystick.get_guid()
|
||||
text_print.tprint(screen, f"GUID: {guid}")
|
||||
|
||||
power_level = joystick.get_power_level()
|
||||
text_print.tprint(screen, f"Joystick's power level: {power_level}")
|
||||
|
||||
# Usually axis run in pairs, up/down for one, and left/right for
|
||||
# the other. Triggers count as axes.
|
||||
axes = joystick.get_numaxes()
|
||||
text_print.tprint(screen, f"Number of axes: {axes}")
|
||||
text_print.indent()
|
||||
|
||||
for i in range(axes):
|
||||
axis = joystick.get_axis(i)
|
||||
gv.vector_dirmov = [axis]
|
||||
text_print.tprint(screen, f"Axis {i} value: {axis:>6.3f}")
|
||||
text_print.unindent()
|
||||
|
||||
# Get Movement Direction Vector
|
||||
# Left stick (translation)
|
||||
vx = joystick.get_axis(1) # forward/back
|
||||
vy = joystick.get_axis(0) # strafe
|
||||
|
||||
vx, vy = normalize_controller_input(vx, vy)
|
||||
|
||||
# Right stick X (rotation) — adjust axis index if needed
|
||||
omega = joystick.get_axis(2)
|
||||
|
||||
# Deadzone for rotation
|
||||
if abs(omega) < 0.2:
|
||||
omega = 0.0
|
||||
|
||||
# Apply axis snapping (only for translation)
|
||||
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
|
||||
|
||||
# Final movement vector
|
||||
gv.vector_dirmov = [vx, vy, omega]
|
||||
|
||||
# Robot state
|
||||
if abs(vx) < 0.2 and abs(vy) < 0.2 and abs(omega) < 0.2:
|
||||
gv.robot_state = "idle"
|
||||
else:
|
||||
gv.robot_state = "walking"
|
||||
|
||||
buttons = joystick.get_numbuttons()
|
||||
text_print.tprint(screen, f"Number of buttons: {buttons}")
|
||||
text_print.indent()
|
||||
|
||||
for i in range(buttons):
|
||||
button = joystick.get_button(i)
|
||||
text_print.tprint(screen, f"Button {i:>2} value: {button}")
|
||||
text_print.unindent()
|
||||
|
||||
hats = joystick.get_numhats()
|
||||
text_print.tprint(screen, f"Number of hats: {hats}")
|
||||
text_print.indent()
|
||||
|
||||
# Hat position. All or nothing for direction, not a float like
|
||||
# get_axis(). Position is a tuple of int values (x, y).
|
||||
for i in range(hats):
|
||||
hat = joystick.get_hat(i)
|
||||
text_print.tprint(screen, f"Hat {i} value: {str(hat)}")
|
||||
text_print.unindent()
|
||||
|
||||
text_print.unindent()
|
||||
|
||||
# Go ahead and update the screen with what we've drawn.
|
||||
pygame.display.flip()
|
||||
|
||||
# Limit to 30 frames per second.
|
||||
clock.tick(30)
|
||||
|
||||
main()
|
||||
pygame.quit()
|
||||
Reference in New Issue
Block a user