Skip to content

Instantly share code, notes, and snippets.

@PonDad PonDad/PCA9685.py
Last active Apr 4, 2018

Embed
What would you like to do?
Raspberry Pi サーボモーターとOpen CVで物体追跡カメラ(Meanshift)
# -*- coding: utf-8 -*-
import numpy as np
import cv2
import time
import Adafruit_PCA9685
pwm = Adafruit_PCA9685.PCA9685()
pwm.set_pwm_freq(60)
cap = cv2.VideoCapture(0)
cap.set(3, 320)
cap.set(4, 320)
x, y, w, h = 75, 75, 100, 100
track_window = (x, y, w, h)
ret,frame = cap.read()
roi = frame[y:y+h, x:x+w]
hsv_roi = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)
img_mask = cv2.inRange(hsv_roi, np.array((0., 60.,32.)), np.array((180.,255.,255.)))
roi_hist = cv2.calcHist([hsv_roi], [0], img_mask, [180], [0,180])
cv2.normalize(roi_hist, roi_hist, 0, 255, cv2.NORM_MINMAX)
term_crit = ( cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 1 )
while(True):
ret, frame = cap.read()
now_degree_x, now_degree_y, move_degree_x, move_degree_y = 475, 325, 0, 0
if ret == True:
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
dst = cv2.calcBackProject([hsv],[0],roi_hist,[0,180], 1)
ret, track_window = cv2.meanShift(dst, track_window, term_crit)
x,y,w,h = track_window
img_dst = cv2.rectangle(frame, (x,y), (x+w, y+h), (255,255,255), 3)
img_x = int(x+w/2)
img_y = int(y+h/2)
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(img_dst, (img_x, img_y), 10, (0,180,0), -1)
cv2.imshow('SHOW MEANSHIFT IMAGE', img_dst)
k = cv2.waitKey(1)
if k == ord('q'):
break
else:
break
cap.release()
cv2.destroyAllWindows()
# https://github.com/adafruit/Adafruit_Python_PCA9685 を少し書き換えています
# 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)
from __future__ import division
import time
import Adafruit_PCA9685
pwm = Adafruit_PCA9685.PCA9685()
pwm.set_pwm_freq(60)
def move(degree_1,degree_2):
degree_1 = int(degree_1 * 5.27)
degree_2 = int(degree_2 * 5.27)
pwm.set_pwm(0, 0, degree_1)
pwm.set_pwm(1, 0, degree_2)
if __name__ == '__main__':
print('動きます...')
move(45,45)
time.sleep(2)
move(135,135)
time.sleep(2)
move(90,60)
time.sleep(2)
print('止まります')
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.