在OPENMV官方给的例程里只有PCA9685例程,似乎作用不大,有一个实例是追小球的云台,但是他是直接用OPENMV自带的舵机IO口实现的,如果工程量大,比如又要用到串口外加三个舵机,那这样IO之间会有所影响,如果直接用PCA9685,那只要用到I2C就可以了。而且也不用买官方的舵机扩展板,直接在某宝上买PCA9685模块即可,一共可以控制16路舵机。而且只要10块钱左右。直接上代码。
main.py
# main - By: DN36 - 周四 1月 28 2021
import sensor, image, time
from pid import PID
from servo1 import Servos
from machine import I2C, Pin
#from pyb import Servo
i2c = I2C(sda=Pin('P5'), scl=Pin('P4'))
servo = Servos(i2c, address=0x40, freq=50, min_us=500, max_us=2500, degrees=180)
red_threshold = (13, 49, 18, 61, 6, 47)
#pan_pid = PID(p=0.07, i=0, imax=90) #脱机运行或者禁用图像传输,使用这个PID
#tilt_pid = PID(p=0.05, i=0, imax=90) #脱机运行或者禁用图像传输,使用这个PID
pan_pid = PID(p=0.1, i=0, imax=90)#在线调试使用这个PID
tilt_pid = PID(p=0.1, i=0, imax=90)#在线调试使用这个PID
pan_current = 0
tilt_current = 0
sensor.reset() # Initialize the camera sensor.
sensor.set_pixformat(sensor.RGB565) # use RGB565.
sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed.
sensor.skip_frames(10) # Let new settings take affect.
sensor.set_auto_whitebal(False) # turn this off.
clock = time.clock() # Tracks FPS.
def find_max(blobs):
max_size=0
for blob in blobs:
if blob[2]*blob[3] > max_size:
max_blob=blob
max_size = blob[2]*blob[3]
return max_blob
while True:
clock.tick() # Track elapsed milliseconds between snapshots().
img = sensor.snapshot() # Take a picture and return the image.
blobs = img.find_blobs([red_threshold])
if blobs:
max_blob = find_max(blobs)
pan_error = max_blob.cx()-img.width()/2
tilt_error = max_blob.cy()-img.height()/2
print("pan_error: ", pan_error)
img.draw_rectangle(max_blob.rect()) # rect
img.draw_cross(max_blob.cx(), max_blob.cy()) # cx, cy
pan_output=pan_pid.get_pid(pan_error,1)/2
tilt_output=tilt_pid.get_pid(tilt_error,1)
print("pan_output",pan_output)
print("tilt_output",pan_output)
servo.position(0, pan_current + pan_output)
pan_current = servo.get_angle()
servo.position(1, tilt_current - tilt_output)
tilt_current = servo.get_angle()
pca9685.py
# pca9685 - By: DN36 - 周四 1月 28 2021
import utime
import ustruct
class PCA9685:
def __init__(self, i2c, address=0x40):
self.i2c = i2c
self.address = address
self.reset()
def _write(self, address, value):
self.i2c.writeto_mem(self.address, address, bytearray([value]))
def _read(self, address):
return self.i2c.readfrom_mem(self.address, address, 1)[0]
def reset(self):
self._write(0x00, 0x00) # Mode1
def freq(self, freq=None):
if freq is None:
return int(25000000.0 / 4096 / (self._read(0xfe) - 0.5))
prescale = int(25000000.0 / 4096.0 / freq + 0.5)
old_mode = self._read(0x00) # Mode 1
self._write(0x00, (old_mode & 0x7F) | 0x10) # Mode 1, sleep
self._write(0xfe, prescale) # Prescale
self._write(0x00, old_mode) # Mode 1
utime.sleep_us(5)
self._write(0x00, old_mode | 0xa1) # Mode 1, autoincrement on
def pwm(self, index, on=None, off=None):
if on is None or off is None:
data = self.i2c.readfrom_mem(self.address, 0x06 + 4 * index, 4)
return ustruct.unpack('<HH', data)
data = ustruct.pack('<HH', on, off)
self.i2c.writeto_mem(self.address, 0x06 + 4 * index, data)
def duty(self, index, value=None, invert=False):
if value is None:
pwm = self.pwm(index)
if pwm == (0, 4096):
value = 0
elif pwm == (4096, 0):
value = 4095
value = pwm[1]
if invert:
value = 4095 - value
return value
if not 0 <= value <= 4095:
raise ValueError("Out of range")
if invert:
value = 4095 - value
if value == 0:
self.pwm(index, 0, 4096)
elif value == 4095:
self.pwm(index, 4096, 0)
else:
self.pwm(index, 0, value)
servo1.py
# servo_1 - By: DN36 - 周四 1月 28 2021
import pca9685
import math
class Servos:
def __init__(self, i2c, address=0x40, freq=50, min_us=500, max_us=2500, degrees=180):
self.period = 1000000 / freq
self.min_duty = self._us2duty(min_us)
self.max_duty = self._us2duty(max_us)
self.degrees = degrees
self.freq = freq
self.pca9685 = pca9685.PCA9685(i2c, address)
self.pca9685.freq(freq)
self._angle = None
def _us2duty(self, value):
return int(4095 * value / self.period)
def position(self, index, degrees=None, radians=None, us=None, duty=None):
span = self.max_duty - self.min_duty
if degrees is not None:
self._angle = degrees
duty = self.min_duty + span * degrees / self.degrees
elif radians is not None:
duty = self.min_duty + span * radians / math.radians(self.degrees)
elif us is not None:
duty = self._us2duty(us)
elif duty is not None:
pass
else:
return self.pca9685.duty(index)
duty = min(self.max_duty, max(self.min_duty, int(duty)))
self.pca9685.duty(index, duty)
def get_angle(self):
return self._angle #获取当前的舵机角度
def release(self, index):
self.pca9685.duty(index, 0)
pid.py
# pid - By: DN36 - 周四 1月 28 2021
from pyb import millis
from math import pi, isnan
class PID:
_kp = _ki = _kd = _integrator = _imax = 0
_last_error = _last_derivative = _last_t = 0
_RC = 1/(2 * pi * 20)
def __init__(self, p=0, i=0, d=0, imax=0):
self._kp = float(p)
self._ki = float(i)
self._kd = float(d)
self._imax = abs(imax)
self._last_derivative = float('nan')
def get_pid(self, error, scaler):
tnow = millis()
dt = tnow - self._last_t
output = 0
if self._last_t == 0 or dt > 1000:
dt = 0
self.reset_I()
self._last_t = tnow
delta_time = float(dt) / float(1000)
output += error * self._kp
if abs(self._kd) > 0 and dt > 0:
if isnan(self._last_derivative):
derivative = 0
self._last_derivative = 0
else:
derivative = (error - self._last_error) / delta_time
derivative = self._last_derivative + \
((delta_time / (self._RC + delta_time)) * \
(derivative - self._last_derivative))
self._last_error = error
self._last_derivative = derivative
output += self._kd * derivative
output *= scaler
if abs(self._ki) > 0 and dt > 0:
self._integrator += (error * self._ki) * scaler * delta_time
if self._integrator < -self._imax: self._integrator = -self._imax
elif self._integrator > self._imax: self._integrator = self._imax
output += self._integrator
return output
def reset_I(self):
self._integrator = 0
self._last_derivative = float('nan')
大家用的时候将pca9685.py和servo1.py和pid.py放入openmv cam driver文件夹内,在openmv ide中运行main.py就可以了。
视频链接
追色块的云台(PCA9685+PID算法)