python/face_tracking.py at main · zane1502/python · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
import time
import djitellopy as tello
import cv2
import numpy as np

global img
drone = tello.Tello()
drone.connect()
battery = drone.get_battery()
print(battery)

drone.streamon()
drone.takeoff()
drone.send_rc_control(0,0,45,0)
time.sleep(1)

w,h = 360,240

fbRange = [6200,6800]

pid = [0.6,0.6,0]

pError = 0

def findFace(img):
    faceCascade = cv2.CascadeClassifier('haarcascade_frontalface_default.xml')
    imgGray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    faces = faceCascade.detectMultiScale(imgGray, 1.2,8)

    myFaceListC = []
    myFaceListCArea = []

    for (x,y,w,h) in faces:
        cv2.rectangle(img, (x,y), (x+w, y+h), (0,0, 255),2)

        cx = x+ w //2

        cy = y +h //2

        area = w*h

        cv2.circle(img,(cx,cy), 5,(0,255,0), cv2.FILLED)

        myFaceListC.append([cx,cy])

        myFaceListC.append(area)

    if len(myFaceListCArea) != 0:

        i = myFaceListCArea.index(max(myFaceListCArea))

        return img, [myFaceListC[i], myFaceListCArea[i]]

    else:

            return img, [[0,0],0]

    def trackFace(info, w, pid, pError):
        area = info[1]
        x, y = info[0]
        fb = 0
        error = x - w//2
        speed = pid[0] * error + pid[1] * (error-pError)
        speed = int(np.clip(speed, -100,100))
        if area > fbRange[0] and area < fbRange[1]:
            fb = 0
        elif area > fbRange[1]:
            fb  = -20
        elif area < fbRange[0] and area != 0:
            fb = 20

        if x == 0:
            speed = 0
            error = 0
        # print (speed,fb)
        drone.send_rc_control(0,fb,0,speed)
        return error

    # cap = cv2.VideoCapture(0)
    while True:
        # _, img = cap.read()
        img = drone.get_frame_read().frame
        img = cv2.resize(img,(w,h))
        img, info = findFace(img)
        pError = trackFace(info, w, pid, pError)
        # print('Centre', info [0], 'Area', info[1])
        cv2.imshow('Output', img)

        if cv2.waitKey(1) & 0xFF == ord('l'):
            drone.land()
            break