Okay, open up the face_tracking.py script and replace this part:
while (True):
clock.tick()
img = sensor.snapshot()
# Extract keypoints using the detect face size as the ROI
kpts2 = img.find_keypoints(threshold=KEYPOINTS_THRESH, normalized=NORMALIZED)
if (kpts2):
# Match the first set of keypoints with the second one
c=image.match_descriptor(image.FREAK, kpts1, kpts2, threshold=MATCHING_THRESH)
# If more than 10% of the keypoints match draw the matching set
if (c[2]>25):
img.draw_cross(c[0], c[1], size=5)
img.draw_string(0, 10, "Match %d%%"%(c[2]))
# Draw FPS
img.draw_string(0, 0, "FPS:%.2f"%(clock.fps()))
With this:
from pyb import Servo
x_pos = 1800 # default
y_pos = 1500 # default
x_min = 1400
x_max = 2200
y_max = 1900
y_min = 1100
x_gain = +1.00 # You have to tweak this value to stablize the control loop.
# You also may need to invert the value if the system goes
# in the wrong direction.
y_gain = +1.00 # You have to tweak this value to stablize the control loop.
# You also may need to invert the value if the system goes
# in the wrong direction.
xServo = Servo(0)
yServo = Servo(1)
xServo.pulse_width(x_pos)
yServo.pulse_width(y_pos)
while (True):
clock.tick()
img = sensor.snapshot()
# Extract keypoints using the detect face size as the ROI
kpts2 = img.find_keypoints(threshold=KEYPOINTS_THRESH, normalized=NORMALIZED)
if (kpts2):
# Match the first set of keypoints with the second one
c=image.match_descriptor(image.FREAK, kpts1, kpts2, threshold=MATCHING_THRESH)
# If more than 10% of the keypoints match draw the matching set
if (c[2]>25):
img.draw_cross(c[0], c[1], size=5)
img.draw_string(0, 10, "Match %d%%"%(c[2]))
x = c[0]
y = c[1]
x_error = x - (img.width()/2)
y_error = y - (img.height()/2)
x_pos += x_error * x_gain
y_pos += y_error * y_gain
# Clamp output between min and max
if (x_pos > x_max):
x_pos = x_max
if (x_pos < x_min):
x_pos = x_min
# Clamp output between min and max
if (y_pos > y_max):
y_pos = y_max
if (y_pos < y_min):
y_pos = y_min
xServo.pulse_width(x_pos)
yServo.pulse_width(y_pos)
# Draw FPS
img.draw_string(0, 0, "FPS:%.2f"%(clock.fps()))
I haven’t actually checked if the code compiles. However, you should be able to see what I am trying to do. Basically, the control loop will increment/decrement the xPosition/yPosition of each servo. The incrementing/decrementing is clamped between a min and max. The increment/decrement is based on the face position difference from the middle of the image. So, the more the face is away from the middle of the image the stronger the response.
Your job to to make sure that you have the x output assigned to the servo that pans horizontally and the y output connected to the servo than tilts vertically. Then you need to tweak the gain value until you find something that works really well. The gain value can either be negative or positive. If its wrong you’ll notice the control loop breaking more or less right away. So, just flip the sign if that happens. If the gain value is too small the system will barely respond. If its too large the system will go crazy.
Anyway, that’s about it. Let me know if you have problems. Also, note that I’m only updating the proportional control code when a face is tracked with enough confidence.