Jordi/spider
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

a

Browse Source
This commit is contained in:
Jordi
2024-12-28 19:30:16 +01:00
parent 1c3e3b77e8
commit 6efb6035f0
1 changed files with 53 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

76
walk2.py
View File

@@ -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
except KeyboardInterrupt:
print("\nProgram terminated.")