89 lines
2.0 KiB
Python
89 lines
2.0 KiB
Python
import threading
|
|
import time
|
|
from adafruit_servokit import ServoKit
|
|
|
|
# Initialize ServoKit
|
|
kit = ServoKit(channels=16)
|
|
|
|
# Constants for angles
|
|
ANGLE_START = 0
|
|
ANGLE_MIDDLE = 110
|
|
ANGLE_NEUTRAL = 55
|
|
|
|
# Constants for delay
|
|
DELAY = 1
|
|
|
|
# Movement functions
|
|
def move_leg(servo1, servo2):
|
|
"""Moves a pair of servos synchronously and prints their positions."""
|
|
kit.servo[servo1].angle = ANGLE_START
|
|
kit.servo[servo2].angle = ANGLE_START
|
|
print(f"Servos {servo1} and {servo2} set to ANGLE_START: {ANGLE_START}")
|
|
time.sleep(DELAY)
|
|
|
|
kit.servo[servo1].angle = ANGLE_MIDDLE
|
|
print(f"Servo {servo1} set to ANGLE_MIDDLE: {ANGLE_MIDDLE}")
|
|
time.sleep(DELAY)
|
|
|
|
kit.servo[servo2].angle = ANGLE_MIDDLE
|
|
print(f"Servo {servo2} set to ANGLE_MIDDLE: {ANGLE_MIDDLE}")
|
|
time.sleep(DELAY)
|
|
|
|
kit.servo[servo1].angle = ANGLE_NEUTRAL
|
|
print(f"Servo {servo1} set to ANGLE_NEUTRAL: {ANGLE_NEUTRAL}")
|
|
|
|
kit.servo[servo2].angle = ANGLE_NEUTRAL
|
|
print(f"Servo {servo2} set to ANGLE_NEUTRAL: {ANGLE_NEUTRAL}")
|
|
|
|
def left_leg_0():
|
|
move_leg(0, 6)
|
|
|
|
def left_leg_1():
|
|
move_leg(1, 7)
|
|
|
|
def left_leg_2():
|
|
move_leg(2, 8)
|
|
|
|
def right_leg_3():
|
|
move_leg(3, 9)
|
|
|
|
def right_leg_4():
|
|
move_leg(4, 10)
|
|
|
|
def right_leg_5():
|
|
move_leg(5, 11)
|
|
|
|
# List of leg movements
|
|
def group_1():
|
|
threads = [
|
|
threading.Thread(target=left_leg_0),
|
|
threading.Thread(target=left_leg_2),
|
|
threading.Thread(target=right_leg_4),
|
|
]
|
|
for thread in threads:
|
|
thread.start()
|
|
for thread in threads:
|
|
thread.join()
|
|
|
|
def group_2():
|
|
threads = [
|
|
threading.Thread(target=left_leg_1),
|
|
threading.Thread(target=right_leg_3),
|
|
threading.Thread(target=right_leg_5),
|
|
]
|
|
for thread in threads:
|
|
thread.start()
|
|
for thread in threads:
|
|
thread.join()
|
|
|
|
# Main loop
|
|
if __name__ == "__main__":
|
|
while True:
|
|
print("Executing Group 1 movements...")
|
|
group_1()
|
|
time.sleep(DELAY)
|
|
|
|
print("Executing Group 2 movements...")
|
|
group_2()
|
|
time.sleep(DELAY)
|