druga wersja

main.py

@@ -1,4 +1,4 @@
-import threading
+from threading import Thread, Event
 import time
 from djitellopy import Tello
 import csv
@@ -6,138 +6,52 @@ 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)
+        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()
-
-        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)
@@ -158,13 +72,9 @@ class TelloDrone():
             # print(barcode.polygon)
         cv2.imshow('frame', cap)
         cv2.waitKey(1)
-        self.drone.streamoff()
 
     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
@@ -176,11 +86,11 @@ class TelloDrone():
         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
+        width_dif = (1280 - width) / 2
+        height_dif = (720 - height) / 2
 
         height_difference = self.height_14 - self.height_23
-                #print(f"Height difference: {height_difference}")
+        # print(f"Height difference: {height_difference}")
         if height_difference < -6:
             self.roll_left = True
             self.roll_right = False
@@ -232,35 +142,21 @@ class TelloDrone():
                 self.drone_down = False
                 self.drone_up = False
                 self.drone_straight = True
-            #print(self.barcode_rect)
-            time.sleep(0.3)
 
     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()