113 lines
2.5 KiB
Python
113 lines
2.5 KiB
Python
import threading
|
|
import time
|
|
from adafruit_servokit import ServoKit # type: ignore
|
|
|
|
# Initialize ServoKit
|
|
kit = ServoKit(channels=16)
|
|
|
|
# Constants for delay
|
|
DELAY = 0.25
|
|
|
|
# front/up back/down neutral
|
|
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, 170, 90),
|
|
7: (10, 120, 65),
|
|
8: (10, 170, 90),
|
|
9: (170, 10, 90),
|
|
10: (120, 10, 65),
|
|
11: (170, 10, 90)
|
|
}
|
|
|
|
# Movement functions
|
|
def move_leg(servo1, servo2):
|
|
"""Moves a pair of servos synchronously and prints their positions."""
|
|
front1, back1, neutral1 = SERVO_ANGLES[servo1]
|
|
front2, back2, neutral2 = SERVO_ANGLES[servo2]
|
|
|
|
kit.servo[servo1].angle = front1
|
|
time.sleep(DELAY)
|
|
kit.servo[servo2].angle = front2
|
|
print(f"Servos {servo1} and {servo2} set to FRONT angles: {front1}, {front2}")
|
|
time.sleep(DELAY)
|
|
|
|
kit.servo[servo1].angle = back1
|
|
print(f"Servo {servo1} set to BACK angle: {back1}")
|
|
time.sleep(DELAY)
|
|
|
|
kit.servo[servo2].angle = back2
|
|
print(f"Servo {servo2} set to BACK angle: {back2}")
|
|
time.sleep(DELAY)
|
|
|
|
kit.servo[servo1].angle = front1
|
|
print(f"Servo {servo1} set to FRONT angle: {front1}")
|
|
|
|
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}")
|
|
|
|
|
|
|
|
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)
|