Last active
November 9, 2017 08:46
-
-
Save PonDad/ba5d124bb2f303e689d3ac366f2232bf to your computer and use it in GitHub Desktop.
Raspberry Pi サーボモーターとOpen CVで顔追跡カメラ(Haar-like)
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
# -*- coding: UTF-8 -*- | |
import cv2 | |
import os | |
import time | |
import Adafruit_PCA9685 | |
pwm = Adafruit_PCA9685.PCA9685() | |
pwm.set_pwm_freq(60) | |
cascade_path = "haarcascade_frontalface_alt.xml" | |
cascade = cv2.CascadeClassifier(cascade_path) | |
cap = cv2.VideoCapture(0) | |
cap.set(3, 320) | |
cap.set(4, 320) | |
color = (255, 255, 255) | |
while(True): | |
ret, frame = cap.read() | |
now_degree_x, now_degree_y, move_degree_x, move_degree_y = 475, 325, 0, 0 | |
facerect = cascade.detectMultiScale(frame, scaleFactor=1.2, minNeighbors=2, minSize=(10, 10)) | |
for rect in facerect: | |
img_x = rect[0]+rect[2]/2 | |
img_y = rect[1]+rect[3]/2 | |
print(img_x, img_y) | |
move_degree_x = now_degree_x - (img_x-160)*0.4 | |
move_degree_y = now_degree_y + (img_y-160)*0.4 | |
print('deg: ', move_degree_x , move_degree_y) | |
pwm.set_pwm(0, 0, int(move_degree_x)) | |
pwm.set_pwm(1, 0, int(move_degree_y)) | |
time.sleep(0.1) | |
now_degree_x = now_degree_x + move_degree_x | |
now_degree_y = now_degree_y + move_degree_y | |
cv2.circle(frame, (int(img_x), int(img_y)), 10, (255,255,255), -1) | |
cv2.rectangle(frame, tuple(rect[0:2]),tuple(rect[0:2] + rect[2:4]), color, thickness=3) | |
cv2.imshow("Show FLAME Image", frame) | |
k = cv2.waitKey(1) | |
if k == ord('q'): | |
break | |
cap.release() | |
cv2.destroyAllWindows() |
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
# Copyright (c) 2016 Adafruit Industries | |
# Author: Tony DiCola | |
# | |
# Permission is hereby granted, free of charge, to any person obtaining a copy | |
# of this software and associated documentation files (the "Software"), to deal | |
# in the Software without restriction, including without limitation the rights | |
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | |
# copies of the Software, and to permit persons to whom the Software is | |
# furnished to do so, subject to the following conditions: | |
# | |
# The above copyright notice and this permission notice shall be included in | |
# all copies or substantial portions of the Software. | |
# | |
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | |
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | |
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | |
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | |
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | |
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | |
# THE SOFTWARE. | |
from __future__ import division | |
import logging | |
import time | |
import math | |
# Registers/etc: | |
PCA9685_ADDRESS = 0x40 | |
MODE1 = 0x00 | |
MODE2 = 0x01 | |
SUBADR1 = 0x02 | |
SUBADR2 = 0x03 | |
SUBADR3 = 0x04 | |
PRESCALE = 0xFE | |
LED0_ON_L = 0x06 | |
LED0_ON_H = 0x07 | |
LED0_OFF_L = 0x08 | |
LED0_OFF_H = 0x09 | |
ALL_LED_ON_L = 0xFA | |
ALL_LED_ON_H = 0xFB | |
ALL_LED_OFF_L = 0xFC | |
ALL_LED_OFF_H = 0xFD | |
# Bits: | |
RESTART = 0x80 | |
SLEEP = 0x10 | |
ALLCALL = 0x01 | |
INVRT = 0x10 | |
OUTDRV = 0x04 | |
logger = logging.getLogger(__name__) | |
def software_reset(i2c=None, **kwargs): | |
"""Sends a software reset (SWRST) command to all servo drivers on the bus.""" | |
# Setup I2C interface for device 0x00 to talk to all of them. | |
if i2c is None: | |
import Adafruit_GPIO.I2C as I2C | |
i2c = I2C | |
self._device = i2c.get_i2c_device(0x00, **kwargs) | |
self._device.writeRaw8(0x06) # SWRST | |
class PCA9685(object): | |
"""PCA9685 PWM LED/servo controller.""" | |
def __init__(self, address=PCA9685_ADDRESS, i2c=None, **kwargs): | |
"""Initialize the PCA9685.""" | |
# Setup I2C interface for the device. | |
if i2c is None: | |
import Adafruit_GPIO.I2C as I2C | |
i2c = I2C | |
self._device = i2c.get_i2c_device(address, **kwargs) | |
self.set_all_pwm(0, 0) | |
self._device.write8(MODE2, OUTDRV) | |
self._device.write8(MODE1, ALLCALL) | |
time.sleep(0.005) # wait for oscillator | |
mode1 = self._device.readU8(MODE1) | |
mode1 = mode1 & ~SLEEP # wake up (reset sleep) | |
self._device.write8(MODE1, mode1) | |
time.sleep(0.005) # wait for oscillator | |
def set_pwm_freq(self, freq_hz): | |
"""Set the PWM frequency to the provided value in hertz.""" | |
prescaleval = 25000000.0 # 25MHz | |
prescaleval /= 4096.0 # 12-bit | |
prescaleval /= float(freq_hz) | |
prescaleval -= 1.0 | |
logger.debug('Setting PWM frequency to {0} Hz'.format(freq_hz)) | |
logger.debug('Estimated pre-scale: {0}'.format(prescaleval)) | |
prescale = int(math.floor(prescaleval + 0.5)) | |
logger.debug('Final pre-scale: {0}'.format(prescale)) | |
oldmode = self._device.readU8(MODE1); | |
newmode = (oldmode & 0x7F) | 0x10 # sleep | |
self._device.write8(MODE1, newmode) # go to sleep | |
self._device.write8(PRESCALE, prescale) | |
self._device.write8(MODE1, oldmode) | |
time.sleep(0.005) | |
self._device.write8(MODE1, oldmode | 0x80) | |
def set_pwm(self, channel, on, off): | |
"""Sets a single PWM channel.""" | |
self._device.write8(LED0_ON_L+4*channel, on & 0xFF) | |
self._device.write8(LED0_ON_H+4*channel, on >> 8) | |
self._device.write8(LED0_OFF_L+4*channel, off & 0xFF) | |
self._device.write8(LED0_OFF_H+4*channel, off >> 8) | |
def set_all_pwm(self, on, off): | |
"""Sets all PWM channels.""" | |
self._device.write8(ALL_LED_ON_L, on & 0xFF) | |
self._device.write8(ALL_LED_ON_H, on >> 8) | |
self._device.write8(ALL_LED_OFF_L, off & 0xFF) | |
self._device.write8(ALL_LED_OFF_H, off >> 8) | |
servo_min = 150 # Min pulse length out of 4096 | |
servo_max = 600 # Max pulse length out of 4096 | |
def set_servo_pulse(channel, pulse): | |
pulse_length = 1000000 # 1,000,000 us per second | |
pulse_length //= 60 # 60 Hz | |
print('{0}us per period'.format(pulse_length)) | |
pulse_length //= 4096 # 12 bits of resolution | |
print('{0}us per bit'.format(pulse_length)) | |
pulse *= 1000 | |
pulse //= pulse_length | |
pwm.set_pwm(channel, 0, pulse) |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment