From 826b31e4e6f933eda8902d25a7fe1729a126c04d Mon Sep 17 00:00:00 2001 From: filipgrabowiecki <150305613+filipgrabowiecki@users.noreply.github.com> Date: Tue, 16 Apr 2024 08:43:51 -0700 Subject: [PATCH] first commit init hubert + requirements --- main.py | 322 +++++++++++++++++++++++++++++++++++++++++++++++ requirements.txt | 5 + 2 files changed, 327 insertions(+) create mode 100644 main.py create mode 100644 requirements.txt diff --git a/main.py b/main.py new file mode 100644 index 0000000..ef87299 --- /dev/null +++ b/main.py @@ -0,0 +1,322 @@ +import threading +import time +from djitellopy import Tello +import csv +import matplotlib.pyplot as plt +import cv2 +from pyzbar.pyzbar import decode +import numpy as np + +global czy_hubert_to_debil +czy_hubert_to_debil = True +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 roll_right + global roll_left + global roll_ok + global barcode_polygon + + global height_14 + global height_23 + + 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.height_14 = 0 + self.height_23 = 0 + + self.roll_left = False + self.roll_right = False + self.roll_ok = False + + def killswitch(self): + while not self.has_landed: + if self.is_flying: + input() + self.drone.emergency() + + 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) + for barcode in decode(cap): + self.barcode_rect = barcode.rect + self.barcode_polygon = barcode.polygon + + + 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) + self.drone.streamoff() + + def follow_qr(self): + while not self.has_landed: + #print(self.barcode_rect) + if self.is_flying: + # 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 + #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() + # self.is_flying = True + # self.has_landed = False + print(f"IS FLYING: {self.is_flying} ") + print(f"HAS LANDED: {self.has_landed} ") + time.sleep(5) + cnt = 0 + while cnt < 250: + #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.roll_left == True: + self.rc_control(10, 0, 0, 0) + print("Turlaj sie w lewo") + elif self.roll_right == True: + self.rc_control(-10, 0, 0, 0) + print("Turlaj sie w prawo") + elif self.roll_ok == True: + print("Turlaj dropsa") + + 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.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") + 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.landing_func() + # self.is_flying = False + # self.has_landed = True + print(f"IS FLYING: {self.is_flying} ") + print(f"HAS LANDED: {self.has_landed} ") + 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() diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..fa2ca4b --- /dev/null +++ b/requirements.txt @@ -0,0 +1,5 @@ +matplotlib~=3.8.4 +opencv-python~=4.9.0.80 +numpy~=1.26.4 +djitellopy~=2.5.0 +pyzbar~=0.1.9 \ No newline at end of file