From 24ce3cfd9a225729fdc9c890db082bc8aa11af71 Mon Sep 17 00:00:00 2001
From: Lucca Pirovano <luccapirovano@gmx.com>
Date: Thu, 11 Jun 2026 14:33:08 -0400
Subject: [PATCH] added gamepad input

---
 control.py | 97 +++++++++++++++++++++++++++++++++---------------------
 1 file changed, 60 insertions(+), 37 deletions(-)

diff --git a/control.py b/control.py
index 4e1af0d..7602ea4 100644
--- a/control.py
+++ b/control.py
@@ -1,64 +1,87 @@
 from codrone_edu.drone import *
 import pygame
 import time
-import sys
 
-# Initialize pygame
+# ====================== SETTINGS ======================
+POWER = 22          # Safe & slow speed (15-30)
+DEADZONE = 0.15
+# =====================================================
+
 pygame.init()
-screen = pygame.display.set_mode((400, 300))  # Small window (focus here)
-pygame.display.set_caption("CoDrone Arrow Key Controller")
+screen = pygame.display.set_mode((500, 300))
+pygame.display.set_caption("CoDrone Controller")
+
+# Gamepad setup
+pygame.joystick.init()
+joysticks = [pygame.joystick.Joystick(i) for i in range(pygame.joystick.get_count())]
+for joy in joysticks:
+    joy.init()
+    print(f"🎮 Detected: {joy.get_name()}")
 
 drone = Drone()
-drone.connect()   # or drone.pair()
-
-print("Connecting...")
+drone.connect()
 time.sleep(3)
 
 try:
     drone.takeoff()
-    print("✅ Drone ready!")
-    print("Use Arrow Keys:")
-    print("↑ Forward  ↓ Backward  ← Left  → Right")
-    print("W = Up    S = Down")
-    print("Press ESC or close window to land")
-
-    power = 20          # Very safe & slow (15-25 recommended)
+    print("✅ Drone flying! Controls: Arrows / Left Stick | W/S | A/D")
+    print("ESC or Start button to land")
 
     clock = pygame.time.Clock()
 
     running = True
     while running:
         for event in pygame.event.get():
-            if event.type == pygame.QUIT or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE):
+            if event.type == pygame.QUIT or \
+               (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE) or \
+               (event.type == pygame.JOYBUTTONDOWN and event.button in (7, 6)):  # Start/Options
                 running = False
 
-        # Get current key states
         keys = pygame.key.get_pressed()
+        roll = pitch = throttle = yaw = 0
 
-        # Reset all movement
-        drone.set_roll(0)
-        drone.set_pitch(0)
-        drone.set_yaw(0)
-        drone.set_throttle(0)
+        # ==================== KEYBOARD ====================
+        if keys[pygame.K_UP]:    pitch = POWER
+        if keys[pygame.K_DOWN]:  pitch = -POWER
+        if keys[pygame.K_LEFT]:  roll = -POWER
+        if keys[pygame.K_RIGHT]: roll = POWER
+        if keys[pygame.K_w]:     throttle = POWER
+        if keys[pygame.K_s]:     throttle = -POWER
+        if keys[pygame.K_a]:     yaw = -POWER
+        if keys[pygame.K_d]:     yaw = POWER
 
-        # Arrow keys for horizontal movement
-        if keys[pygame.K_UP]:
-            drone.set_pitch(power)      # Forward
-        if keys[pygame.K_DOWN]:
-            drone.set_pitch(-power)     # Backward
-        if keys[pygame.K_LEFT]:
-            drone.set_roll(-power)      # Left
-        if keys[pygame.K_RIGHT]:
-            drone.set_roll(power)       # Right
+        # ==================== GAMEPAD ====================
+        if joysticks:
+            joy = joysticks[0]
 
-        # W/S for altitude
-        if keys[pygame.K_w]:
-            drone.set_throttle(power)   # Up
-        if keys[pygame.K_s]:
-            drone.set_throttle(-power)  # Down
+            # Left stick
+            roll += int(joy.get_axis(0) * POWER)
+            pitch += int(-joy.get_axis(1) * POWER)
 
-        drone.move(0.1)   # Send command
-        clock.tick(30)    # ~30 updates per second (smooth but not too fast)
+            # Right stick / Triggers
+            if joy.get_numaxes() > 3:
+                throttle += int(-joy.get_axis(3) * POWER)
+                yaw += int(joy.get_axis(2) * POWER)
+
+            # Common trigger mapping (Xbox/PS)
+            if joy.get_numaxes() > 5:
+                yaw += int(joy.get_axis(5) * POWER)   # Right trigger
+                yaw += int(-joy.get_axis(4) * POWER)  # Left trigger
+
+        # Clamp values
+        roll = max(-100, min(100, roll))
+        pitch = max(-100, min(100, pitch))
+        throttle = max(-100, min(100, throttle))
+        yaw = max(-100, min(100, yaw))
+
+        # Apply movement
+        drone.set_roll(roll)
+        drone.set_pitch(pitch)
+        drone.set_throttle(throttle)
+        drone.set_yaw(yaw)
+        drone.move(0.1)
+
+        clock.tick(30)
 
 except Exception as e:
     print("Error:", e)