一、实验目标
前面利用了颜色对象在摄像头画面的x，y不同位置通过云台进行跟踪，接下来我们可以在这个基础上修改下，x方向改为车身左右运动进行跟随，然后根据识别的图像外切圆大小进行前进的控制，通过二者叠加来整体控制小车电机运动。
二、实验源码
import sys
sys.path.append('/home/pi/project_demo/lib')
#导入麦克纳姆小车驱动库 Import Mecanum Car Driver Library
from McLumk_Wheel_Sports import *
sys.path.append('/home/pi/software/oled_yahboom/')
from yahboom_oled import *
# 创建oled对象 Create an oled object
oled = Yahboom_OLED(debug=False)
#bgr8转jpeg格式 bgr8 to jpeg format
import enum
import cv2
def bgr8_to_jpeg(value, quality=75):return bytes(cv2.imencode('.jpg', value)[1])
#显示摄像头组件 Display camera components
import traitlets
import ipywidgets.widgets as widgets
from IPython.display import display
import time
# 线程功能操作库 Thread function operation library
import threading
import inspect
import ctypes
import numpy as np
image_widget = widgets.Image(format='jpeg', width=640, height=480)
image = cv2.VideoCapture(0)
image.set(3, 320)
image.set(4, 240)
image.set(5, 30) #设置帧率 Setting the frame rate
# image.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc('M', 'J', 'P', 'G'))
# image.set(cv2.CAP_PROP_BRIGHTNESS, 62) #设置亮度 -64 - 64 0.0 Set Brightness -64 - 64 0.0
# image.set(cv2.CAP_PROP_CONTRAST, 63) #设置对比度 -64 - 64 2.0 Set Contrast -64 - 64 2.0
# image.set(cv2.CAP_PROP_EXPOSURE, 4800) #设置曝光值 1.0 - 5000 156.0 Set the exposure value 1.0 - 5000 156.0
ret, frame = image.read()
# from picamera2 import Picamera2, Preview
# import libcamera
# picam2 = Picamera2()
# camera_config = picam2.create_preview_configuration(main={"format":'RGB888',"size":(320,240)})
# camera_config["transform"] = libcamera.Transform(hflip=1, vflip=1)
# picam2.configure(camera_config)
# # picam2.stop_preview() #停止相机预览，防止占用资源，从而开启相机失败 Stop camera preview to prevent resources from being occupied, which may cause camera startup failure
# # picam2.start_preview(True) #开启相机预览 Open camera preview
# picam2.start()
# frame = picam2.capture_array()
image_widget.value = bgr8_to_jpeg(frame)
global g_mode
g_mode = 0
global color_x, color_y, color_radius
color_x = color_y = color_radius = 0
global color_lower
#color_lower = np.array([0, 43, 46])
global color_upper
#color_upper = np.array([10, 255, 255])
color_lower = np.array([0,70,72])
color_upper = np.array([7, 255, 255])
Redbutton = widgets.Button( value=False, description='red', disabled=False, button_style='', # 'success', 'info', 'warning', 'danger' or '' tooltip='Description', icon='uncheck' )
Greenbutton = widgets.Button( value=False, description='green', disabled=False, button_style='', # 'success', 'info', 'warning', 'danger' or '' tooltip='Description', icon='uncheck' )
Bluebutton = widgets.Button( value=False, description='blue', disabled=False, button_style='', # 'success', 'info', 'warning', 'danger' or '' tooltip='Description', icon='uncheck' )
Yellowbutton = widgets.Button( value=False, description='yellow', disabled=False, button_style='', # 'success', 'info', 'warning', 'danger' or '' tooltip='Description', icon='uncheck' )
Orangebutton = widgets.Button( value=False, description='orange', disabled=False, button_style='', # 'success', 'info', 'warning', 'danger' or '' tooltip='Description', icon='uncheck' )
Closebutton = widgets.Button( value=False, description='close', disabled=False, button_style='', # 'success', 'info', 'warning', 'danger' or '' tooltip='Description', icon='uncheck' )
output = widgets.Output()
def ALL_Uncheck():Redbutton.icon = 'uncheck'Greenbutton.icon = 'uncheck'Bluebutton.icon = 'uncheck'Yellowbutton.icon = 'uncheck'Orangebutton.icon = 'uncheck'def on_Redbutton_clicked(b):global color_lower, color_upper, g_mode global target_valuex, target_valueyALL_Uncheck()b.icon = 'check'#color_lower = np.array([0, 43, 46])#color_upper = np.array([10, 255, 255])color_lower = np.array([0,43,89])color_upper = np.array([7, 255, 255])g_mode = 1with output:bot.Ctrl_WQ2812_ALL(1,0)#红色 redoled.clear()oled.add_line("Color_Follow", 1)oled.add_line("color: red", 3)oled.refresh()bot.Ctrl_Servo(1,90)bot.Ctrl_Servo(2,10)print("RedButton clicked.")
def on_Greenbutton_clicked(b):global color_lower, color_upper, g_mode global target_valuex, target_valueyALL_Uncheck()b.icon = 'check'#color_lower = np.array([35, 43, 46])#color_upper = np.array([77, 255, 255])color_lower = np.array([54,104,64])color_upper = np.array([78, 255, 255])g_mode = 1with output:bot.Ctrl_WQ2812_ALL(1,1)#绿色 greenoled.clear()oled.add_line("Color_Follow", 1)oled.add_line("color: green", 3)oled.refresh()bot.Ctrl_Servo(1,90)bot.Ctrl_Servo(2,10)print("GreenButton clicked.")def on_Bluebutton_clicked(b):global color_lower, color_upper, g_mode global target_valuex, target_valueyALL_Uncheck()b.icon = 'check'#color_lower=np.array([100, 43, 46])#color_upper = np.array([124, 255, 255])color_lower = np.array([92,100,62])color_upper = np.array([121, 255, 255])g_mode = 1with output:bot.Ctrl_WQ2812_ALL(1,2)#蓝色oled.clear()oled.add_line("Color_Follow", 1)oled.add_line("color: blue", 3)oled.refresh()bot.Ctrl_Servo(1,90)bot.Ctrl_Servo(2,10)print("Bluebutton clicked.")
def on_Yellowbutton_clicked(b):global color_lower, color_upper, g_mode global target_valuex, target_valueyALL_Uncheck()b.icon = 'check'#color_lower = np.array([26, 43, 46])#color_upper = np.array([34, 255, 255])color_lower = np.array([26,100,91])color_upper = np.array([32, 255, 255])g_mode = 1with output:bot.Ctrl_WQ2812_ALL(1,3)#黄色 yellowoled.clear()oled.add_line("Color_Follow", 1)oled.add_line("color: yellow", 3)oled.refresh()bot.Ctrl_Servo(1,90)bot.Ctrl_Servo(2,10)print("Yellowbutton clicked.")def on_Orangebutton_clicked(b):global color_lower, color_upper, g_mode global target_valuex, target_valueyALL_Uncheck()b.icon = 'check'color_lower = np.array([11, 43, 46])color_upper = np.array([25, 255, 255])g_mode = 1with output:bot.Ctrl_WQ2812_brightness_ALL(255, 48, 0)oled.clear()oled.add_line("Color_Follow", 1)oled.add_line("color: orange", 3)oled.refresh()bot.Ctrl_Servo(1,90)bot.Ctrl_Servo(2,10)print("Orangebutton clicked.")def on_Closebutton_clicked(b): global g_mode ALL_Uncheck()g_mode = 0with output:bot.Ctrl_WQ2812_ALL(0,0)oled.clear()oled.add_line("Color_Follow", 1)oled.add_line("color: none", 3)oled.refresh()bot.Ctrl_Servo(1,90)bot.Ctrl_Servo(2,10)print("CloseButton clicked.")
Redbutton.on_click(on_Redbutton_clicked)
Greenbutton.on_click(on_Greenbutton_clicked)
Bluebutton.on_click(on_Bluebutton_clicked)
Yellowbutton.on_click(on_Yellowbutton_clicked)
Orangebutton.on_click(on_Orangebutton_clicked)
Closebutton.on_click(on_Closebutton_clicked)
import PID
direction_pid = PID.PositionalPID(0.8, 0, 0.2)
yservo_pid = PID.PositionalPID(0.8, 0.4, 0.01)
speed_pid = PID.PositionalPID(1.1, 0, 0.2)
# 定义 target_servox 和 target_servoy 在外部 Define target_servox and target_servoy externally
target_servox = 90
target_servoy = 25
def servo_reset():bot.Ctrl_Servo(1,90)bot.Ctrl_Servo(2,25)
servo_reset()
def Color_Recongnize():oled.init_oled_process() #初始化oled进程 Initialize oled processglobal color_lower, color_upper, g_mode, first_read, while_cntglobal color_x, target_servox,target_servoy,picture,speed_valuet_start = time.time()fps = 0ret, frame = image.read()#frame = picam2.capture_array()#frame = cv2.resize(frame, (300, 300))frame = cv2.GaussianBlur(frame,(5,5),0) first_read = 1while_cnt = 0speed=30time.sleep(1)while True:ret, frame = image.read() #frame = picam2.capture_array()#frame = cv2.resize(frame, (300, 300))#frame = cv2.GaussianBlur(frame,(3,3),0) hsv = cv2.cvtColor(frame,cv2.COLOR_BGR2HSV)mask = cv2.inRange(hsv,color_lower,color_upper) mask = cv2.erode(mask,None,iterations=2)mask = cv2.dilate(mask,None,iterations=2)mask = cv2.GaussianBlur(mask,(5,5),0)cnts = cv2.findContours(mask.copy(),cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)[-2]if g_mode == 1: # 按钮切换开关 Push button switchif len(cnts) > 0:cnt = max (cnts, key = cv2.contourArea)(color_x,color_y),color_radius = cv2.minEnclosingCircle(cnt)
if color_radius > 10:# 将检测到的颜色用原形线圈标记出来 Mark the detected color with a prototype circlecv2.circle(frame,(int(color_x),int(color_y)),int(color_radius),(255,0,255),2) direction_pid.SystemOutput = color_xdirection_pid.SetStepSignal(250)direction_pid.SetInertiaTime(0.01, 0.05)target_valuex = int(direction_pid.SystemOutput+65)#print("color_x %f", color_x) #print("target_valuex %d", target_valuex) # 将云台转动至PID调校位置 Turn the gimbal to the PID adjustment position
if math.fabs(180 - color_y) > 75:# 输入Y轴方向参数PID控制输入 Input Y-axis direction parameter PID control inputyservo_pid.SystemOutput = color_yyservo_pid.SetStepSignal(250)yservo_pid.SetInertiaTime(0.01, 0.1)target_valuey = int(1150+yservo_pid.SystemOutput)target_servoy = int((target_valuey-500)/10) #print("target_servoy %d", target_servoy) if target_servoy > 110:target_servoy = 110if target_servoy < 0:target_servoy = 0#print(target_servoy)
speed_pid.SystemOutput = int(color_radius)speed_pid.SetStepSignal(80)speed_pid.SetInertiaTime(0.01, 0.1) speed_value = int(speed_pid.SystemOutput)#print("color_radius %d", color_radius)
if speed_value > 255:speed_value = 255if speed_value < 0:speed_value = 0if(target_valuex>70):rotate_left(int(speed/6))# speedelif(target_valuex<-70):rotate_right(int(speed/6))elif(65<color_radius<90):#调试目标半径70~90 Debug target radius 70~90stop_robot()elif(color_radius>90):#调试目标半径90 Debug target radius 90if(abs(target_valuex)<30):move_backward(speed)elif(20<color_radius<75):if(abs(target_valuex)<30):move_forward(speed_value)else:stop_robot()
bot.Ctrl_Servo(2,target_servoy)else:stop_robot()else:stop_robot()else:stop_robot()
fps = fps + 1mfps = fps / (time.time() - t_start)cv2.putText(frame, "FPS " + str(int(mfps)), (40,40), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0,255,255), 3)#cv2.putText(frame, "1" , (160,120), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0,255,255), 3)
image_widget.value = bgr8_to_jpeg(frame)
1. 无死区控制，实时性高，舵机会一直工作,抖动频繁;
display(image_widget)
display(Redbutton, Greenbutton, Bluebutton, Yellowbutton, Orangebutton, Closebutton, output)
thread1 = threading.Thread(target=Color_Recongnize)
thread1.setdaemon=True
thread1.start()2. 带死区控制，跟随实时性差一些，舵机在死区范围内不运动，抖动较稳定；
stop_thread(thread1)
servo_reset()#使用完成对象记住释放掉对象，不然下一个程序使用这个对象模块会被占用，导致无法使用# Release resources
# picam2.stop()
# picam2.close()
image.release()
stop_robot() 跟随是依据识别颜色在图像中呈现的大小进行接近的，也就是物体越小需要靠得越近才会停止接近，物体越大可能离得很远就不接近。
三、实验现象
代码块运行后，我们可以通过按钮控制小车的跟随不同的颜色，选择后，小车会随着带颜色的物体前后左右运动。同时RGB灯条也会亮起识别的颜色，oled显示识别的颜色因摄像头受光线影响较大。若效果不好时，需要调节代码里识别的颜色的hsv值。
