266 lines
9.1 KiB
Python
266 lines
9.1 KiB
Python
from threading import Thread, Event
|
|
import time
|
|
from djitellopy import Tello
|
|
import csv
|
|
import matplotlib.pyplot as plt
|
|
import cv2
|
|
from pyzbar.pyzbar import decode
|
|
import numpy as np
|
|
import keyboard
|
|
import os
|
|
|
|
class TelloDrone:
|
|
|
|
class TelloKillSwitch(Thread):
|
|
|
|
tc_handler = None
|
|
|
|
def __init__(self, tc_handler):
|
|
Thread.__init__(self)
|
|
self.tc_handler = tc_handler
|
|
|
|
def run(self):
|
|
keyboard.wait('space')
|
|
self.tc_handler.force_emergency_stop()
|
|
|
|
|
|
class Threading(Thread):
|
|
interval = 1.0
|
|
running = None
|
|
func = None
|
|
|
|
def __init__(self, interval, event, func):
|
|
Thread.__init__(self)
|
|
self.running = event
|
|
self.interval = interval
|
|
self.func = func
|
|
|
|
def run(self):
|
|
while not self.running.wait(self.interval):
|
|
self.func()
|
|
|
|
|
|
drone = None
|
|
stop_controller = None
|
|
|
|
def force_emergency_stop(self):
|
|
self.drone.emergency()
|
|
self.stop_controller.set()
|
|
|
|
def battery_temp_check(self):
|
|
print(f"\nBATTERY: {self.drone.get_battery()}")
|
|
print(f"TEMPERATURE: {self.drone.get_temperature()}\n")
|
|
|
|
def show_camera(self):
|
|
cap = self.drone.get_frame_read().frame
|
|
cap = cv2.resize(cap, (1280, 720))
|
|
cap = cv2.cvtColor(cap, cv2.COLOR_BGR2RGB)
|
|
for barcode in decode(cap):
|
|
self.barcode_rect = barcode.rect
|
|
self.barcode_polygon = barcode.polygon
|
|
self.barcode_data = barcode.data.decode('utf-8')
|
|
self.height_14 = (barcode.polygon[0][1] - barcode.polygon[3][1])
|
|
self.height_23 = (barcode.polygon[1][1] - barcode.polygon[2][1])
|
|
|
|
# print(f"Height 1-4: {self.height_14}")
|
|
# print(f"Height 2-3: {self.height_23}")
|
|
# print(f"Height difference: {self.height_14 - self.height_23}")
|
|
|
|
pts = np.array([barcode.polygon], np.int32)
|
|
pts = pts.reshape((-1,1,2))
|
|
cv2.polylines(cap,[pts],True,(255,0,255),5)
|
|
# print(barcode.polygon)
|
|
cv2.imshow('frame', cap)
|
|
cv2.waitKey(1)
|
|
|
|
def follow_qr(self):
|
|
|
|
if self.barcode_data == 'right':
|
|
self.right_90 = True
|
|
self.left_90 = False
|
|
elif self.barcode_data == 'left':
|
|
self.left_90 = True
|
|
self.right_90 = False
|
|
# print("działa w chuj")
|
|
left = self.barcode_rect[0]
|
|
top = self.barcode_rect[1]
|
|
width = self.barcode_rect[2]
|
|
height = self.barcode_rect[3]
|
|
width_dif = (1280 - width) / 2
|
|
height_dif = (720 - height) / 2
|
|
|
|
height_difference = self.height_14 - self.height_23
|
|
# print(f"Height difference: {height_difference}")
|
|
if height_difference < -6:
|
|
self.roll_left = True
|
|
self.roll_right = False
|
|
self.roll_ok = False
|
|
print("Turlaj w lewo")
|
|
elif height_difference > 6:
|
|
self.roll_left = False
|
|
self.roll_right = True
|
|
self.roll_ok = False
|
|
print("Turlaj w prawo")
|
|
elif height_difference > -6 and height_difference < 6:
|
|
self.roll_left = False
|
|
self.roll_right = False
|
|
self.roll_ok = True
|
|
print("Roll ok")
|
|
|
|
# print(self.barcode_rect)
|
|
if left != 0:
|
|
if left > (width_dif + 50 + width):
|
|
# print("DRONE RIGHT")
|
|
self.drone_left = False
|
|
self.drone_right = True
|
|
self.drone_rotate_stop = False
|
|
self.drone_up = False
|
|
self.drone_down = False
|
|
elif left < width_dif - 50:
|
|
# print("DRONE LEFT")
|
|
self.drone_left = True
|
|
self.drone_right = False
|
|
self.drone_rotate_stop = False
|
|
self.drone_up = False
|
|
self.drone_down = False
|
|
elif left > width_dif - 50 and left < (width_dif + 50 + width):
|
|
# print("DRONE ROTATION STOP")
|
|
self.drone_left = False
|
|
self.drone_right = False
|
|
self.drone_rotate_stop = True
|
|
|
|
if top != 0:
|
|
if top > (height_dif + 100 + height):
|
|
self.drone_down = True
|
|
self.drone_up = False
|
|
self.drone_straight = False
|
|
if top < height_dif - 100:
|
|
self.drone_down = False
|
|
self.drone_up = True
|
|
self.drone_straight = False
|
|
if top > (height_dif - 100) and top < (height_dif + 100 + height):
|
|
self.drone_down = False
|
|
self.drone_up = False
|
|
self.drone_straight = True
|
|
|
|
def rc_control(self, lr, fb, up, y):
|
|
self.drone.send_rc_control(left_right_velocity=lr, forward_backward_velocity=fb,
|
|
up_down_velocity=up, yaw_velocity=y)
|
|
def mission_func(self):
|
|
self.drone.takeoff()
|
|
self.rc_control(0,0,0,0)
|
|
time.sleep(5)
|
|
cnt = 0
|
|
|
|
while cnt < 450:
|
|
# print(self.roll_left, self.roll_right, self.roll_ok)
|
|
if self.barcode_rect == [0, 0, 0, 0]:
|
|
self.rc_control(0, 0, 0, 20)
|
|
print("noqr")
|
|
|
|
else:
|
|
if self.drone_left == True:
|
|
self.rc_control(0, 0, 0, -10)
|
|
print("left")
|
|
elif self.drone_right == True:
|
|
print("right")
|
|
self.rc_control(0, 0, 0, 10)
|
|
elif self.drone_rotate_stop == True:
|
|
print("straight")
|
|
if self.roll_left == True:
|
|
self.rc_control(-15, 0, 0, 0)
|
|
print("Turlaj sie w lewo")
|
|
elif self.roll_right == True:
|
|
self.rc_control(15, 0, 0, 0)
|
|
print("Turlaj sie w prawo")
|
|
elif self.roll_ok == True:
|
|
print("Turlaj dropsa")
|
|
if self.drone_up == True:
|
|
print("up")
|
|
self.rc_control(0, 0, 15, 0)
|
|
elif self.drone_down == True:
|
|
print("Hubert")
|
|
self.rc_control(0, 0, (-15), 0)
|
|
elif self.drone_straight == True:
|
|
print("straight as an arrow")
|
|
width = self.barcode_rect[2]
|
|
height = self.barcode_rect[3]
|
|
if width > 250 or height > 210:
|
|
self.rc_control(0, 0, 0, 0)
|
|
print("Jesteśmy totalnie zajebiści ale chóbert nie")
|
|
print(self.barcode_data)
|
|
if self.left_90 == True:
|
|
self.drone.rotate_counter_clockwise(90)
|
|
print("90 left")
|
|
self.left_90 = False
|
|
elif self.right_90 == True:
|
|
self.drone.rotate_clockwise(90)
|
|
print("90 right")
|
|
self.right_90 = False
|
|
else:
|
|
self.rc_control(0, 20, 0, 0)
|
|
|
|
time.sleep(0.2)
|
|
cnt += 1
|
|
# print(cnt)
|
|
|
|
self.rc_control(0, 0, 0, 0)
|
|
time.sleep(5)
|
|
self.drone.land()
|
|
|
|
def main(self):
|
|
|
|
#KILLSWITCH
|
|
self.kill_switch = self.TelloKillSwitch(self)
|
|
self.kill_switch.start()
|
|
self.stop_controller = Event()
|
|
|
|
#START TELLO
|
|
self.drone = Tello()
|
|
self.drone.connect()
|
|
|
|
#CAMERA
|
|
self.drone.streamon()
|
|
camera_show = self.Threading(0.001, self.stop_controller, self.show_camera)
|
|
camera_show.start()
|
|
|
|
#BATTERY
|
|
battery_check = self.Threading(25, self.stop_controller, self.battery_temp_check)
|
|
battery_check.start()
|
|
|
|
#FOLLOWQR
|
|
follow_qr = self.Threading(0.3, self.stop_controller, self.follow_qr)
|
|
follow_qr.start()
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def __init__(self):
|
|
self.is_flying = False
|
|
self.has_landed = False
|
|
self.barcode_rect = [0,0,0,0]
|
|
self.drone_left = False
|
|
self.drone_right = False
|
|
self.drone_straight = False
|
|
self.drone_up = False
|
|
self.drone_down = False
|
|
self.drone_rotate_stop = False
|
|
self.barcode_data = ''
|
|
self.height_14 = 0
|
|
self.height_23 = 0
|
|
self.right_90 = False
|
|
self.left_90 = False
|
|
self.roll_left = False
|
|
self.roll_right = False
|
|
self.roll_ok = False
|
|
|
|
self.main()
|
|
self.mission_func()
|
|
|
|
self.drone.streamoff()
|
|
self.drone.end()
|
|
|
|
if _name_ == "_main_":
|
|
td = TelloDrone() |