今天实现了对物体检测并控制舵机运动的应用。
舵机使用了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
