feature/button_input (#2)
* Added keyboard functionality for existing features * Droid won't repeat most recent noise * Starting button input * bcm * async * no args * no arglist * asycnio * lgpio * asyncio * wait * async * asycn * remove wait * added keyboard interrup * params * test * got callbacks working * added keyboard control update * droid commands have movement * i do not honestly know what is different * added movement stub * added movement support? * string * format string * trying less space * added global * manual control * select_speed * stop * droid running * stupid multiple naming * play sound fixed * testing forward functionality * callback * move * rotate head * typo
This commit is contained in:
hornet
GitHub
parent
91c94a771f
commit
8e02ed1a8b
131
button_input.py
Normal file
131
button_input.py
Normal file
|
|
@ -0,0 +1,131 @@
|
||||||
|
from droid import Directions
|
||||||
|
import droid_commands as d
|
||||||
|
import asyncio
|
||||||
|
from time import sleep
|
||||||
|
|
||||||
|
import lgpio as GPIO
|
||||||
|
|
||||||
|
NOISE = 16
|
||||||
|
|
||||||
|
LEFT = 26
|
||||||
|
DOWN = 27
|
||||||
|
UP = 25
|
||||||
|
RIGHT = 19
|
||||||
|
|
||||||
|
ROT_LEFT = 6
|
||||||
|
ROT_RIGHT = 5
|
||||||
|
|
||||||
|
|
||||||
|
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())
|
||||||
373
droid.py
373
droid.py
|
|
@ -1,177 +1,196 @@
|
||||||
import asyncio
|
import asyncio
|
||||||
from time import sleep
|
from time import sleep
|
||||||
from bleak import BleakScanner, BleakClient, BleakError
|
from bleak import BleakScanner, BleakClient, BleakError
|
||||||
|
|
||||||
class Droid():
|
class Motors:
|
||||||
def __init__(self, profile):
|
LEFT=0
|
||||||
print("Initializing")
|
RIGHT=1
|
||||||
self.disabledLeds = 0x00
|
HEAD=2
|
||||||
self.profile = profile
|
|
||||||
|
# Directions
|
||||||
# assumes theta in degrees and r = 0 to 100 %
|
class Directions:
|
||||||
# returns a tuple of percentages: (left_thrust, right_thrust)
|
FORWARD = 0
|
||||||
def __throttle_angle_to_thrust__(r, theta):
|
BACKWARD = 8
|
||||||
theta = ((theta + 180) % 360) - 180 # normalize value to [-180, 180)
|
ROTATE_LEFT = 0
|
||||||
r = min(max(0, r), 100) # normalize value to [0, 100]
|
ROTATE_RIGHT = 8
|
||||||
v_a = r * (45 - theta % 90) / 45 # falloff of main motor
|
|
||||||
v_b = min(100, 2 * r + v_a, 2 * r - v_a) # compensation of other motor
|
class Droid():
|
||||||
if theta < -90: return -v_b, -v_a
|
|
||||||
if theta < 0: return -v_a, v_b
|
def __init__(self, profile):
|
||||||
if theta < 90: return v_b, v_a
|
print("Initializing")
|
||||||
return v_a, -v_b
|
self.disabledLeds = 0x00
|
||||||
|
self.profile = profile
|
||||||
async def connect(self):
|
|
||||||
timeout=0.0
|
# assumes theta in degrees and r = 0 to 100 %
|
||||||
print("Connecting")
|
# returns a tuple of percentages: (left_thrust, right_thrust)
|
||||||
self.droid = BleakClient(self.profile)
|
def __throttle_angle_to_thrust__(r, theta):
|
||||||
await self.droid.connect()
|
theta = ((theta + 180) % 360) - 180 # normalize value to [-180, 180)
|
||||||
while not self.droid.is_connected and timeout < 10:
|
r = min(max(0, r), 100) # normalize value to [0, 100]
|
||||||
sleep (.1)
|
v_a = r * (45 - theta % 90) / 45 # falloff of main motor
|
||||||
timeout += .1
|
v_b = min(100, 2 * r + v_a, 2 * r - v_a) # compensation of other motor
|
||||||
print ("Connected!")
|
if theta < -90: return -v_b, -v_a
|
||||||
connectCode = bytearray.fromhex("222001")
|
if theta < 0: return -v_a, v_b
|
||||||
await self.droid.write_gatt_char(0x000d, connectCode, False)
|
if theta < 90: return v_b, v_a
|
||||||
await self.droid.write_gatt_char(0x000d, connectCode, False)
|
return v_a, -v_b
|
||||||
print("Locked")
|
|
||||||
light_blink = bytearray.fromhex("2C000449020001ff01ff0aff00")
|
async def connect(self):
|
||||||
await self.droid.write_gatt_char(0x000d, light_blink)
|
timeout=0.0
|
||||||
connect_sound = bytearray.fromhex("25000c421102")
|
print("Connecting")
|
||||||
await self.droid.write_gatt_char(0x000d, connect_sound)
|
self.droid = BleakClient(self.profile)
|
||||||
sleep(3)
|
await self.droid.connect()
|
||||||
|
# while not self.droid.is_connected and timeout < 10:
|
||||||
async def disconnect(self):
|
# sleep (.1)
|
||||||
print ("Disconnecting")
|
# timeout += .1
|
||||||
try:
|
print ("Connected!")
|
||||||
soundBank = bytearray.fromhex("27420f4444001f09")
|
connectCode = bytearray.fromhex("222001")
|
||||||
await self.droid.write_gatt_char(0x000d, soundBank)
|
await self.droid.write_gatt_char(0x000d, connectCode, False)
|
||||||
soundSelection = bytearray.fromhex("27420f4444001800")
|
await self.droid.write_gatt_char(0x000d, connectCode, False)
|
||||||
await self.droid.write_gatt_char(0x000d, soundSelection)
|
print("Locked")
|
||||||
sleep(3)
|
light_blink = bytearray.fromhex("2C000449020001ff01ff0aff00")
|
||||||
finally:
|
await self.droid.write_gatt_char(0x000d, light_blink)
|
||||||
await self.droid.disconnect()
|
connect_sound = bytearray.fromhex("25000c421102")
|
||||||
print("Disconnected")
|
await self.droid.write_gatt_char(0x000d, connect_sound)
|
||||||
|
sleep(3)
|
||||||
async def led_disable_sound(self, leds):
|
|
||||||
ledDisableCommand = bytearray.fromhex(f"27420f4444004a{leds}")
|
async def disconnect(self):
|
||||||
await self.droid.write_gatt_char(0x000d, ledDisableCommand)
|
print ("Disconnecting")
|
||||||
self.disabledLeds = self.disabledLeds|int(leds, 16)
|
try:
|
||||||
print(self.disabledLeds)
|
soundBank = bytearray.fromhex("27420f4444001f09")
|
||||||
|
await self.droid.write_gatt_char(0x000d, soundBank)
|
||||||
async def led_enable_sound(self, leds):
|
soundSelection = bytearray.fromhex("27420f4444001800")
|
||||||
ledEnableCommand = bytearray.fromhex(f"27420f4444004b{leds}")
|
await self.droid.write_gatt_char(0x000d, soundSelection)
|
||||||
await self.droid.write_gatt_char(0x000d, ledEnableCommand)
|
sleep(3)
|
||||||
self.disabledLeds = self.disabledLeds-(int(leds, 16)&self.disabledLeds)
|
finally:
|
||||||
print(self.disabledLeds)
|
await self.droid.disconnect()
|
||||||
|
print("Disconnected")
|
||||||
|
|
||||||
async def led_flash(self, leds, duration):
|
async def led_disable_sound(self, leds):
|
||||||
pass
|
ledDisableCommand = bytearray.fromhex(f"27420f4444004a{leds}")
|
||||||
|
await self.droid.write_gatt_char(0x000d, ledDisableCommand)
|
||||||
async def led_off(self, leds):
|
self.disabledLeds = self.disabledLeds|int(leds, 16)
|
||||||
ledOffCommand = bytearray.fromhex( f"27420f44440049{leds}" )
|
print(self.disabledLeds)
|
||||||
await self.droid.write_gatt_char(0x000d, ledOffCommand)
|
|
||||||
print(f"{self.disabledLeds:02x}")
|
async def led_enable_sound(self, leds):
|
||||||
print((f"{(~self.disabledLeds & 0x1F):02x}"))
|
ledEnableCommand = bytearray.fromhex(f"27420f4444004b{leds}")
|
||||||
await self.led_enable_sound(f"{(~self.disabledLeds & 0x1F):02x}")
|
await self.droid.write_gatt_char(0x000d, ledEnableCommand)
|
||||||
|
self.disabledLeds = self.disabledLeds-(int(leds, 16)&self.disabledLeds)
|
||||||
|
print(self.disabledLeds)
|
||||||
async def led_on(self, leds):
|
|
||||||
ledOnCommand = bytearray.fromhex(f"27420f44440048{leds}")
|
|
||||||
await self.droid.write_gatt_char(0x000d, ledOnCommand)
|
async def led_flash(self, leds, duration):
|
||||||
|
pass
|
||||||
def move (self, degrees, duration):
|
|
||||||
thrust = self.__throttle_angle_to_thrust__(degrees)
|
async def led_off(self, leds):
|
||||||
|
ledOffCommand = bytearray.fromhex( f"27420f44440049{leds}" )
|
||||||
async def play_sound(self, sound_id=None, bank_id=None, cycle=False, volume=None):
|
await self.droid.write_gatt_char(0x000d, ledOffCommand)
|
||||||
if volume:
|
print(f"{self.disabledLeds:02x}")
|
||||||
self.set_volume(volume)
|
print((f"{(~self.disabledLeds & 0x1F):02x}"))
|
||||||
if bank_id and (not hasattr(self, "soundbank") or self.soundbank != bank_id):
|
await self.led_enable_sound(f"{(~self.disabledLeds & 0x1F):02x}")
|
||||||
await self.set_soundbank(bank_id)
|
|
||||||
if sound_id:
|
|
||||||
soundSelection = bytearray.fromhex("27420f44440018{}".format(sound_id))
|
async def led_on(self, leds):
|
||||||
elif cycle:
|
ledOnCommand = bytearray.fromhex(f"27420f44440048{leds}")
|
||||||
soundSelection = bytearray.fromhex("26420f4344001c")
|
await self.droid.write_gatt_char(0x000d, ledOnCommand)
|
||||||
else:
|
|
||||||
soundSelection = bytearray.fromhex("27420f44440010{}".format(self.bank_id))
|
async def move (self, degrees, duration):
|
||||||
await self.droid.write_gatt_char(0x000d, soundSelection)
|
thrust = self.__throttle_angle_to_thrust__(degrees)
|
||||||
|
|
||||||
async def run_routine(self, routineId):
|
|
||||||
full_id = bytearray.fromhex("25000c42{}02".format(routineId))
|
async def move_motors(self, direction, motor, strength):
|
||||||
await self.droid.write_gatt_char(0x000d, full_id)
|
move_selection = bytearray.fromhex("29420546{}{}{}012C0000".format(direction, motor, strength))
|
||||||
|
await self.droid.write_gatt_char(0x000d, move_selection)
|
||||||
async def set_soundbank(self, bank_id):
|
|
||||||
self.soundbank = bank_id
|
|
||||||
soundBank = bytearray.fromhex("27420f4444001f{}".format(bank_id))
|
async def play_sound(self, sound_id=None, bank_id=None, cycle=False, volume=None):
|
||||||
await self.droid.write_gatt_char(0x000d, soundBank)
|
if volume:
|
||||||
|
self.set_volume(volume)
|
||||||
async def set_volume(self, volume):
|
if bank_id and (not hasattr(self, "soundbank") or self.soundbank != bank_id):
|
||||||
volume_command = bytearray.fromhex("27420f4444000e{}".format(volume))
|
await self.set_soundbank(bank_id)
|
||||||
await self.droid.write_gatt_char(0x000d, volume_command)
|
if sound_id:
|
||||||
|
soundSelection = bytearray.fromhex("27420f44440018{}".format(sound_id))
|
||||||
def findDroid(candidate, data):
|
elif cycle:
|
||||||
if candidate.name == "DROID":
|
soundSelection = bytearray.fromhex("26420f4344001c")
|
||||||
return True
|
else:
|
||||||
else:
|
soundSelection = bytearray.fromhex("27420f44440010{}".format(self.bank_id))
|
||||||
return False
|
await self.droid.write_gatt_char(0x000d, soundSelection)
|
||||||
|
|
||||||
async def discoverDroid(retry=False):
|
async def run_routine(self, routineId):
|
||||||
myDroid = None
|
full_id = bytearray.fromhex("25000c42{}02".format(routineId))
|
||||||
|
await self.droid.write_gatt_char(0x000d, full_id)
|
||||||
while retry and myDroid is None:
|
|
||||||
try:
|
async def set_soundbank(self, bank_id):
|
||||||
myDroid = await BleakScanner.find_device_by_filter(findDroid)
|
self.soundbank = bank_id
|
||||||
if myDroid is None:
|
soundBank = bytearray.fromhex("27420f4444001f{}".format(bank_id))
|
||||||
if not retry:
|
await self.droid.write_gatt_char(0x000d, soundBank)
|
||||||
print("Droid discovery timed out.")
|
|
||||||
return
|
async def set_volume(self, volume):
|
||||||
else:
|
volume_command = bytearray.fromhex("27420f4444000e{}".format(volume))
|
||||||
print("Droid discovery timed out. Retrying...")
|
await self.droid.write_gatt_char(0x000d, volume_command)
|
||||||
continue
|
|
||||||
except BleakError as err:
|
def findDroid(candidate, data):
|
||||||
print("Droid discovery failed. Retrying...")
|
if candidate.name == "DROID":
|
||||||
continue
|
return True
|
||||||
|
else:
|
||||||
|
return False
|
||||||
print (f"Astromech successfully discovered: [ {myDroid} ]")
|
|
||||||
|
async def discoverDroid(retry=False):
|
||||||
d = Droid(myDroid)
|
myDroid = None
|
||||||
return d
|
|
||||||
|
while retry and myDroid is None:
|
||||||
|
try:
|
||||||
|
myDroid = await BleakScanner.find_device_by_filter(findDroid)
|
||||||
async def main():
|
if myDroid is None:
|
||||||
|
if not retry:
|
||||||
d = await discoverDroid(retry=True)
|
print("Droid discovery timed out.")
|
||||||
|
return
|
||||||
try:
|
else:
|
||||||
await d.connect()
|
print("Droid discovery timed out. Retrying...")
|
||||||
sleep (3)
|
continue
|
||||||
# await arms.run_routine("05")
|
except BleakError as err:
|
||||||
# sleep (5)
|
print("Droid discovery failed. Retrying...")
|
||||||
# await arms.set_soundbank("05")
|
continue
|
||||||
# await arms.play_sound("00")
|
|
||||||
# sleep (5)
|
|
||||||
# for i in range(5):
|
print (f"Astromech successfully discovered: [ {myDroid} ]")
|
||||||
# await arms.play_sound(cycle=True)
|
|
||||||
# sleep(5)
|
d = Droid(myDroid)
|
||||||
# await arms.play_sound("00", "00")
|
return d
|
||||||
# sleep(8)
|
|
||||||
await d.led_disable_sound("01")
|
|
||||||
await d.play_sound("00", "00")
|
|
||||||
sleep(10)
|
async def main():
|
||||||
await d.led_on("1f")
|
|
||||||
sleep(10)
|
d = await discoverDroid(retry=True)
|
||||||
await d.led_off("1f")
|
|
||||||
await d.play_sound("00", "00")
|
try:
|
||||||
sleep(10)
|
await d.connect()
|
||||||
|
sleep (3)
|
||||||
except OSError as err:
|
# await arms.run_routine("05")
|
||||||
print(f"Discovery failed due to operating system: {err}")
|
# sleep (5)
|
||||||
except BleakError as err:
|
# await arms.set_soundbank("05")
|
||||||
print(f"Discovery failed due to Bleak: {err}")
|
# await arms.play_sound("00")
|
||||||
|
# sleep (5)
|
||||||
finally:
|
# for i in range(5):
|
||||||
await d.disconnect()
|
# await arms.play_sound(cycle=True)
|
||||||
|
# sleep(5)
|
||||||
if __name__ == "__main__":
|
# await arms.play_sound("00", "00")
|
||||||
asyncio.run(main())
|
# sleep(8)
|
||||||
|
await d.led_disable_sound("01")
|
||||||
|
await d.play_sound("00", "00")
|
||||||
|
sleep(10)
|
||||||
|
await d.led_on("1f")
|
||||||
|
sleep(10)
|
||||||
|
await d.led_off("1f")
|
||||||
|
await d.play_sound("00", "00")
|
||||||
|
sleep(10)
|
||||||
|
|
||||||
|
except OSError as err:
|
||||||
|
print(f"Discovery failed due to operating system: {err}")
|
||||||
|
except BleakError as err:
|
||||||
|
print(f"Discovery failed due to Bleak: {err}")
|
||||||
|
|
||||||
|
finally:
|
||||||
|
await d.disconnect()
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
asyncio.run(main())
|
||||||
|
|
|
||||||
89
droid_commands.py
Normal file
89
droid_commands.py
Normal file
|
|
@ -0,0 +1,89 @@
|
||||||
|
import droid
|
||||||
|
import asyncio
|
||||||
|
from time import sleep
|
||||||
|
from random import randrange
|
||||||
|
|
||||||
|
d = ""
|
||||||
|
|
||||||
|
|
||||||
|
lastSound = -1
|
||||||
|
async def play_sound():
|
||||||
|
global lastSound
|
||||||
|
global d
|
||||||
|
sound = lastSound
|
||||||
|
while sound == lastSound:
|
||||||
|
sound = randrange(0,5)
|
||||||
|
lastSound = sound
|
||||||
|
|
||||||
|
while d is None:
|
||||||
|
sleep(0.1)
|
||||||
|
|
||||||
|
await d.play_sound(f"0{sound}","00")
|
||||||
|
|
||||||
|
|
||||||
|
async def play_specific_sound(bank, sound):
|
||||||
|
global d
|
||||||
|
await d.play_sound( sound_id = sound, bank_id = bank)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
async def move_droid(forward=False, backward=False, left=False, right=False):
|
||||||
|
|
||||||
|
l = droid.Directions.FORWARD
|
||||||
|
r = droid.Directions.FORWARD
|
||||||
|
global d
|
||||||
|
|
||||||
|
|
||||||
|
if forward:
|
||||||
|
l = droid.Directions.FORWARD
|
||||||
|
r = droid.Directions.FORWARD
|
||||||
|
if backward:
|
||||||
|
l = droid.Directions.BACKWARD
|
||||||
|
r = droid.Directions.BACKWARD
|
||||||
|
if left:
|
||||||
|
# spin left
|
||||||
|
l = droid.Directions.BACKWARD
|
||||||
|
r = droid.Directions.FORWARD
|
||||||
|
if right:
|
||||||
|
# spin right
|
||||||
|
l = droid.Directions.FORWARD
|
||||||
|
r = droid.Directions.BACKWARD
|
||||||
|
|
||||||
|
await d.move_motors(l, droid.Motors.LEFT, "FF")
|
||||||
|
await d.move_motors(r, droid.Motors.RIGHT,"FF")
|
||||||
|
|
||||||
|
|
||||||
|
async def move_manually(direction, motor, strength):
|
||||||
|
global d
|
||||||
|
await d.move_motors(direction, motor, strength)
|
||||||
|
|
||||||
|
|
||||||
|
async def move_stop():
|
||||||
|
global d
|
||||||
|
await move_manually(droid.Directions.FORWARD, droid.Motors.LEFT, "00")
|
||||||
|
await move_manually(droid.Directions.FORWARD, droid.Motors.RIGHT, "00")
|
||||||
|
|
||||||
|
|
||||||
|
async def rotate_head(direction):
|
||||||
|
global d
|
||||||
|
await d.move_motors(direction, droid.Motors.HEAD, "FF")
|
||||||
|
# eventually should we stop this call so we don't waste a bunch of battery?
|
||||||
|
|
||||||
|
async def stop_rotate_head():
|
||||||
|
global d
|
||||||
|
await d.move_motors(droid.Directions.ROTATE_LEFT, droid.Motors.HEAD, "00")
|
||||||
|
|
||||||
|
|
||||||
|
async def start_droid():
|
||||||
|
"""
|
||||||
|
This function should be in a try loop
|
||||||
|
"""
|
||||||
|
global d
|
||||||
|
d = await droid.discoverDroid(retry=True)
|
||||||
|
await d.connect()
|
||||||
|
|
||||||
|
|
||||||
|
async def stop_droid():
|
||||||
|
global d
|
||||||
|
await d.disconnect()
|
||||||
|
|
||||||
88
keyboard_control.py
Normal file
88
keyboard_control.py
Normal file
|
|
@ -0,0 +1,88 @@
|
||||||
|
import asyncio
|
||||||
|
from re import L
|
||||||
|
from time import sleep
|
||||||
|
from random import randrange
|
||||||
|
import droid_commands as d
|
||||||
|
from droid import Directions
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
runningDroid = True
|
||||||
|
|
||||||
|
async def select_noise(d):
|
||||||
|
bank = input("\tBank> ")
|
||||||
|
sound = input("\tSound> ")
|
||||||
|
await d.play_specific_sound(bank, sound)
|
||||||
|
|
||||||
|
|
||||||
|
async def play_sound(d):
|
||||||
|
await d.play_sound()
|
||||||
|
|
||||||
|
async def select_speed(d):
|
||||||
|
direction = input("\tDirection> ")
|
||||||
|
motor = input("\tMotor> ")
|
||||||
|
speed = input("\tSpeed> ")
|
||||||
|
await d.move_manually(direction, motor, speed)
|
||||||
|
|
||||||
|
async def forward(d):
|
||||||
|
await d.move_droid(Directions.FORWARD)
|
||||||
|
|
||||||
|
async def backward(d):
|
||||||
|
await d.move_droid(Directions.BACKWARD)
|
||||||
|
|
||||||
|
async def left(d):
|
||||||
|
await d.move_droid(Directions.LEFT)
|
||||||
|
|
||||||
|
async def right(d):
|
||||||
|
await d.move_droid(Directions.RIGHT)
|
||||||
|
|
||||||
|
async def move_stop(d):
|
||||||
|
await d.move_stop()
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
async def quit(d):
|
||||||
|
global runningDroid
|
||||||
|
runningDroid = False
|
||||||
|
|
||||||
|
async def main():
|
||||||
|
# first, get droid
|
||||||
|
await d.start_droid()
|
||||||
|
|
||||||
|
commands = {
|
||||||
|
"w": forward,
|
||||||
|
"a": left,
|
||||||
|
"s": backward,
|
||||||
|
"d": right,
|
||||||
|
"x": move_stop,
|
||||||
|
"z": select_speed,
|
||||||
|
#"e": rotate counter-clockwise,
|
||||||
|
#"r": rotate clockwise,
|
||||||
|
#"f": special effect
|
||||||
|
"n": play_sound,
|
||||||
|
"m": select_noise,
|
||||||
|
"q": quit
|
||||||
|
}
|
||||||
|
try:
|
||||||
|
global runningDroid
|
||||||
|
while runningDroid:
|
||||||
|
# next, await input
|
||||||
|
command = input("Please input a command > ")
|
||||||
|
# next, parse that command
|
||||||
|
c = command.lower()[0:1]
|
||||||
|
if c in commands.keys():
|
||||||
|
await commands[c](d)
|
||||||
|
else:
|
||||||
|
print("Command does not exist.")
|
||||||
|
|
||||||
|
|
||||||
|
sleep(0.2)
|
||||||
|
|
||||||
|
except Exception as e:
|
||||||
|
print(e)
|
||||||
|
|
||||||
|
finally:
|
||||||
|
await d.stop_droid()
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
asyncio.run(main())
|
||||||
Loading…
Reference in New Issue
Block a user