Last active
February 10, 2017 17:43
-
-
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
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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