From 980bca205da84ef00906ff234e28f932ce860df2 Mon Sep 17 00:00:00 2001
From: Lucca Pirovano <luccapirovano@gmx.com>
Date: Wed, 15 Jul 2026 13:39:30 -0400
Subject: [PATCH] added head control with IJKL

---
 naowalk.py | 75 ++++++++++++++++++++++++++++++++----------------------
 1 file changed, 45 insertions(+), 30 deletions(-)

diff --git a/naowalk.py b/naowalk.py
index 80186e0..6c53e9e 100644
--- a/naowalk.py
+++ b/naowalk.py
@@ -18,40 +18,24 @@ class NaoTeleop:
         self.video_client = None
         self.running = True
 
-        # Ultra-optimized video setup
+        # Head position tracking
+        self.head_yaw = 0.0
+        self.head_pitch = 0.0
+
         self._init_video_maxfps()
 
         pygame.init()
-        self.screen = pygame.display.set_mode((320, 240))   # Smaller window = faster
-        pygame.display.set_caption("NAO Teleop - MAX FPS")
+        self.screen = pygame.display.set_mode((320, 240))
+        pygame.display.set_caption("NAO Teleop - Head Control IJKL")
         self.clock = pygame.time.Clock()
 
     def _init_video_maxfps(self):
         try:
             self.video = self.session.service("ALVideoDevice")
-            print("✅ ALVideoDevice found")
+            self.video_client = self.video.subscribeCamera("TeleopCam", 0, 0, 11, 25)
+            print("✅ Camera ready (MAX FPS)")
         except:
-            print("❌ No video service")
-            return
-
-        try:
-            # MAX SPEED SETTINGS:
-            self.video_client = self.video.subscribeCamera(
-                "MaxFpsCam", 
-                0,      # Top camera
-                0,      # 0 = kQQVGA (80x60)  → fastest
-                11,     # RGB
-                30      # Ask for 30fps (realistic ~15-20)
-            )
-            print("🚀 Camera subscribed in MAX FPS mode (80x60)")
-        except Exception as e:
-            print(f"Subscription failed: {e}")
-            # Fallback
-            try:
-                self.video_client = self.video.subscribe("MaxFpsCam", 0, 11, 20)
-                print("✅ Fallback subscription used")
-            except:
-                pass
+            print("Camera not available")
 
     def get_image(self):
         if not self.video or not self.video_client:
@@ -62,23 +46,51 @@ class NaoTeleop:
                 w, h = image[0], image[1]
                 arr = np.frombuffer(bytearray(image[6]), dtype=np.uint8).reshape((h, w, 3))
                 frame = cv2.cvtColor(arr, cv2.COLOR_RGB2BGR)
-                # Minimal processing
                 return cv2.resize(frame, (320, 240), interpolation=cv2.INTER_NEAREST)
         except:
             pass
         return None
 
+    def handle_head_movement(self):
+        keys = pygame.key.get_pressed()
+        speed = 0.3  # Head movement speed (radians)
+
+        changed = False
+
+        if keys[pygame.K_i]:   # Head Up
+            self.head_pitch = max(self.head_pitch - speed, -0.5)
+            changed = True
+        if keys[pygame.K_k]:   # Head Down
+            self.head_pitch = min(self.head_pitch + speed, 0.5)
+            changed = True
+        if keys[pygame.K_j]:   # Head Left
+            self.head_yaw = min(self.head_yaw + speed, 2.0)
+            changed = True
+        if keys[pygame.K_l]:   # Head Right
+            self.head_yaw = max(self.head_yaw - speed, -2.0)
+            changed = True
+
+        if changed:
+            try:
+                self.motion.setAngles(["HeadYaw", "HeadPitch"], [self.head_yaw, self.head_pitch], 0.3)
+            except:
+                pass
+
     def run(self):
         self.motion.wakeUp()
         self.posture.goToPosture("StandInit", 0.5)
-        print("🚀 MAX FPS MODE ACTIVE - WASD/Arrows | ESC to quit")
+        
+        print("🎮 Controls:")
+        print("   WASD / Arrows = Walk")
+        print("   I J K L       = Head (Up, Left, Down, Right)")
+        print("   ESC = Quit")
 
         while self.running:
             for event in pygame.event.get():
                 if event.type == pygame.QUIT or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE):
                     self.running = False
 
-            # Fast input handling
+            # Walking
             keys = pygame.key.get_pressed()
             x = y = theta = 0.0
             if keys[pygame.K_w] or keys[pygame.K_UP]:    x = 0.9
@@ -91,6 +103,9 @@ class NaoTeleop:
             else:
                 self.motion.stopMove()
 
+            # Head control
+            self.handle_head_movement()
+
             # Camera
             frame = self.get_image()
             if frame is not None:
@@ -101,9 +116,9 @@ class NaoTeleop:
                 self.screen.fill((10, 10, 30))
 
             pygame.display.flip()
-            self.clock.tick(0)   # MAX speed (no limit)
+            self.clock.tick(0)  # Max speed
 
-        # Clean shutdown
+        # Shutdown
         print("Shutting down...")
         self.motion.stopMove()
         self.motion.rest()