如上述视频所示，这是一个板球系统，下面我介绍一下硬件构成及软件
1.有两个兼容乐高舵机，这个系统用的是270°舵机，具体舵机及采购如下：
2.使用树莓派3B作为控制器，树莓派摄像头视频采集，使用i2c控制PCA9685舵机控制板控制，具体使用方法参考上一条文章
3.机械结构构造参考B站程欢欢开源的PID原理介绍视频。
树莓派内Python程序如下
'''
import time # 引入time库
from adafruit_servokit import ServoKit # sudo pip3 install adafruit-circuitpython-servokit安装库
import numpy as np
import cv2
from camera import Camera
kit = ServoKit(channels=16) # 明确PCA9685的舵机控制数
画面中点 = (320 / 2, 240 / 2)
X轴舵机端口号 = 14
Y轴舵机端口号 = 15
X轴舵机中点 = 81
Y轴舵机中点 = 85
X轴舵机范围 = (-35, 35)
Y轴舵机范围 = (-35, 35)
def on_EVENT_LBUTTONDOWN(event, x, y, flags, param): # 鼠标点击事件：改变目标坐标
global 目标量
if event == cv2.EVENT_LBUTTONDOWN:
xy = "%d,%d" % (x, y)
目标量 = [x,y]
print(x,y)
def 控制平台(x, y):
global X轴舵机端口号, Y轴舵机端口号, X轴舵机范围, Y轴舵机范围, X轴舵机中点, Y轴舵机中点
if x < X轴舵机范围[0]:
x = X轴舵机范围[0]
if x > X轴舵机范围[1]:
x = X轴舵机范围[1]
if y < Y轴舵机范围[0]:
y = Y轴舵机范围[0]
if y > Y轴舵机范围[1]:
y = Y轴舵机范围[1]
kit.servo[X轴舵机端口号].angle = X轴舵机中点 + x # x轴，向右减小
kit.servo[Y轴舵机端口号].angle = Y轴舵机中点 + y # y轴，向上增加
# 控制平台(0,0) #执行一次控制平台，使平台归零
控制平台(0, 0)
#
# 比例P系数 = [0.16, 0.16] #[0.16, 0.16] 需要调参 #调好的参数：P=0.16 I=0.03 D=0.036
# 积分I系数 =[0.03,0.03]#[0.03,0.03]
# 微分D系数 =[0.036 ,0.036]#[[0.036 ,0.036]
比例P系数=[0.2,0.2] #需要调参 #调好的参数：P=0.2 I=0.23 D=0.064
积分I系数=[0.23,0.23]
微分D系数=[0.064,0.064]
积分I最大值 = [5, 5] # 调好的参数；5
比例P = [0, 0] # 系统自动使用的变量
积分I = [0, 0]
微分D = [0, 0]
偏差量 = [0, 0]
上一次偏差量 = [0, 0]
执行量 = [0, 0]
小球位置 = [0, 0]
目标量 = [320 / 2, 240 / 2]
计时 = time.time()
if __name__ == '__main__':
camera = Camera(0)
camera.open()
while True:
ret, img = camera.getframe()
if ret:
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
retval, dst = cv2.threshold(gray, 0, 255, cv2.THRESH_OTSU) # 大津法二值化
# retval, dst = cv2.threshold(gray, 170, 255, cv2.THRESH_BINARY) # 灰度阈值法
im, contours, hierarchy = cv2.findContours(dst, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) # 查找轮廓
x, y, w, h = 0, 0, 0, 0
# 选出最大面积的轮廓
areas = []
for c in range(len(contours)):
areas.append(cv2.contourArea(contours[c]))
max_id = areas.index(max(areas))
# max_rect = cv2.minAreaRect(contours[max_id])
x, y, w, h = cv2.boundingRect(contours[max_id])
print(w, h, x, y)
if 100 > w > 20 and 100 > h > 20: # 过滤掉长宽大于40小于20的结果
targetPos_x = int(x + w / 2)
targetPos_y = int(y + h / 2)
# print(w,' ',h)
cv2.circle(img, (targetPos_x, targetPos_y), 10, (0, 255, 0), 2)
cv2.circle(img, (int(目标量[0]),int(目标量[1])), 2, (0, 0, 255), 2)
小球位置 = [targetPos_x, targetPos_y]
else: # 没有找到小球
积分I = [0, 0] # 积分I清零
小球位置 = 目标量 # 告知系统小球达到目标，使系统停转
运行时间 = time.time() - 计时
计时 = time.time()
for n in range(0, 2):
偏差量[n] = 目标量[n] - 小球位置[n]
比例P[n] = 偏差量[n] * 比例P系数[n]
积分I[n] = 积分I[n] + 偏差量[n] * 积分I系数[n] * 运行时间
if 积分I[n] > 积分I最大值[n]:
积分I[n] = 积分I最大值[n]
elif 积分I[n] < -积分I最大值[n]:
积分I[n] = -积分I最大值[n]
微分D[n] = (偏差量[n] - 上一次偏差量[n]) * 微分D系数[n] / 运行时间
上一次偏差量[n] = 偏差量[n]
执行量[n] = 比例P[n] + 积分I[n] + 微分D[n]
# 4）控制平台-执行量输入执行器
控制平台(执行量[0], 执行量[1])
cv2.setMouseCallback("frame", on_EVENT_LBUTTONDOWN)
cv2.imshow('frame', img)
cv2.imshow('bin',dst)
cv2.waitKey(1)
# 按键q退出while循环
if cv2.waitKey(1) & 0xFF == ord('q'):
break
kit.servo[14].angle = None # disable channel
kit.servo[15].angle = None # disable channel
camera.close()
cv2.destroyAllWindows()'''
