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

Move tello in steps

parent 81d3c85d
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
tello_code_hunt_pygame_manual_control.py
+ 123
− 49
View file @ 00b6f186
...@@ -5,6 +5,8 @@ ...@@ -5,6 +5,8 @@
To control the drone and show the video, it uses pygame. To control the drone and show the video, it uses pygame.
""" """
import time import time
import queue
import threading
import cv2 import cv2
import pygame import pygame
import numpy as np import numpy as np
...@@ -12,7 +14,9 @@ from djitellopy import Tello ...@@ -12,7 +14,9 @@ from djitellopy import Tello
from dbr import * from dbr import *
# Speed of the drone # Speed of the drone
S = 60 S = 100
# Step size
s_S = 100
# Frames per second of the pygame window display # Frames per second of the pygame window display
# A low number also results in input lag, as input information is processed once per frame. # A low number also results in input lag, as input information is processed once per frame.
FPS = 120 FPS = 120
...@@ -28,6 +32,58 @@ class FrontEnd(): ...@@ -28,6 +32,58 @@ class FrontEnd():
- W and S: Up and down. - W and S: Up and down.
""" """
class CommandThread(threading.Thread):
""" Class tu send commands to Tello."""
def __init__(self,command_queue,tello):
threading.Thread.__init__(self)
self.commands_to_run = command_queue
self.tello = tello
self.is_done = False
def run(self):
""" Main thread."""
while True:
command = self.commands_to_run.get(block=True, timeout=None)
if command == "e": # forward
success = self.tello.move_forward(s_S)
while not success:
time.sleep(1)
success = self.tello.move_forward(s_S)
elif command == "h": # back
success = self.tello.move_back(s_S)
while not success:
time.sleep(1)
success = self.tello.move_back(s_S)
elif command == "b": # left
success = self.tello.move_left(s_S)
while not success:
time.sleep(1)
success = self.tello.move_left(s_S)
elif command == "j": # right
success = self.tello.move_right(s_S)
while not success:
time.sleep(1)
success = self.tello.move_right(s_S)
elif command == "f": # up
success = self.tello.move_up(int(s_S/4))
while not success:
time.sleep(1)
success = self.tello.move_up(int(s_S/4))
elif command == "l": # down
success = self.tello.move_down(int(s_S/4))
while not success:
time.sleep(1)
success = self.tello.move_down(int(s_S/4))
elif command == "ccw": # turn left
success = self.tello.rotate_counter_clockwise(15)
while not success:
time.sleep(1)
success = self.tello.rotate_counter_clockwise(15)
elif command == "cw": # turn right
success = self.tello.rotate_clockwise(15)
while not success:
time.sleep(1)
success = self.tello.rotate_clockwise(15)
def __init__(self): def __init__(self):
# Init pygame # Init pygame
pygame.init() pygame.init()
...@@ -43,13 +99,7 @@ class FrontEnd(): ...@@ -43,13 +99,7 @@ class FrontEnd():
self.tello = Tello() self.tello = Tello()
# Drone velocities between -100~100 # Drone velocities between -100~100
self.for_back_velocity = 0 self.speed = S
self.left_right_velocity = 0
self.up_down_velocity = 0
self.yaw_velocity = 0
self.speed = 10
self.send_rc_control = False
# create update timer # create update timer
pygame.time.set_timer(pygame.USEREVENT + 1, 1000 // FPS) pygame.time.set_timer(pygame.USEREVENT + 1, 1000 // FPS)
...@@ -94,6 +144,13 @@ class FrontEnd(): ...@@ -94,6 +144,13 @@ class FrontEnd():
# store AR images for overlay to decrease flashing # store AR images for overlay to decrease flashing
self.warped = [] self.warped = []
# queue to store commands
self.command_queue = queue.Queue(0)
# thread to send commands
self.command_thread = self.CommandThread(self.command_queue,self.tello)
self.command_thread.start()
def on_barcode_result(self,data): def on_barcode_result(self,data):
""" The callback function for receiving barcode results.""" """ The callback function for receiving barcode results."""
self.qrcodes = data self.qrcodes = data
...@@ -251,15 +308,15 @@ class FrontEnd(): ...@@ -251,15 +308,15 @@ class FrontEnd():
self.tello.streamoff() self.tello.streamoff()
self.tello.streamon() self.tello.streamon()
self.tello.send_control_command("mon")
frame_read = self.tello.get_frame_read() frame_read = self.tello.get_frame_read()
should_stop = False should_stop = False
while not should_stop: while not should_stop:
for event in pygame.event.get(): for event in pygame.event.get():
if event.type == pygame.USEREVENT + 1: if event.type == pygame.QUIT:
self.update()
elif event.type == pygame.QUIT:
should_stop = True should_stop = True
elif event.type == pygame.KEYDOWN: elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_ESCAPE: if event.key == pygame.K_ESCAPE:
...@@ -285,9 +342,9 @@ class FrontEnd(): ...@@ -285,9 +342,9 @@ class FrontEnd():
self.calculate_warped() self.calculate_warped()
frame = self.decorate_frame(frame) frame = self.decorate_frame(frame)
frame = frame.astype('float32') frame = frame.astype('float32')
text = "Battery: {}%".format(self.tello.get_battery().rstrip()) # text = "Battery: {}%".format(self.tello.get_battery()) # .rstrip())
cv2.putText(frame, text, (5, self.im_height - 5), # cv2.putText(frame, text, (5, self.im_height - 5),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2) # cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
frame = np.rot90(frame) frame = np.rot90(frame)
frame = np.flipud(frame) frame = np.flipud(frame)
...@@ -307,49 +364,66 @@ class FrontEnd(): ...@@ -307,49 +364,66 @@ class FrontEnd():
Arguments: Arguments:
key: pygame key key: pygame key
""" """
if key == pygame.K_UP: # set forward velocity # if key == pygame.K_UP: # set forward velocity
self.for_back_velocity = S # self.for_back_velocity = S
elif key == pygame.K_DOWN: # set backward velocity # elif key == pygame.K_DOWN: # set backward velocity
self.for_back_velocity = -S # self.for_back_velocity = -S
elif key == pygame.K_LEFT: # set left velocity # elif key == pygame.K_LEFT: # set left velocity
self.left_right_velocity = -S # self.left_right_velocity = -S
elif key == pygame.K_RIGHT: # set right velocity # elif key == pygame.K_RIGHT: # set right velocity
self.left_right_velocity = S # self.left_right_velocity = S
elif key == pygame.K_w: # set up velocity # elif key == pygame.K_w: # set up velocity
self.up_down_velocity = S # self.up_down_velocity = S
elif key == pygame.K_s: # set down velocity # elif key == pygame.K_s: # set down velocity
self.up_down_velocity = -S # self.up_down_velocity = -S
elif key == pygame.K_a: # set yaw counter clockwise velocity # elif key == pygame.K_a: # set yaw counter clockwise velocity
self.yaw_velocity = -S # self.yaw_velocity = -S
elif key == pygame.K_d: # set yaw clockwise velocity # elif key == pygame.K_d: # set yaw clockwise velocity
self.yaw_velocity = S # self.yaw_velocity = S
def keyup(self, key): def keyup(self, key):
""" Update velocities based on key released """ Update velocities based on key released
Arguments: Arguments:
key: pygame key key: pygame key
""" """
if key == pygame.K_UP or key == pygame.K_DOWN: # set zero forward/backward velocity # if key == pygame.K_UP or key == pygame.K_DOWN: # set zero forward/backward velocity
self.for_back_velocity = 0 # self.for_back_velocity = 0
elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero left/right velocity # elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero left/right velocity
self.left_right_velocity = 0 # self.left_right_velocity = 0
elif key == pygame.K_w or key == pygame.K_s: # set zero up/down velocity # elif key == pygame.K_w or key == pygame.K_s: # set zero up/down velocity
self.up_down_velocity = 0 # self.up_down_velocity = 0
elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity # elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity
self.yaw_velocity = 0 # self.yaw_velocity = 0
# elif key == pygame.K_t: # takeoff
# self.tello.takeoff()
# self.send_rc_control = True
# elif key == pygame.K_l: # land
# not self.tello.land()
# self.send_rc_control = False
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)
elif key == pygame.K_t: # takeoff elif key == pygame.K_t: # takeoff
self.tello.takeoff() self.tello.takeoff()
self.send_rc_control = True
elif key == pygame.K_l: # land elif key == pygame.K_l: # land
not self.tello.land() self.tello.land()
self.send_rc_control = False elif key == pygame.K_e: # emergency land
self.tello.emergency()
def update(self):
""" Update routine. Send velocities to Tello."""
if self.send_rc_control:
self.tello.send_rc_control(self.left_right_velocity, self.for_back_velocity,
self.up_down_velocity, self.yaw_velocity)
def main(): def main():
""" Main class to create and start the frontend.""" """ Main class to create and start the frontend."""
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment