# 下載範例程式
~$ wget http://max543.com/debugger/class/python02/人臉辨識與物體辨識/code/ch7.zip
~$ unzip ch7.zip



### Lab7-1 ###

# Tello 原廠官網
https://www.ryzerobotics.com/zh-tw/tello

# SDK PDF 文件
https://reurl.cc/2WOR2X



### Lab7-2 ###

# 安裝 tello-python 模組
>>> pip install tello-python

# tello-python 模組網址
https://pypi.org/project/tello-python



### Lab7-3 (7-1-tello-python.py) ###

# 使用 tello-python 模組，連線 Tello 並設定為 SDK 的 command 模式
import tello

drone = tello.Tello()



### Lab7-4 ###

>>> drone.command()		# 發送 command，設定 Tello 為 SDK 模式 (預設 drone = tello.Tello() 已設定過了)

>>> drone.takeoff()		# 自動起飛

>>> drone.forword(100)		# 向前飛 x 釐米 (x = 20-500)

>>> drone.cw(90)			# 順時針旋轉 x 度 (x = 1-360)

>>> drone.flip('r')		# 朝左 ('l')、右 ('r')、前 ('f')、後 ('b') 方向翻滾

>>> drone.streamon()		# 開啟視頻串流

>>> drone.streamoff()		# 關閉視頻串流

>>> drone.land()			# 自動降落



### Lab7-5 (7-2-tello_command.py) ###

import socket								# socket 模組: 是通訊中的一種方式，主要用來處理客戶端與伺服器端之串連，只需要 protocol、IP、Port 三項目即可進行網路串連。
import threading								# threading 模組: 一般 Python 在執行時，通常是採用同步的任務處理模式 (一個處理完成後才會接下去處理第二個 )，然而 Python 的標準函式 threading 採用執行緒的方式，運用多個執行緒，在同一時間內處理多個任務 (非同步)。
import time
import sys									# sys 模組: 用來處理 Python 執行時配置以及資源，從而可以與當前程式之外的系統環境互動。

tello_address = ('192.168.10.1', 8889)					# Tello 預設的 IP 與 UDP port
local_address = ('你的電腦 IP', 9000)					# 你的電腦 IP 與欲使用的 UDP port

sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)		# 建立一個 UDP 連結以發送指令。socket.AF_INET: 於伺服器與伺服器之間進行串接，socket.SOCK_DGRAM: 使用 UDP() 的方式通用的免連線訊息交換通道
sock.bind(local_address)							# 用於伺服器端 (你的電腦) 需監聽的 IP 和 UDP Port

def send(message, delay):							# 定義發送訊息給 Tello 的函式
    try:										# 嘗試發送消息，否則顯示異常
        sock.sendto(message.encode(), tello_address)					# 可用於傳送資料過去給串接對象。message.encode(): 欲傳送指令字串與編碼
        print('Sending message: ' + message)						# 顯示欲傳送之指令字串
    except Exception as e:								# 顯示異常
        print('Error sending: ' + str(e))							# 顯示 Error
    if message == 'takeoff':							# 如果是 'takeoff' (自動起飛)，則設定延遲 8 秒後，才能輸入下一個指令
        delay = 8
    elif message == 'land':								# 如果是 'land' (自動降落)，則設定延遲 6 秒後，才能輸入下一個指令
        delay = 6
        
    time.sleep(delay)								# 暫停
    
def receive():												# 定義從 Tello 接收訊息的函式
    while True:													# 循環迴圈接收訊息
        try:															# 嘗試接收消息，否則打印異常
            response, ip_address = sock.recvfrom(128)										# 用於接收資料，並會回傳 (data, addr) 接收到的資料和IP位址資訊。128: 為宣告接收最多字數值
            print('Received message: ' + response.decode(encoding = 'utf-8'))						# 顯示回傳的 data (編碼成 'utf-8')
        except Exception as e:												# 如果有錯誤關閉 socket，並跳出循環
            sock.close()														# 關閉 socket
            print('socket closed ==> Error receiving: ' + str(e))								# 顯示 Error
            break															# 離開 while
      
receiveThread = threading.Thread(target = receive)			# 建立並在背景執行監聽緒
receiveThread.daemon = True							# 此執行緒使用上述之接收訊息的函式，持續不斷的運行
receiveThread.start()							# 開啟 執行緒

print("請輸入 Tello SDK 指令，並按下 'Enter' 鍵，或輸入 'quit' 離開程式。")		# 操作指示

while True:										# 一直循環等待指令的輸入或是使用 'quit' 或 ctrl-c 退出
    try:											# 使用 try
        message = input(':: ')								# 讀取鍵盤輸入 
        if 'quit' in message:									# 當收到 'quit' 指令，則結束程式並關閉連結
            print('Program exited sucessfully')							# 顯示程式結束
        	sock.close()										# 關閉 socket
        	break												# 離開 while
    
        send(message, 3)									# 送出指令
    
    except KeyboardInterrupt as e:							# 處理當使用 ctrl-c 時之例外處理
        sock.close()										# 關閉 socket
        break											# 離開 while



### Lab7-6 (7-3a-look4corner.py) ###

import socket
import threading
import time

tello_address = ('192.168.10.1', 8889)
local_address = ('你的電腦 IP', 9000)

sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind(local_address)

def send(message, delay):
    try:
        sock.sendto(message.encode(), tello_address)
        print('Sending message: ' + message)
    except Exception as e:
        print('Error sending: ' + str(e))
    time.sleep(delay)

def receive():
    while True:
        try:
            response, ip_address = sock.recvfrom(128)
            print('Received message: ' + response.decode(encoding = 'utf-8'))
        except Exception as e:
            sock.close()
            print('socket closed ==> Error receiving: ' + str(e))
            break
 
receiveThread = threading.Thread(target = receive)
receiveThread.daemon = True
receiveThread.start()

box_leg_distance = 100
yaw_angle = 90
yaw_direction = 'cw'
send('command', 2)
send('takeoff', 8)

for i in range(4):
    send('forward ' + str(box_leg_distance), 4)
    send('cw ' + str(yaw_angle), 4)
    
send('land', 5)

print('Mission completed successfully!')
sock.close()



### Lab7-7 (7-3b-look4corner.py) ###

import socket
import threading
import time
import tello						# 匯入 tello-python 模組

tello_address = ('192.168.10.1', 8889)
local_address = ('192.168.10.2', 9000)
drone = tello.Tello()					# 宣告 Tello() 物件

sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind(local_address)

def send(message, delay):
    try:
        sock.sendto(message.encode(), tello_address)
        print('Sending message: ' + message)
    except Exception as e:
        print('Error sending: ' + str(e))
    time.sleep(delay)

def receive():
    while True:
        try:
            response, ip_address = sock.recvfrom(128)
            print('Received message: ' + response.decode(encoding = 'utf-8'))
        except Exception as e:
            sock.close()
            print('socket closed ==> Error receiving: ' + str(e))
            break
 
receiveThread = threading.Thread(target = receive)
receiveThread.daemon = True
receiveThread.start()

box_leg_distance = 100
yaw_angle = 90
yaw_direction = 'cw'
send('command', 2)
send('takeoff', 8)
drone.streamon()						# 開啟視頻串流

for i in range(4):
    send('forward ' + str(box_leg_distance), 4)
    send('cw ' + str(yaw_angle), 4)
    
drone.streamoff()						# 關閉視頻串流
send('land', 5)

print('Mission completed successfully!')
sock.close()



### Lab7-8 (7-4-starFlight.py) ###

import socket
import threading
import time
import tello

tello_address = ('192.168.10.1', 8889)
local_address = ('你的電腦 IP', 9000)
drone = tello.Tello()

sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind(local_address)

def send(message, delay = 3):
    try:
        sock.sendto(message.encode(), tello_address)
        print('Sending message: ' + message)
    except Exception as e:
        print('Error sending: ' + str(e))
        if message == 'takeoff':  
            delay = 8
        elif message == 'land':  
            delay = 6
    time.sleep(delay)
    
def receive():
    while True:
        try:
            response, ip_address = sock.recvfrom(128)
            print('Received message: ' + response.decode(encoding = 'utf-8'))
        except Exception as e:
            sock.close()
            print('Error receiving: ' + str(e))
            break

receiveThread = threading.Thread(target = receive)
receiveThread.daemon = True
receiveThread.start()
  
leg_distance = 60
yaw_degrees = 144

send('command', 1)
send('takeoff', 7)
drone.streamon()

for i in range(5):
    send('forward ' + str(leg_distance), 5)
    send('cw ' + str(yaw_degrees), 5)

drone.streamoff()
send('land', 5)

print('Mission completed successfully!')
sock.close()



### Lab7-9 (7-5-Tello-Face-Recognition) ###

import numpy as np
import cv2 as cv
import tello_drone as tello

host = '192.168.10.1'
port = 8889
local_address = (host, port)

drone = tello.Tello(host, port, is_dummy = False)

def adjust_tello_position(offset_x, offset_y, offset_z):
    """
    Adjusts the position of the tello drone based on the offset values given from the frame
    :param offset_x: Offset between center and face x coordinates
    :param offset_y: Offset between center and face y coordinates
    :param offset_z: Area of the face detection rectangle on the frame
    """
    if not -90 <= offset_x <= 90 and offset_x is not 0:
        if offset_x < 0:
            drone.rotate_ccw(10)
        elif offset_x > 0:
            drone.rotate_cw(10)
    
    if not -70 <= offset_y <= 70 and offset_y is not -30:
        if offset_y < 0:
            drone.move_up(20)
        elif offset_y > 0:
            drone.move_down(20)
    
    if not 15000 <= offset_z <= 30000 and offset_z is not 0:
        if offset_z < 15000:
            drone.move_forward(20)
        elif offset_z > 30000:
            drone.move_backward(20) 

face_cascade = cv.CascadeClassifier('cascades/haarcascade_frontalface_default.xml')
frame_read = drone.get_frame_read()

while True:
    frame = frame_read.frame
    cap = drone.get_video_capture()

    height = cap.get(cv.CAP_PROP_FRAME_HEIGHT)
    width = cap.get(cv.CAP_PROP_FRAME_WIDTH)

    center_x = int(width/2)
    center_y = int(height/2)

    cv.circle(frame, (center_x, center_y), 10, (0, 255, 0))
    
    gray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)
    faces = face_cascade.detectMultiScale(gray, 1.3, minNeighbors=5)

    face_center_x = center_x
    face_center_y = center_y
    z_area = 0

    for face in faces:
        (x, y, w, h) = face
        cv.rectangle(frame, (x, y), (x + w, y + h),(255, 255, 0), 2)

        face_center_x = x + int(h/2)
        face_center_y = y + int(w/2)
        z_area = w * h

        cv.circle(frame, (face_center_x, face_center_y), 10, (0, 0, 255))

    offset_x = face_center_x - center_x
    offset_y = face_center_y - center_y - 30

    cv.putText(frame, f'[{offset_x}, {offset_y}, {z_area}]', (10, 50), cv.FONT_HERSHEY_SIMPLEX, 2, (255,255,255), 2, cv.LINE_AA)
    adjust_tello_position(offset_x, offset_y, z_area)
    
    cv.imshow('Tello detection...', frame)
    if cv.waitKey(1) == ord('q'):
        break

drone.end()
cv.destroyAllWindows()