Skip to content
Snippets Groups Projects
Commit 7573be4c authored by Zsedrovits Tamás's avatar Zsedrovits Tamás
Browse files

v1.0

parent a67815a2
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
tello_code_hunt_pygame_manual_control.py
+ 74
− 69
View file @ 7573be4c
......@@ -16,7 +16,8 @@ from dbr import *
# Speed of the drone
S = 100
# Step size
STEP = 100
STEP = 40
RSTEP = 16
# Frames per second of the pygame window display
# A low number also results in input lag, as input information is processed once per frame.
FPS = 120
......@@ -33,7 +34,7 @@ class FrontEnd():
"""
class CommandThread(threading.Thread):
""" Class tu send commands to Tello."""
""" Class to send commands to Tello."""
def __init__(self,command_queue,tello,updatefunction,battery_queue):
threading.Thread.__init__(self)
self.commands_to_run = command_queue
......@@ -44,33 +45,27 @@ class FrontEnd():
""" Main thread."""
while True:
command = self.commands_to_run.get(block=True, timeout=None)
if command == "e": # forward
while not self.tello.move_forward(STEP):
time.sleep(1)
elif command == "h": # back
while not self.tello.move_back(STEP):
time.sleep(1)
elif command == "b": # left
while not self.tello.move_left(STEP):
time.sleep(1)
elif command == "j": # right
while not self.tello.move_right(STEP):
time.sleep(1)
elif command == "f": # up
while not self.tello.move_up(int(STEP/4)):
time.sleep(1)
elif command == "l": # down
while not self.tello.move_down(int(STEP/4)):
time.sleep(1)
elif command == "ccw": # turn left
while not self.tello.rotate_counter_clockwise(15):
time.sleep(1)
elif command == "cw": # turn right
while not self.tello.rotate_clockwise(15):
time.sleep(1)
elif command == "rc":
self.updatefunction()
try:
if command == "e": # forward
self.tello.move_forward(STEP)
elif command == "h": # back
self.tello.move_back(STEP)
elif command == "b": # left
self.tello.move_left(STEP)
elif command == "j": # right
self.tello.move_right(STEP)
elif command == "f": # up
self.tello.move_up(int(STEP/2))
elif command == "l": # down
self.tello.move_down(int(STEP/2))
elif command == "ccw": # turn left
self.tello.rotate_counter_clockwise(RSTEP)
elif command == "cw": # turn right
self.tello.rotate_clockwise(RSTEP)
elif command == "rc":
self.updatefunction()
except Exception:
pass
battery_info = self.tello.get_battery()
try:
text = "Battery: {}%".format(battery_info.rstrip())
......@@ -102,6 +97,7 @@ class FrontEnd():
self.send_rc_control = False
self.battery_text = ""
self.read_mission_pad = False
# create update timer
pygame.time.set_timer(pygame.USEREVENT + 1, 1000 // FPS)
......@@ -315,7 +311,7 @@ class FrontEnd():
self.tello.streamoff()
self.tello.streamon()
self.tello.send_control_command("mon")
# self.tello.send_control_command("mon")
try:
battery_info = self.tello.get_battery()
......@@ -372,12 +368,16 @@ class FrontEnd():
frame = pygame.surfarray.make_surface(frame)
self.screen.blit(frame, (0, 0))
pygame.display.update()
if self.read_mission_pad:
print(self.tello.get_mission_pad_id())
time.sleep(1 / FPS)
# Call it always before finishing. To deallocate resources.
self.reader.stop_video_mode()
self.tello.end()
try:
self.reader.stop_video_mode()
self.tello.end()
except Exception:
pass
def keydown(self, key):
""" Update velocities based on key pressed
......@@ -401,48 +401,53 @@ class FrontEnd():
self.yaw_velocity = -S
elif key == pygame.K_d: # set yaw clockwise velocity
self.yaw_velocity = S
self.command_queue.put("rc",block=True, timeout=None)
self.command_queue.put("rc")
def keyup(self, key):
""" Update velocities based on key released
Arguments:
key: pygame key
"""
if key == pygame.K_SPACE: # toggle rc and step control
self.send_rc_control = not self.send_rc_control
elif key == pygame.K_t: # takeoff
self.tello.takeoff()
elif key == pygame.K_l: # land
self.tello.land()
elif key == pygame.K_e: # emergency land
self.tello.emergency()
if self.send_rc_control:
if key == pygame.K_UP or key == pygame.K_DOWN: # set zero forward/backward velocity
self.for_back_velocity = 0
elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero left/right velocity
self.left_right_velocity = 0
elif key == pygame.K_w or key == pygame.K_s: # set zero up/down velocity
self.up_down_velocity = 0
elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity
self.yaw_velocity = 0
self.command_queue.put("rc",block=True, timeout=None)
else:
if key == pygame.K_UP: # forward
self.command_queue.put("e",block=True, timeout=None)
elif key == pygame.K_DOWN: # back
self.command_queue.put("h",block=True, timeout=None)
elif key == pygame.K_LEFT: # left
self.command_queue.put("b",block=True, timeout=None)
elif key == pygame.K_RIGHT: # right
self.command_queue.put("j",block=True, timeout=None)
elif key == pygame.K_w: # up
self.command_queue.put("f",block=True, timeout=None)
elif key == pygame.K_s: # down
self.command_queue.put("l",block=True, timeout=None)
elif key == pygame.K_d: # turn right
self.command_queue.put("cw",block=True, timeout=None)
elif key == pygame.K_a: # turn left
self.command_queue.put("ccw",block=True, timeout=None)
try:
if key == pygame.K_SPACE: # toggle rc and step control
self.send_rc_control = not self.send_rc_control
elif key == pygame.K_p: # read mission pad id
self.read_mission_pad = not self.read_mission_pad
elif key == pygame.K_t: # takeoff
self.tello.takeoff()
elif key == pygame.K_l: # land
self.tello.land()
elif key == pygame.K_e: # emergency land
self.tello.emergency()
if self.send_rc_control:
if key == pygame.K_UP or key == pygame.K_DOWN: # set zero forward/backward velocity
self.for_back_velocity = 0
elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero left/right velocity
self.left_right_velocity = 0
elif key == pygame.K_w or key == pygame.K_s: # set zero up/down velocity
self.up_down_velocity = 0
elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity
self.yaw_velocity = 0
self.command_queue.put("rc")
else:
if key == pygame.K_UP: # forward
self.command_queue.put("e")
elif key == pygame.K_DOWN: # back
self.command_queue.put("h")
elif key == pygame.K_LEFT: # left
self.command_queue.put("b")
elif key == pygame.K_RIGHT: # right
self.command_queue.put("j")
elif key == pygame.K_w: # up
self.command_queue.put("f")
elif key == pygame.K_s: # down
self.command_queue.put("l")
elif key == pygame.K_d: # turn right
self.command_queue.put("cw")
elif key == pygame.K_a: # turn left
self.command_queue.put("ccw")
except Exception:
pass
def update(self):
""" Update routine. Send velocities to Tello."""
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment