From d9ef3bfcd7951c6060103bc638ff9bcf5a242eb8 Mon Sep 17 00:00:00 2001 From: Cezary <60828381+Pieczarek@users.noreply.github.com> Date: Wed, 17 Apr 2024 19:34:16 +0200 Subject: [PATCH] druga wersja --- main.py | 439 ++++++++++++++++++++++++-------------------------------- 1 file changed, 187 insertions(+), 252 deletions(-) diff --git a/main.py b/main.py index 2cf4a08..e0c55d2 100644 --- a/main.py +++ b/main.py @@ -1,4 +1,4 @@ -import threading +from threading import Thread, Event import time from djitellopy import Tello import csv @@ -6,261 +6,157 @@ import matplotlib.pyplot as plt import cv2 from pyzbar.pyzbar import decode import numpy as np +import keyboard +import os -global czy_hubert_to_debil -czy_hubert_to_debil = True +class TelloDrone: -class TelloDrone(): - drone = Tello() - start = time.time() - global is_flying - global has_landed - global barcode_rect - global drone_left - global drone_right - global drone_straight - global drone_up - global drone_down - global drone_rotate_stop - global barcode_data - global roll_right - global roll_left - global roll_ok - global barcode_polygon - global right_90 - global left_90 - global height_14 - global height_23 + class TelloKillSwitch(Thread): - def __init__(self): - self.drone.connect() - 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 + tc_handler = None - def killswitch(self): - while not self.has_landed: - if self.is_flying: - input() - self.rc_control(0, 0, 0, 0) - self.drone.emergency() + 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 graphs(self): - x = [] - pitch = [] - yaw = [] - roll = [] - - plt.plot(x, pitch, label='pitch') - plt.plot(x, yaw, label='yaw') - plt.plot(x, roll, label='roll') - - cnt = 0 - is_alive = True - - while not self.has_landed: - x.append(cnt) - pitch.append(self.drone.get_pitch()) - yaw.append(self.drone.get_yaw()) - roll.append(self.drone.get_roll()) - - plt.gca().lines[0].set_xdata(x) - plt.gca().lines[0].set_ydata(pitch) - plt.gca().relim() - plt.gca().autoscale_view() - plt.pause(0.1) - - plt.gca().lines[1].set_xdata(x) - plt.gca().lines[1].set_ydata(yaw) - plt.gca().relim() - plt.gca().autoscale_view() - plt.pause(0.1) - - plt.gca().lines[2].set_xdata(x) - plt.gca().lines[2].set_ydata(roll) - plt.gca().relim() - plt.gca().autoscale_view() - plt.pause(0.1) - - cnt += 1 - - if cnt > 20: - # cnt = 0 - pitch = pitch[1:] - yaw = yaw[1:] - roll = roll[1:] - x = x[1:] - # plt.pause(2.0) - # is_alive = False - - def flight_data(self): - while not self.has_landed: - # print(self.is_flying) - if self.is_flying == True: - file = open(f"flight_data.csv", "w") - r = csv.writer(file, delimiter=";") - r.writerow(["BATTERY",'FLIGHT_TIME',"ROLL","PITCH","YAW","VEL_X","VEL_Y","VEL_Z","ACC_X","ACC_Y","ACC_Z"]) - while self.is_flying: - battery = self.drone.get_battery() - flight_time = self.drone.get_flight_time() - roll = self.drone.get_roll() - pitch = self.drone.get_pitch() - yaw = self.drone.get_yaw() - vel_x = self.drone.get_speed_x() - vel_y = self.drone.get_speed_y() - vel_z = self.drone.get_speed_z() - acc_x = self.drone.get_acceleration_x() - acc_y = self.drone.get_acceleration_y() - acc_z = self.drone.get_acceleration_z() - r.writerow([battery, flight_time, roll, pitch,yaw,vel_x,vel_y,vel_z,acc_x,acc_y,acc_z]) - time.sleep(0.1) - file.close() - def show_camera(self): - self.drone.streamon() + 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]) - while not self.has_landed: - 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}") - # 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) - self.drone.streamoff() + 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): - while not self.has_landed: - #print(self.barcode_rect) - if self.is_flying: - 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 + 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") + 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 + # 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 - #print(self.barcode_rect) - time.sleep(0.3) + 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 takeoff_func(self): - self.is_flying = True - self.drone.takeoff() - - def landing_func(self): - self.drone.land() - time.sleep(3) - self.is_flying = False - self.has_landed = True - def mission_func(self): - self.takeoff_func() - print(f"IS FLYING: {self.is_flying} ") - print(f"HAS LANDED: {self.has_landed} ") + 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") + 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: @@ -281,10 +177,10 @@ class TelloDrone(): print("Turlaj dropsa") if self.drone_up == True: print("up") - self.rc_control(0,0,15,0) + self.rc_control(0, 0, 15, 0) elif self.drone_down == True: print("Hubert") - self.rc_control(0,0,(-15),0) + self.rc_control(0, 0, (-15), 0) elif self.drone_straight == True: print("straight as an arrow") width = self.barcode_rect[2] @@ -302,30 +198,69 @@ class TelloDrone(): print("90 right") self.right_90 = False else: - self.rc_control(0,20,0,0) + self.rc_control(0, 20, 0, 0) time.sleep(0.2) cnt += 1 - #print(cnt) + # print(cnt) - self.rc_control(0,0,0,0) + self.rc_control(0, 0, 0, 0) time.sleep(5) - self.landing_func() - print(f"IS FLYING: {self.is_flying} ") - print(f"HAS LANDED: {self.has_landed} ") + 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() -def main(): - drone = TelloDrone() - drone.battery_temp_check() - threading.Thread(target=drone.killswitch).start() - # threading.Thread(target=drone.graphs).start() - threading.Thread(target=drone.show_camera).start() - # threading.Thread(target=drone.flight_data).start() - threading.Thread(target=drone.follow_qr).start() - threading.Thread (target=drone.mission_func).start() - print("\nBateria po misji: \n") - drone.battery_temp_check() - -if __name__ == '__main__': - main() +if _name_ == "_main_": + td = TelloDrone() \ No newline at end of file