for (x,y,w,h) in faces:
cv2.rectangle(img,(x,y),(x+w,y+h),(255,0,0),2)
center_x.append(x+w/2.0)
center_y.append(y+h/2.0)
print(len(center_x))
if(len(center_x)>0):
a=center_x[0]
for i in range((len(center_x))-1):
if(abs(CENTER-center_x[i+1])<abs(CENTER-a)):
a=center_x[i+1]
print(a, "Following center",CENTER)
current_difference = (a-CENTER)
print ("Difference left %s" % (TDifference - current_difference))
pidout = pid.compute_output(current_difference)
pidout += baseAngle
val=pidout
if(a > CENTER ):
print("<<<<<<<<<")
elif(a< CENTER):
print(">>>>>>>>>")
if(val>180 or val<0):
val=90
# ser.write("%s\n"%val)
print("val",val)
baseAngle=val
cv2.imshow('img',img)
k = cv2.waitKey(30) & 0xff
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2]
center = None
if len(cnts) > 0:
c = max(cnts, key=cv2.contourArea)
((x, y), radius) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
# print (600 - center[0])
# print(center)
pidout = int(pid.compute_output(600 - center[1]))
pidout += baseThrust
print("Height = ", 600 - center[1], "px, ", "Thrust = ", pidout)
cf.commander.send_setpoint(rollTrim, pitchTrim, yawTrim, pidout)
if radius > 10:
cv2.circle(frame, (int(x), int(y)), int(radius), (0, 255, 255), 2)
cv2.circle(frame, center, 5, (0, 0, 255), -1)
else:
print("Out of bounds.")
cf.commander.send_setpoint(rollTrim, pitchTrim, yawTrim, 40000)
pass
# pts.appendleft(center)
