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

toggle control, give battery info

parent f79f767d
Branches
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
tello_code_hunt_pygame_manual_control.py
+ 87
− 57
View file @ 0b9aea8d
......@@ -34,17 +34,17 @@ class FrontEnd():
class CommandThread(threading.Thread):
""" Class tu send commands to Tello."""
def __init__(self,command_queue,tello):
def __init__(self,command_queue,tello,updatefunction,battery_queue):
threading.Thread.__init__(self)
self.commands_to_run = command_queue
self.tello = tello
self.is_done = False
self.updatefunction = updatefunction
self.battery_queue = battery_queue
def run(self):
""" Main thread."""
while True:
#try:
command = self.commands_to_run.get(block=True, timeout=None)
if command == "e": # forward
while not self.tello.move_forward(STEP):
time.sleep(1)
......@@ -69,6 +69,15 @@ class FrontEnd():
elif command == "cw": # turn right
while not self.tello.rotate_clockwise(15):
time.sleep(1)
elif command == "rc":
self.updatefunction()
battery_info = self.tello.get_battery()
try:
text = "Battery: {}%".format(battery_info.rstrip())
except Exception:
text = "Battery: {}%".format(battery_info)
self.battery_queue.put(text)
time.sleep(1 / FPS)
def __init__(self):
# Init pygame
......@@ -91,6 +100,9 @@ class FrontEnd():
self.yaw_velocity = 0
self.speed = S
self.send_rc_control = False
self.battery_text = ""
# create update timer
pygame.time.set_timer(pygame.USEREVENT + 1, 1000 // FPS)
......@@ -136,10 +148,21 @@ class FrontEnd():
# queue to store commands
self.command_queue = queue.Queue(0)
# queue to get battery information
self.battery_queue = queue.Queue(0)
# thread to send commands
self.command_thread = self.CommandThread(self.command_queue,self.tello)
self.command_thread = self.CommandThread(self.command_queue,self.tello,self.update,self.battery_queue)
self.command_thread.start()
battery_info = self.tello.get_battery()
try:
text = "Battery: {}%".format(battery_info.rstrip())
except Exception:
text = "Battery: {}%".format(battery_info)
self.battery_queue.put(text)
def on_barcode_result(self,data):
""" The callback function for receiving barcode results."""
......@@ -332,9 +355,12 @@ class FrontEnd():
self.calculate_warped()
frame = self.decorate_frame(frame)
frame = frame.astype('float32')
# text = "Battery: {}%".format(self.tello.get_battery()) # .rstrip())
# cv2.putText(frame, text, (5, self.im_height - 5),
# cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
try:
self.battery_text = self.battery_queue.get_nowait()
except queue.Empty:
pass
cv2.putText(frame, self.battery_text, (5, self.im_height - 5),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
frame = np.rot90(frame)
frame = np.flipud(frame)
......@@ -354,66 +380,70 @@ class FrontEnd():
Arguments:
key: pygame key
"""
# if key == pygame.K_UP: # set forward velocity
# self.for_back_velocity = S
# elif key == pygame.K_DOWN: # set backward velocity
# self.for_back_velocity = -S
# elif key == pygame.K_LEFT: # set left velocity
# self.left_right_velocity = -S
# elif key == pygame.K_RIGHT: # set right velocity
# self.left_right_velocity = S
# elif key == pygame.K_w: # set up velocity
# self.up_down_velocity = S
# elif key == pygame.K_s: # set down velocity
# self.up_down_velocity = -S
# elif key == pygame.K_a: # set yaw counter clockwise velocity
# self.yaw_velocity = -S
# elif key == pygame.K_d: # set yaw clockwise velocity
# self.yaw_velocity = S
if self.send_rc_control:
if key == pygame.K_UP: # set forward velocity
self.for_back_velocity = S
elif key == pygame.K_DOWN: # set backward velocity
self.for_back_velocity = -S
elif key == pygame.K_LEFT: # set left velocity
self.left_right_velocity = -S
elif key == pygame.K_RIGHT: # set right velocity
self.left_right_velocity = S
elif key == pygame.K_w: # set up velocity
self.up_down_velocity = S
elif key == pygame.K_s: # set down velocity
self.up_down_velocity = -S
elif key == pygame.K_a: # set yaw counter clockwise velocity
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)
def keyup(self, key):
""" Update velocities based on key released
Arguments:
key: pygame key
"""
# 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
# 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)
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)
def update(self):
""" Update routine. Send velocities to Tello."""
self.tello.send_rc_control(self.left_right_velocity, self.for_back_velocity,
self.up_down_velocity, self.yaw_velocity)
def main():
""" 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