Skip to content

Instantly share code, notes, and snippets.

@bjpirt
Last active February 10, 2017 17:43
Show Gist options
  • Save bjpirt/a2dd83cc609c180ad9df031b27c978ec to your computer and use it in GitHub Desktop.
Save bjpirt/a2dd83cc609c180ad9df031b27c978ec to your computer and use it in GitHub Desktop.
A quick demo of doing face tracking on the MeArm Pi so that you can make the arm follow you
from picamera.array import PiRGBArray
from picamera import PiCamera
import time
import cv2
from mearm import MeArm
width = 640
arm = MeArm()
arm.base.moveTo(0)
def detect(img, cascade):
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
rects = cascade.detectMultiScale(gray, scaleFactor=1.2, minNeighbors=4, minSize=(30, 30),
flags=cv2.CASCADE_SCALE_IMAGE)
if len(rects) == 0:
return []
rects[:,2:] += rects[:,:2]
return rects
def draw_rects(img, rects, color):
for x1, y1, x2, y2 in rects:
cv2.rectangle(img, (x1, y1), (x2, y2), color, 2)
def find_centre(rects):
max_rect = [999999, 999999, -1, -1]
for x1, y1, x2, y2 in rects:
if x1 < max_rect[0]: max_rect[0] = x1
if y1 < max_rect[1]: max_rect[1] = y1
if x2 > max_rect[2]: max_rect[2] = x2
if y2 > max_rect[3]: max_rect[3] = y2
centre = (max_rect[0] + (max_rect[2] - max_rect[0])/2, max_rect[1] + (max_rect[3] - max_rect[1])/2)
return centre
# initialize the camera and grab a reference to the raw camera capture
camera = PiCamera()
camera.resolution = (width, int(width * 0.75))
faceCascade = cv2.CascadeClassifier('/usr/local/share/OpenCV/haarcascades/haarcascade_frontalface_default.xml')
cv2.startWindowThread()
cv2.namedWindow("viewer")
cv2.moveWindow("viewer", 0, 0)
while True:
rawCapture = PiRGBArray(camera)
# grab an image from the camera
camera.capture(rawCapture, format="bgr")
image = rawCapture.array
rects = detect(image, faceCascade)
print "Found {0} faces!".format(len(rects))
if len(rects) > 0:
draw_rects(image, rects, (255,0,0))
centre = find_centre(rects)
x_diff = centre[0] - (width/2)
print x_diff
angle = (x_diff / (float(width)/2)) * 27
print angle
arm.base.moveBy(-angle)
cv2.circle(image, centre, 10, (255, 0, 0), 1)
# display the image on screen
cv2.imshow("viewer", image)
import os
import math
import pigpio
#import RPi.GPIO as GPIO
pi = pigpio.pi()
class Servo:
def __init__(self, config):
self.pin = config['pin']
self.min = config['min']
self.max = config['max']
self.minAngle = config['minAngle']
self.maxAngle = config['maxAngle']
def moveTo(self, angle):
self.moveToAngle(angle)
def moveBy(self, angle):
newAngle = self.currentAngle + angle
self.moveToAngle(newAngle)
def moveToCentre(self):
centre = self.minAngle + (self.maxAngle - self.minAngle)/2
self.moveToAngle(centre)
def moveToAngle(self, angle):
if angle > self.maxAngle:
angle = self.maxAngle
if angle < self.minAngle:
angle = self.minAngle
self.currentAngle = angle
print("Moving servo %d to %d degrees" % (self.pin, angle))
self.updateServo()
def updateServo(self):
pulseWidth = math.floor(self.min + ((float(self.currentAngle - self.minAngle) / float(self.maxAngle - self.minAngle)) * (self.max - self.min)));
pi.set_servo_pulsewidth(self.pin, pulseWidth)
class MeArm:
def __init__(self):
self.base = Servo({'pin': 4, 'min': 530, 'max': 2400, 'minAngle': -90, 'maxAngle': 90});
self.lower = Servo({'pin': 17, 'min': 530, 'max': 1450, 'minAngle': 0, 'maxAngle': 90});
self.upper = Servo({'pin': 22, 'min': 530, 'max': 2000, 'minAngle': 0, 'maxAngle': 135});
self.grip = Servo({'pin': 10, 'min': 1400, 'max': 2400, 'minAngle': 0, 'maxAngle': 90});
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment