update control face tracking with servo

python/examples/face_tracker_servo.py

+import time  
+from ctypes import *
+import ctypes 
+import cv2
+
+btm_kp = 4 
+top_kp = -4 
+ 
+offset_dead_block = 0.1 
+                      
+so = ctypes.cdll.LoadLibrary   
+pwm = so("./libPCA9685.so")
+# Set frequency to 50hz, good for servos.  
+pwm.setPWMFreq(50)  
+
+
+FaceCascade = cv2.CascadeClassifier('./haarcascade_frontalface_alt2.xml')
+
+cv2.namedWindow('FaceDetect',flags=cv2.WINDOW_NORMAL)
+cap = cv2.VideoCapture(0)
+cap.set(3,320)
+cap.set(4,160)
+#cap.set(cv2.CAP_PROP_BUFFERSIZE,1)
+
+def btm_servo_control(offset_x):
+
+    global offset_dead_block 
+    global btm_kp 
+    global last_btm_degree 
+    
+    if abs(offset_x) < offset_dead_block:
+       offset_x = 0
+
+    delta_degree = offset_x * btm_kp
+    next_btm_degree = last_btm_degree + delta_degree
+
+    if next_btm_degree < 0:
+        next_btm_degree = 0
+    elif next_btm_degree > 180:
+        next_btm_degree = 180
+    
+    return int(next_btm_degree)
+
+def top_servo_control(offset_y):
+
+    global offset_dead_block
+    global top_kp 
+    global last_top_degree 
+
+    if abs(offset_y) < offset_dead_block:
+        offset_y = 0
+
+    delta_degree = offset_y * top_kp
+
+    next_top_degree = last_top_degree + delta_degree
+
+    if next_top_degree < 0:
+        next_top_degree = 0
+    elif next_top_degree > 180:
+        next_top_degree = 180
+    
+    return int(next_top_degree)
+
+def face_filter(faces):
+
+    if len(faces) == 0:
+        return None
+    
+    max_face =  max(faces, key=lambda face: face[2]*face[3])
+    (x, y, w, h) = max_face
+    if w < 10 or h < 10:
+        return None
+    return max_face
+
+def calculate_offset(img_width, img_height, face):
+
+    (x, y, w, h) = face
+    face_x = float(x + w/2.0)
+    face_y = float(y + h/2.0)
+
+    offset_x = float(face_x / img_width - 0.5) * 2
+    offset_y = float(face_y / img_height - 0.5) * 2
+
+    return (offset_x, offset_y)
+	
+	
+def update_btm_kp(value):
+    global btm_kp
+    btm_kp = value
+
+def update_top_kp(value):
+    global top_kp
+    top_kp = value
+	
+def set_servo_angle(channel, angle):             
+    date=4096*((angle*11)+500)/20000              
+    pwm.setPWM(channel, int(date))  
+   
+def get_servo_angle(channel):  
+    date=pwm.getPWM(channel+1)       
+    angle=(20000*date - 500*11)/(4096*11) -45
+    print(str(channel) +':'+ str(int(angle)))  
+    return angle
+  
+
+last_btm_degree=get_servo_angle(1)
+last_top_degree=get_servo_angle(2)        
+
+  
+while cap.isOpened():
+    ret, img = cap.read()
+    img = cv2.flip(img, 1)
+    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+    faces = FaceCascade.detectMultiScale(
+        gray,
+        scaleFactor=1.1,
+        minNeighbors=5
+    )
+
+    face = face_filter(faces)
+    if face is not None:
+        (x, y, w, h) = face
+        cv2.rectangle(img, (x, y), (x+w, y+h), (0, 255, 0), 4)
+
+        img_height, img_width,_ = img.shape
+        print("img h:{} w:{}".format(img_height, img_width))
+        (offset_x, offset_y) = calculate_offset(img_width, img_height, face)
+        next_btm_degree = btm_servo_control(offset_x)
+        next_top_degree = top_servo_control(offset_y)
+        print(next_btm_degree)
+        print(next_top_degree)
+
+        #set_cloud_platform_degree(next_btm_degree, next_top_degree)
+        set_servo_angle(1, next_btm_degree)
+        set_servo_angle(2, next_top_degree)
+
+        last_btm_degree = next_btm_degree
+        last_top_degree = next_top_degree
+
+    cv2.imshow('FaceDetect', img)
+    key = cv2.waitKey(1)
+    if key == ord('q'):
+
+        break
+    elif key == ord('r'):
+        print('reset')
+
+        last_btm_degree = 160
+        last_top_degree = 30
+        #set_cloud_platform_degree(last_btm_degree, last_top_degree)
+        set_servo_angle(1, last_btm_degree)
+        set_servo_angle(2, last_top_degree)
+
+cap.release()
+cv2.destroyAllWindows()
+