From 6efb6035f05e5adc30b9370ec8a20abb4fc157a7 Mon Sep 17 00:00:00 2001
From: Jordi <jordibolz.jb@gmail.com>
Date: Sat, 28 Dec 2024 19:30:16 +0100
Subject: [PATCH] a

---
 walk2.py | 78 ++++++++++++++++++++++++++++++++++++++------------------
 1 file changed, 53 insertions(+), 25 deletions(-)

diff --git a/walk2.py b/walk2.py
index 93466db..dd448dc 100644
--- a/walk2.py
+++ b/walk2.py
@@ -2,6 +2,7 @@ import threading
 import time
 import sys
 import select
+import queue
 from adafruit_servokit import ServoKit  # type: ignore
 
 # Initialize ServoKit
@@ -10,8 +11,8 @@ kit = ServoKit(channels=16)
 # Constants for delay
 DELAY = 0.25
 
-# Default Servo Angles
-DEFAULT_SERVO_ANGLES = {
+# Servo angle configurations
+FORWARD = {
     0: (0, 110, 70),
     1: (0, 110, 70),
     2: (0, 110, 70),
@@ -26,36 +27,59 @@ DEFAULT_SERVO_ANGLES = {
     11: (110, 10, 90),
 }
 
+ROTATE_RIGHT = {  # Placeholder, update values later
+    0: (0, 100, 60),
+    1: (0, 100, 60),
+    2: (0, 100, 60),
+    3: (0, 100, 60),
+    4: (0, 100, 60),
+    5: (0, 100, 60),
+    6: (10, 100, 80),
+    7: (10, 110, 55),
+    8: (60, 160, 80),
+    9: (160, 60, 80),
+    10: (110, 10, 55),
+    11: (100, 10, 80),
+}
+
+ROTATE_LEFT = {  # Placeholder, update values later
+    0: (0, 120, 80),
+    1: (0, 120, 80),
+    2: (0, 120, 80),
+    3: (0, 120, 80),
+    4: (0, 120, 80),
+    5: (0, 120, 80),
+    6: (10, 120, 100),
+    7: (10, 130, 75),
+    8: (80, 180, 100),
+    9: (180, 80, 100),
+    10: (130, 10, 75),
+    11: (120, 10, 100),
+}
+
 # Movement functions
 def move_leg(servo1, servo2, servo_angles):
-    """Moves a pair of servos synchronously and prints their positions."""
     up, down, neutral1 = servo_angles[servo1]
     front, back, neutral2 = servo_angles[servo2]
 
     kit.servo[servo1].angle = up
     time.sleep(DELAY)
     kit.servo[servo2].angle = front
-    print(f"Servos {servo1} and {servo2} set to UP and FRONT angles: {up}, {front}")
     time.sleep(DELAY)
 
     kit.servo[servo1].angle = down
-    print(f"Servo {servo1} set to DOWN angle: {down}")
     time.sleep(DELAY)
 
     kit.servo[servo2].angle = back
-    print(f"Servo {servo2} set to BACK angle: {back}")
     time.sleep(DELAY)
 
     kit.servo[servo1].angle = up
-    print(f"Servo {servo1} set to UP angle: {up}")
     time.sleep(DELAY)
 
     kit.servo[servo2].angle = neutral2
-    print(f"Servo {servo2} set to NEUTRAL angle: {neutral2}")
     time.sleep(DELAY)
 
     kit.servo[servo1].angle = neutral1
-    print(f"Servo {servo1} set to NEUTRAL angle: {neutral1}")
 
 # Leg movement functions
 def left_leg_0(servo_angles):
@@ -101,35 +125,39 @@ def group_2(servo_angles):
 
 # Main loop control
 def is_input_available():
-    """Check if input is available without blocking."""
     return select.select([sys.stdin], [], [], 0)[0]
 
 # Main execution
 if __name__ == "__main__":
-    print("Press 'w' to execute movements. Press 'q' to quit.")
+    print("Press 'w' for FORWARD, 'a' for LEFT, 'd' for RIGHT. Press 'q' to quit.")
+
+    command_queue = queue.Queue()
 
-    running = False
     try:
         while True:
             if is_input_available():
                 input_char = sys.stdin.read(1).strip()
-                if input_char == 'w':
-                    running = True
+                if input_char in ['w', 'a', 'd']:
+                    command_queue.put(input_char)
                 elif input_char == 'q':
                     print("Exiting...")
                     break
-                else:
-                    running = False
 
-            if running:
-                print("Executing Group 1 movements...")
-                group_1(DEFAULT_SERVO_ANGLES)
-                time.sleep(DELAY)
+            while not command_queue.empty():
+                command = command_queue.get()
+                if command == 'w':
+                    print("Executing FORWARD movement...")
+                    group_1(FORWARD)
+                    group_2(FORWARD)
+                elif command == 'a':
+                    print("Executing LEFT movement...")
+                    group_1(ROTATE_LEFT)
+                    group_2(ROTATE_LEFT)
+                elif command == 'd':
+                    print("Executing RIGHT movement...")
+                    group_1(ROTATE_RIGHT)
+                    group_2(ROTATE_RIGHT)
 
-                print("Executing Group 2 movements...")
-                group_2(DEFAULT_SERVO_ANGLES)
-                time.sleep(DELAY)
-            else:
-                time.sleep(0.1)  # Idle loop
+            time.sleep(0.1)  # Idle loop
     except KeyboardInterrupt:
         print("\nProgram terminated.")