182 lines
4.4 KiB
Python
182 lines
4.4 KiB
Python
import threading
|
|
import time
|
|
import sys
|
|
import select
|
|
import queue
|
|
from adafruit_servokit import ServoKit # type: ignore
|
|
|
|
# Initialize ServoKit
|
|
kit = ServoKit(channels=16)
|
|
|
|
# Constants for delay
|
|
DELAY = 0.15
|
|
|
|
# Servo angle configurations
|
|
FORWARD = {
|
|
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),
|
|
}
|
|
|
|
BACK = {
|
|
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: (110, 10, 90),
|
|
7: (120, 10, 65),
|
|
8: (170, 70, 90),
|
|
9: (70, 170, 90),
|
|
10: (10, 120, 65),
|
|
11: (10, 110, 90),
|
|
}
|
|
|
|
ROTATE_RIGHT = {
|
|
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: (10, 170, 90),
|
|
10: (10, 120, 65),
|
|
11: (10, 170, 90),
|
|
}
|
|
|
|
ROTATE_LEFT = {
|
|
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: (170, 10, 90),
|
|
7: (120, 10, 65),
|
|
8: (170, 10, 90),
|
|
9: (170, 10, 90),
|
|
10: (120, 10, 65),
|
|
11: (170, 10, 90),
|
|
}
|
|
|
|
# Movement functions
|
|
def move_leg(servo1, servo2, servo_angles):
|
|
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
|
|
time.sleep(DELAY)
|
|
|
|
kit.servo[servo1].angle = down
|
|
time.sleep(DELAY)
|
|
|
|
kit.servo[servo2].angle = back
|
|
time.sleep(DELAY*2)
|
|
|
|
kit.servo[servo1].angle = up
|
|
time.sleep(DELAY)
|
|
|
|
kit.servo[servo2].angle = neutral2
|
|
time.sleep(DELAY)
|
|
|
|
kit.servo[servo1].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
|
|
def is_input_available():
|
|
return select.select([sys.stdin], [], [], 0)[0]
|
|
|
|
# Main execution
|
|
if __name__ == "__main__":
|
|
print("Press 'w' for FORWARD, 'a' for LEFT, 'd' for RIGHT, 's' for BACK. Press 'q' to quit.")
|
|
|
|
command_queue = queue.Queue()
|
|
|
|
try:
|
|
while True:
|
|
if is_input_available():
|
|
input_char = sys.stdin.read(1).strip()
|
|
if input_char in ['w', 'a', 'd', 's']:
|
|
command_queue.put(input_char)
|
|
elif input_char == 'q':
|
|
print("Exiting...")
|
|
break
|
|
|
|
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)
|
|
elif command == 's':
|
|
print("Executing BACK movement...")
|
|
group_1(BACK)
|
|
group_2(BACK)
|
|
|
|
time.sleep(0.1) # Idle loop
|
|
except KeyboardInterrupt:
|
|
print("\nProgram terminated.") |