今天实现了对物体检测并控制舵机运动的应用。
舵机使用了pca9685控制，因此备份一下pac9685舵机控制板的驱动：

PCA9685.py

import time
import math

class PWM:
    _mode_adr      = 0x00
    _base_adr_low  = 0x08
    _base_adr_high = 0x09
    _prescale_adr  = 0xFE

    def __init__(self, bus, address = 0x40):
        '''
        Creates an instance of the PWM chip at given i2c address.
        @param bus: the SMBus instance to access the i2c port(0 or 1).
        @param address: the address of the i2c chip (default: 0x40).
        '''
        self.bus = bus
        self.address = address
        self._writeByte(self._mode_adr, 0x00)

    def setFreq(self, freq):
        '''
        Sets the PWM frequency. The value is stored in the device.
        @param freq: the frequency in Hz (approx.)
        '''
        prescaleValue = 25000000.0  # 25MHz
        prescaleValue /= 4096.0     # 12-bit
        prescaleValue /= float(freq)
        prescaleValue -= 1.0  
        prescale = math.floor(prescaleValue + 0.5)
        oldmode = self._readByte(self._mode_adr)
        if oldmode == None:
            return
        newmode = (oldmode & 0x7F) | 0x10
        self._writeByte(self._mode_adr, newmode)
        self._writeByte(self._prescale_adr, int(math.floor(prescale)))
        self._writeByte(self._mode_adr, oldmode)
        time.sleep(0.005)
        self._writeByte(self._mode_adr, oldmode | 0x80)

    def setDuty(self, channel, duty):
        '''
        Sets a single PWM channel. The value is stored in the device.
        @param channel: one of the channels 0..15
        @param duty: the duty cycle 0..100
        '''
        data = int(duty * 4096 /100)  # 0..4096 (included)
        self._writeByte(self._base_adr_low + 4 * channel, data & 0xFF)
        self._writeByte(self._base_adr_high + 4 * channel, data >> 8)

    def _writeByte(self, reg, value):
        try:
            self.bus.write_byte_data(self.address, reg, value)
        except:
            print("Error while writing to I2C device")

    def _readByte(self, reg):
        try:
            result = self.bus.read_byte_data(self.address, reg)
            return result
        except:
            print("Error while Reading from I2C device")
            return None

调用方法简单：

import time
from smbus2 import SMBus
from pca9685 import PWM

freq = 50
addr = 0x40
channels = [0, 1, 2] 
a = 12.5
b = 2

bus = SMBus(1)
pwm = PWM(bus, addr)
pwm.setFreq(freq)

def setPos(channel, pos):
    duty = a / 180 * pos + b 
    pwm.setDuty(channel, duty)
    time.sleep(0.1)

while True:
    try:
        for pos in range(60, 120, 2):
            setPos(channels[0], pos)
            time.sleep(0.1)

        for pos in range(60, 120, 3):
            setPos(channels[1], pos)
            time.sleep(0.5)

        for pos in range(0, 90, 3):
            setPos(channels[2], pos)
            time.sleep(0.01)

    except KeyboardInterrupt:
        for i in channels:
            setPos(i, 0)
        break

OpenCV 通过颜色检测并控制：

import cv2
import time
import numpy as np
from pca9685 import PWM
from smbus2 import SMBus

freq = 50
addr = 0x40
ch1  = 0
ch2  = 1

bus = SMBus(1)
pwm = PWM(bus, addr)
pwm.setFreq(freq)

def setPos(channel, position):
    pwm.setDuty(channel, position)
    time.sleep(0.01)

def posMap(x, in_min, in_max, out_min, out_max):
    return int((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)

cap = cv2.VideoCapture(0)
cap.set(3, 320)
cap.set(4, 160)
ret, frame = cap.read()

cx = int(frame.shape[1] / 2)
center = int(frame.shape[1] / 2)
pos = 90

while True:
    ret, frame = cap.read()
    frame = cv2.flip(frame, 1)
    hsv_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    ly = np.array([51, 0, 0])
    hy = np.array([180, 255, 255])

    mask = cv2.inRange(hsv_frame, ly, hy)
    contours, hir = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
    contours = sorted(contours, key=lambda x: cv2.contourArea(x), reverse=True)
    # cv2.drawContours(frame, contours[1], -1, (0, 0, 255), 2)
    for cnt in contours[1:]:
        (x, y, w, h) = cv2.boundingRect(cnt)
        cv2.rectangle(frame, (x, y), (x+w, y+h), (255, 255,0), 2)
        cx = int((x + x + w)/ 2)
        # cv2.drawContours(frame, cnt, -1, (0, 0, 255), 2)
        cv2.line(frame, (cx, 0), (cx, frame.shape[0]), (0, 0, 255), 1)
        if cx < center:
            pos -= 1.5
        elif cx > center:
            pos += 1.5  
        # pos = posMap(pos, 1, 480, 60, 120)
        setPos(ch1, pos)

        break
    # mask = cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR)
    # hstack = np.hstack([mask, frame])
    cv2.imshow('frame', frame)
    if cv2.waitKey(1) & 0xFF == 27:
        break

cap.release()
cv2.destroyAllWindows()
setPos(ch1, 0)

总结

就是控制dutyCycle，频率肯定要50hz，一般的多级可能需要a， b函数作为微调。根据自己的舵机微调a和b的值。

可以尝试 a 设置8.5， b设置2