131 lines
3.1 KiB
Python
131 lines
3.1 KiB
Python
from droid import Directions
|
|
import droid_commands as d
|
|
import asyncio
|
|
from time import sleep
|
|
|
|
import lgpio as GPIO
|
|
|
|
NOISE = 23
|
|
|
|
LEFT = 26
|
|
DOWN = 6
|
|
UP = 5
|
|
RIGHT = 25
|
|
|
|
ROT_LEFT = 19
|
|
ROT_RIGHT =24
|
|
|
|
|
|
todo = []
|
|
|
|
|
|
def play_sound(handle, gpio, edge, time):
|
|
print(f"Playing sound at {time}")
|
|
todo.append(d.play_sound())
|
|
|
|
|
|
def move(handle, gpio, edge, time):
|
|
# first, kill all other movements
|
|
todo.append(d.move_stop())
|
|
|
|
# then, check if this was a falling edge
|
|
if edge == 0:
|
|
return
|
|
|
|
if gpio == RIGHT:
|
|
print("Pressed right")
|
|
todo.append(d.move_droid(right=True))
|
|
elif gpio == LEFT:
|
|
print("Pressed left")
|
|
todo.append(d.move_droid(left=True))
|
|
elif gpio == UP:
|
|
print("Pressed forward")
|
|
todo.append(d.move_droid(forward=True))
|
|
elif gpio == DOWN:
|
|
todo.append(d.move_droid(backward=True))
|
|
print("Pressed backward")
|
|
|
|
|
|
def move_head(handle, gpio, edge, time):
|
|
todo.append(d.stop_rotate_head())
|
|
|
|
if edge == 0:
|
|
return
|
|
|
|
if gpio == ROT_LEFT:
|
|
print("Rotating left")
|
|
todo.append(d.rotate_head(Directions.ROTATE_LEFT))
|
|
elif gpio == ROT_RIGHT:
|
|
print("Rotating right")
|
|
todo.append(d.rotate_head(Directions.ROTATE_RIGHT))
|
|
|
|
|
|
|
|
async def droid_connect(pi):
|
|
try:
|
|
await d.start_droid()
|
|
while True:
|
|
if len(todo) > 0:
|
|
await todo[0]
|
|
todo.pop(0)
|
|
except KeyboardInterrupt:
|
|
print("Thanks for using")
|
|
finally:
|
|
GPIO.gpiochip_close(pi)
|
|
await d.stop_droid()
|
|
|
|
|
|
async def main():
|
|
pi = GPIO.gpiochip_open(0)
|
|
if pi < 0:
|
|
return
|
|
|
|
GPIO.gpio_claim_alert(pi, NOISE, GPIO.RISING_EDGE)
|
|
GPIO.gpio_set_debounce_micros(pi, NOISE, 200)
|
|
|
|
# feet
|
|
GPIO.gpio_claim_alert(pi, UP, GPIO.BOTH_EDGES)
|
|
GPIO.gpio_set_debounce_micros(pi, UP, 200)
|
|
|
|
GPIO.gpio_claim_alert(pi, DOWN, GPIO.BOTH_EDGES)
|
|
GPIO.gpio_set_debounce_micros(pi, DOWN, 200)
|
|
|
|
GPIO.gpio_claim_alert(pi, LEFT, GPIO.BOTH_EDGES)
|
|
GPIO.gpio_set_debounce_micros(pi, LEFT, 200)
|
|
|
|
GPIO.gpio_claim_alert(pi, RIGHT, GPIO.BOTH_EDGES)
|
|
GPIO.gpio_set_debounce_micros(pi, RIGHT, 200)
|
|
|
|
# head
|
|
GPIO.gpio_claim_alert(pi, ROT_LEFT, GPIO.BOTH_EDGES)
|
|
GPIO.gpio_set_debounce_micros(pi, ROT_LEFT, 200)
|
|
|
|
GPIO.gpio_claim_alert(pi, ROT_RIGHT, GPIO.BOTH_EDGES)
|
|
GPIO.gpio_set_debounce_micros(pi, ROT_RIGHT, 200)
|
|
|
|
|
|
|
|
cbs = []
|
|
print("Configuring callbacks")
|
|
|
|
# noise
|
|
cbs.append(GPIO.callback(pi, NOISE, func=play_sound))
|
|
|
|
# feet
|
|
cbs.append(GPIO.callback(pi, UP, func=move))
|
|
cbs.append(GPIO.callback(pi, DOWN, func=move))
|
|
cbs.append(GPIO.callback(pi, LEFT, func=move))
|
|
cbs.append(GPIO.callback(pi, RIGHT, func=move))
|
|
|
|
# head
|
|
cbs.append(GPIO.callback(pi, ROT_LEFT, func=move_head))
|
|
cbs.append(GPIO.callback(pi, ROT_RIGHT, func=move_head))
|
|
|
|
print("Callback configured")
|
|
await droid_connect(pi)
|
|
|
|
|
|
|
|
|
|
if __name__ == "__main__":
|
|
asyncio.run(main()) |