diff --git a/walk2.py b/walk2.py new file mode 100644 index 0000000..e0562ec --- /dev/null +++ b/walk2.py @@ -0,0 +1,138 @@ +import threading +import time +from adafruit_servokit import ServoKit # type: ignore +from pynput import keyboard + +# Initialize ServoKit +kit = ServoKit(channels=16) + +# Constants for delay +DELAY = 0.25 + +# Default Servo Angles +DEFAULT_SERVO_ANGLES = { + 0: (0, 110, 70), + 1: (0, 110, 70), + 2: (0, 110, 70), + 3: (0, 110, 70), + 4: (0, 110, 70), + 5: (0, 110, 70), + 6: (10, 110, 90), + 7: (10, 120, 65), + 8: (70, 170, 90), + 9: (170, 70, 90), + 10: (120, 10, 65), + 11: (110, 10, 90), +} + +# 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): + move_leg(0, 6, servo_angles) + +def left_leg_1(servo_angles): + move_leg(1, 7, servo_angles) + +def left_leg_2(servo_angles): + move_leg(2, 8, servo_angles) + +def right_leg_3(servo_angles): + move_leg(3, 9, servo_angles) + +def right_leg_4(servo_angles): + move_leg(4, 10, servo_angles) + +def right_leg_5(servo_angles): + move_leg(5, 11, servo_angles) + +# Group movement functions +def group_1(servo_angles): + threads = [ + threading.Thread(target=left_leg_0, args=(servo_angles,)), + threading.Thread(target=left_leg_2, args=(servo_angles,)), + threading.Thread(target=right_leg_4, args=(servo_angles,)), + ] + for thread in threads: + thread.start() + for thread in threads: + thread.join() + +def group_2(servo_angles): + threads = [ + threading.Thread(target=left_leg_1, args=(servo_angles,)), + threading.Thread(target=right_leg_3, args=(servo_angles,)), + threading.Thread(target=right_leg_5, args=(servo_angles,)), + ] + for thread in threads: + thread.start() + for thread in threads: + thread.join() + +# Main loop control +running = False # Control flag + +def on_press(key): + global running + try: + if key.char == 'w': + running = True + except AttributeError: + pass + +def on_release(key): + global running + if key == keyboard.Key.esc: + return False # Stop listener + try: + if key.char == 'w': + running = False + except AttributeError: + pass + +# Main execution +if __name__ == "__main__": + print("Press 'w' to execute movements. Press 'Esc' to exit.") + listener = keyboard.Listener(on_press=on_press, on_release=on_release) + listener.start() + + while True: + if running: + print("Executing Group 1 movements...") + group_1(DEFAULT_SERVO_ANGLES) + time.sleep(DELAY) + + print("Executing Group 2 movements...") + group_2(DEFAULT_SERVO_ANGLES) + time.sleep(DELAY) + else: + time.sleep(0.1) # Idle loop