371 lines
12 KiB
Python
371 lines
12 KiB
Python
|
'''
|
|||
|
|
K210 视频循迹示例
|
|||
|
|
'''
|
|||
|
|
import math
|
|||
|
|
import sensor, image, time, lcd
|
|||
|
|
import binascii
|
|||
|
|
from Maix import GPIO
|
|||
|
|
from machine import Timer, PWM, UART, Timer
|
|||
|
|
from fpioa_manager import fm
|
|||
|
|
|
|||
|
|
|
|||
|
|
# Debug模式开关，打开后方便显示调试信息
|
|||
|
|
#is_debug = True
|
|||
|
|
is_debug = False
|
|||
|
|
|
|||
|
|
QR_num = 0
|
|||
|
|
QR_Flag = False
|
|||
|
|
#--------------感光芯片配置 START -------------------
|
|||
|
|
|
|||
|
|
DISTORTION_FACTOR = 1 # 设定畸变系数
|
|||
|
|
IMG_WIDTH = 240
|
|||
|
|
IMG_HEIGHT = 320
|
|||
|
|
def init_sensor():
|
|||
|
|
'''
|
|||
|
|
初始化感光芯片
|
|||
|
|
'''
|
|||
|
|
lcd.init(freq=15000000) #初始化LCD
|
|||
|
|
sensor.reset() #复位和初始化摄像头
|
|||
|
|
sensor.set_vflip(1) #将摄像头设置成后置方式（所见即所得）
|
|||
|
|
sensor.set_pixformat(sensor.RGB565) # 设置像素格式为彩色 RGB565
|
|||
|
|
#sensor.set_pixformat(sensor.GRAYSCALE) # 设置像素格式为灰色
|
|||
|
|
sensor.set_framesize(sensor.QVGA) # 设置帧大小为 QVGA (320x240)
|
|||
|
|
#sensor.set_auto_gain(0,0)
|
|||
|
|
sensor.skip_frames(time = 2000) # 等待设置生效
|
|||
|
|
clock = time.clock() # 创建一个时钟来追踪 FPS（每秒拍摄帧数）
|
|||
|
|
|
|||
|
|
|
|||
|
|
init_sensor()
|
|||
|
|
#--------------感光芯片配置 END -------------------
|
|||
|
|
|
|||
|
|
|
|||
|
|
#--------------串口UART部分 START -------------------
|
|||
|
|
|
|||
|
|
#映射串口引脚
|
|||
|
|
fm.register(6, fm.fpioa.UART1_RX, force=True)
|
|||
|
|
fm.register(7, fm.fpioa.UART1_TX, force=True)
|
|||
|
|
|
|||
|
|
#初始化串口
|
|||
|
|
uart = UART(UART.UART1, 115200, read_buf_len=4096)
|
|||
|
|
|
|||
|
|
|
|||
|
|
def get_symbol(num):
|
|||
|
|
'''
|
|||
|
|
根据数值正负，返回数值对应的符号
|
|||
|
|
正数： ‘+’， 负数‘-’ 主要为了方便C语言解析待符号的数值。
|
|||
|
|
|
|||
|
|
'''
|
|||
|
|
if num >=0:
|
|||
|
|
return ord('+')
|
|||
|
|
else:
|
|||
|
|
return ord('-')
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
def data_format_wrapper(down_center, state_crossing, deflection_angle):
|
|||
|
|
'''
|
|||
|
|
根据通信协议封装循迹数据
|
|||
|
|
TODO 重新编写通信协议 与配套C解析代码
|
|||
|
|
|
|||
|
|
'''
|
|||
|
|
send_data = [
|
|||
|
|
0x55,
|
|||
|
|
0x02,
|
|||
|
|
0x91,
|
|||
|
|
down_center, # 底部色块中心是否在中点附近
|
|||
|
|
1 if state_crossing else 0,
|
|||
|
|
get_symbol(deflection_angle), # 偏航角符号
|
|||
|
|
abs(int(deflection_angle)), # 偏航角
|
|||
|
|
0xbb]
|
|||
|
|
global is_debug
|
|||
|
|
if is_debug:
|
|||
|
|
print(send_data)
|
|||
|
|
return bytes(send_data)
|
|||
|
|
|
|||
|
|
|
|||
|
|
def UsartSend(str_data):
|
|||
|
|
'''
|
|||
|
|
串口发送函数
|
|||
|
|
'''
|
|||
|
|
uart.write(str_data)
|
|||
|
|
|
|||
|
|
|
|||
|
|
#--------------串口UART部分 END -------------------
|
|||
|
|
|
|||
|
|
|
|||
|
|
#--------------定时器部分 START -------------------
|
|||
|
|
|
|||
|
|
is_need_send_data = False # 是否需要发送数据的信号标志
|
|||
|
|
def uart_time_trigger(tim):
|
|||
|
|
'''
|
|||
|
|
串口发送数据的定时器，定时器的回调函数
|
|||
|
|
'''
|
|||
|
|
global is_need_send_data, QR_Flag, QR_num
|
|||
|
|
if QR_Flag:
|
|||
|
|
QR_num += 1
|
|||
|
|
if QR_num >= 200: # 10秒计时
|
|||
|
|
QR_Flag = False
|
|||
|
|
QR_num = 0
|
|||
|
|
|
|||
|
|
|
|||
|
|
is_need_send_data = True
|
|||
|
|
|
|||
|
|
'''
|
|||
|
|
初始化定时器, 每秒执行20次
|
|||
|
|
period ： 周期1000/20=50
|
|||
|
|
callback： 回调函数
|
|||
|
|
'''
|
|||
|
|
tim1 = Timer(Timer.TIMER1, Timer.CHANNEL1, mode=Timer.MODE_PERIODIC, period=50, callback=uart_time_trigger)
|
|||
|
|
|
|||
|
|
|
|||
|
|
#--------------定时器部分 END -------------------
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
#--------------舵机配置 START -------------------
|
|||
|
|
|
|||
|
|
def Servo(angle):
|
|||
|
|
'''
|
|||
|
|
说明：舵机控制函数
|
|||
|
|
功能：180度舵机：angle:-90至90 表示相应的角度
|
|||
|
|
360连续旋转度舵机：angle:-90至90 旋转方向和速度值。
|
|||
|
|
【duty】占空比值：0-100
|
|||
|
|
'''
|
|||
|
|
#PWM通过定时器配置，接到IO17引脚
|
|||
|
|
tim_pwm = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
|
|||
|
|
S1 = PWM(tim_pwm, freq=50, duty=0, pin=17)
|
|||
|
|
S1.duty((angle+90)/180*10+2.5)
|
|||
|
|
|
|||
|
|
|
|||
|
|
Servo(0) # 默认向下0度
|
|||
|
|
time.sleep(1)
|
|||
|
|
|
|||
|
|
#--------------舵机配置 END -------------------
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
#--------------直线检测部分 START -------------------
|
|||
|
|
|
|||
|
|
|
|||
|
|
# 直线灰度图颜色阈值
|
|||
|
|
LINE_COLOR_THRESHOLD = [(6, 65)]
|
|||
|
|
#LIGHT_LINE_COLOR_THRESHOLD = [(9, 88)]
|
|||
|
|
#LINE_COLOR_THRESHOLD = [(0, 52, -19, 0, -1, 14)]
|
|||
|
|
|
|||
|
|
# 以实际空间坐标为基准，取样窗口
|
|||
|
|
'''
|
|||
|
|
********屏幕ROI区域*******
|
|||
|
|
* | ------上方----- |
|
|||
|
|
* 左------中上-----右
|
|||
|
|
* 侧------中下-----侧
|
|||
|
|
* | ------下方----- |
|
|||
|
|
********屏幕ROI区域*******
|
|||
|
|
'''
|
|||
|
|
ROIS = {
|
|||
|
|
#'left': (0, 0, 320, 50), # 纵向取样-左侧
|
|||
|
|
#'right': (0, 190, 320, 50), # 纵向取样-右侧
|
|||
|
|
'left': (0, 0, 180, 50), # 纵向取样-左侧
|
|||
|
|
'right': (0, 190, 180, 50), # 纵向取样-右侧
|
|||
|
|
'up': (240, 0, 80, 240), # 横向取样-上方
|
|||
|
|
'middle_up': (160, 0, 80, 240), # 横向取样-中上
|
|||
|
|
'middle_down': (80, 0, 80, 240), # 横向取样-中下
|
|||
|
|
'down': (0, 0, 80, 240), # 横向取样-下方
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
def find_blobs_in_rois(img):
|
|||
|
|
'''
|
|||
|
|
说明：在ROIS中寻找色块，获取ROI中色块的中心区域与是否有色块的信息
|
|||
|
|
'''
|
|||
|
|
global ROIS
|
|||
|
|
global is_debug
|
|||
|
|
canvas = img.copy()
|
|||
|
|
roi_blobs_result = {} # 在各个ROI中寻找色块的结果记录
|
|||
|
|
for roi_direct in ROIS.keys():
|
|||
|
|
roi_blobs_result[roi_direct] = {
|
|||
|
|
'cx':0,
|
|||
|
|
'cy':0,
|
|||
|
|
'w':0,
|
|||
|
|
'blob_flag': False
|
|||
|
|
}
|
|||
|
|
for roi_direct, roi in ROIS.items():
|
|||
|
|
blobs=canvas.find_blobs(LINE_COLOR_THRESHOLD, roi=roi, merge=True)
|
|||
|
|
if len(blobs) != 0:
|
|||
|
|
largest_blob = max(blobs, key=lambda b: b.pixels())
|
|||
|
|
if largest_blob.area() > 1000:
|
|||
|
|
x,y,width,height = largest_blob[:4]
|
|||
|
|
roi_blobs_result[roi_direct]['cx'] = largest_blob.cy()
|
|||
|
|
roi_blobs_result[roi_direct]['cy'] = largest_blob.cx()
|
|||
|
|
roi_blobs_result[roi_direct]['w'] = largest_blob.h()
|
|||
|
|
roi_blobs_result[roi_direct]['blob_flag'] = True
|
|||
|
|
if is_debug:
|
|||
|
|
img.draw_rectangle((x,y,width, height), color=(255))
|
|||
|
|
else:
|
|||
|
|
#blobs=canvas.find_blobs(LINE_COLOR_THRESHOLD, roi=roi, merge=True, pixels_area=10)
|
|||
|
|
continue
|
|||
|
|
return roi_blobs_result
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
def state_deflection_angle(roi_blobs_result):
|
|||
|
|
'''
|
|||
|
|
说明：偏转状态值返回
|
|||
|
|
'''
|
|||
|
|
# ROI区域权重值
|
|||
|
|
#ROIS_WEIGHT = [1, 1, 1, 1]
|
|||
|
|
ROIS_WEIGHT = [1, 0, 0, 1]
|
|||
|
|
state_crossing = False
|
|||
|
|
deflection_angle = 0
|
|||
|
|
down_center = 0
|
|||
|
|
center_num = 0
|
|||
|
|
|
|||
|
|
# 偏转值计算，ROI中心区域X值
|
|||
|
|
centroid_sum = roi_blobs_result['up']['cx']*ROIS_WEIGHT[0] + roi_blobs_result['middle_up']['cx']*ROIS_WEIGHT[1] \
|
|||
|
|
+ roi_blobs_result['middle_down']['cx']*ROIS_WEIGHT[2] + roi_blobs_result['down']['cx']*ROIS_WEIGHT[3]
|
|||
|
|
if roi_blobs_result['up']['blob_flag']:
|
|||
|
|
center_num += ROIS_WEIGHT[0]
|
|||
|
|
if roi_blobs_result['middle_up']['blob_flag']:
|
|||
|
|
center_num += ROIS_WEIGHT[1]
|
|||
|
|
if roi_blobs_result['middle_down']['blob_flag']:
|
|||
|
|
center_num += ROIS_WEIGHT[2]
|
|||
|
|
if roi_blobs_result['down']['blob_flag']:
|
|||
|
|
center_num += ROIS_WEIGHT[3]
|
|||
|
|
center_pos = centroid_sum / (ROIS_WEIGHT[0]+ROIS_WEIGHT[1]+ROIS_WEIGHT[2]+ROIS_WEIGHT[3])
|
|||
|
|
deflection_angle = (IMG_WIDTH/2)- center_pos
|
|||
|
|
|
|||
|
|
# 判断两侧ROI区域检测到黑色线
|
|||
|
|
if roi_blobs_result['left']['blob_flag'] and roi_blobs_result['right']['blob_flag']:
|
|||
|
|
# 判断两侧ROI区域检测到黑色线处于图像下方1/3处
|
|||
|
|
if roi_blobs_result['left']['cy'] <= ((IMG_HEIGHT/3)) or roi_blobs_result['right']['cy'] <= ((IMG_HEIGHT/3)):
|
|||
|
|
# 当最下方ROI区域的黑线宽度大于140像素（检测到路口）
|
|||
|
|
if roi_blobs_result['down']['w'] > 140:
|
|||
|
|
state_crossing = True
|
|||
|
|
|
|||
|
|
|
|||
|
|
return down_center, state_crossing, deflection_angle
|
|||
|
|
|
|||
|
|
|
|||
|
|
#--------------直线与路口检测部分 END -------------------
|
|||
|
|
|
|||
|
|
|
|||
|
|
#--------------二维码识别部分 START -------------------
|
|||
|
|
|
|||
|
|
def QR_Check():
|
|||
|
|
'''
|
|||
|
|
说明：二维码函数
|
|||
|
|
'''
|
|||
|
|
global QR_Flag
|
|||
|
|
if QR_Flag:
|
|||
|
|
res = img.find_qrcodes() # 寻找二维码
|
|||
|
|
if len(res) > 0:
|
|||
|
|
# 在图片和终端显示二维码信息
|
|||
|
|
img.draw_rectangle(res[0].rect())
|
|||
|
|
img.draw_string(2,2, res[0].payload(), color=(0,128,0), scale=2)
|
|||
|
|
print(res[0].payload())
|
|||
|
|
# 串口发送二维码信息
|
|||
|
|
uart.write(bytes([0x55]))
|
|||
|
|
uart.write(bytes([0x92]))
|
|||
|
|
uart.write(bytes([0x01]))
|
|||
|
|
uart.write(bytes([len(res[0].payload())]))
|
|||
|
|
for qrdata in res[0].payload():
|
|||
|
|
uart.write(bytes([ord(qrdata)]))
|
|||
|
|
uart.write(bytes([0xbb]))
|
|||
|
|
|
|||
|
|
#--------------二维码识别部分 END -------------------
|
|||
|
|
|
|||
|
|
#--------------红绿灯识别部分 START -------------------
|
|||
|
|
|
|||
|
|
|
|||
|
|
#--------------红绿灯识别部分 END -------------------
|
|||
|
|
|
|||
|
|
#---------------------MAIN-----------------------
|
|||
|
|
|
|||
|
|
last_cx = 0
|
|||
|
|
last_cy = 0
|
|||
|
|
Flag_track = False
|
|||
|
|
#将蓝灯引脚IO12配置到GPIO0，K210引脚支持任意配置
|
|||
|
|
fm.register(12, fm.fpioa.GPIO0)
|
|||
|
|
LED_B = GPIO(GPIO.GPIO0, GPIO.OUT) #构建LED对象
|
|||
|
|
#按键KEY用于清屏
|
|||
|
|
fm.register(16, fm.fpioa.GPIO1, force=True)
|
|||
|
|
btn_debug = GPIO(GPIO.GPIO1, GPIO.IN)
|
|||
|
|
x_num = 0
|
|||
|
|
while True:
|
|||
|
|
LED_B.value(0) #点亮LED
|
|||
|
|
# 串口数据接收处理
|
|||
|
|
data = uart.read(8)
|
|||
|
|
if data is not None:
|
|||
|
|
print(data)
|
|||
|
|
print(len(data))
|
|||
|
|
print(binascii.hexlify(data).decode('utf_8'))
|
|||
|
|
if(len(data) >= 8):
|
|||
|
|
if((data[1] == 0x02)&(data[7] == 0xBB)):
|
|||
|
|
# 巡线与控制舵机
|
|||
|
|
if(data[2] == 0x91):
|
|||
|
|
if(data[3] == 0x01): # 启动识别
|
|||
|
|
print("巡线")
|
|||
|
|
print("开始识别")
|
|||
|
|
sensor.set_pixformat(sensor.GRAYSCALE) # 设置像素格式为灰色
|
|||
|
|
Flag_track = True
|
|||
|
|
tim1.start()
|
|||
|
|
if(data[3] == 0x02):
|
|||
|
|
print("巡线")
|
|||
|
|
print("停止识别")
|
|||
|
|
sensor.set_pixformat(sensor.RGB565) # 设置像素格式为彩色 RGB565
|
|||
|
|
Flag_track = False
|
|||
|
|
tim1.stop() # 停止定时器
|
|||
|
|
if(data[3] == 0x03): # 舵机调整
|
|||
|
|
angle = data[5]
|
|||
|
|
# 判断舵机控制方向
|
|||
|
|
if data[4] == ord('-'):
|
|||
|
|
# 限制舵机角度，防止过大损坏舵机
|
|||
|
|
if angle > 80:
|
|||
|
|
angle = 80
|
|||
|
|
angle = -angle
|
|||
|
|
elif data[4] == ord('+'):
|
|||
|
|
# 限制舵机角度，防止过大损坏舵机
|
|||
|
|
if angle > 35:
|
|||
|
|
angle = 35
|
|||
|
|
angle = angle
|
|||
|
|
Servo(angle) # 控制舵机
|
|||
|
|
time.sleep(1) # 等待舵机角度更新
|
|||
|
|
# 二维码识别
|
|||
|
|
if(data[2] == 0x92):
|
|||
|
|
print("识别二维码")
|
|||
|
|
# Servo(-15) # 向下-15度
|
|||
|
|
# time.sleep(1)
|
|||
|
|
if(data[3] == 0x01): #启动识别
|
|||
|
|
print("开始识别")
|
|||
|
|
tim1.start()
|
|||
|
|
QR_Flag = True
|
|||
|
|
if(data[3] == 0x02):
|
|||
|
|
print("停止识别")
|
|||
|
|
tim1.stop() # 停止定时器
|
|||
|
|
QR_Flag = False
|
|||
|
|
# 红绿灯识别
|
|||
|
|
if(data[3] == 0x03):
|
|||
|
|
print("识别红绿灯")
|
|||
|
|
print("开始识别")
|
|||
|
|
# 拍摄图片
|
|||
|
|
img = sensor.snapshot()
|
|||
|
|
# 去除图像畸变
|
|||
|
|
#img.lens_corr(DISTORTION_FACTOR)
|
|||
|
|
# 二维码识别
|
|||
|
|
QR_Check()
|
|||
|
|
# 巡线
|
|||
|
|
if Flag_track:
|
|||
|
|
roi_blobs_result = find_blobs_in_rois(img)
|
|||
|
|
down_center, state_crossing, deflection_angle = state_deflection_angle(roi_blobs_result)
|
|||
|
|
if is_need_send_data:
|
|||
|
|
UsartSend(data_format_wrapper(down_center, state_crossing, deflection_angle))
|
|||
|
|
#time.sleep_ms(10)
|
|||
|
|
#is_need_send_data = False
|
|||
|
|
# 在LCD上显示
|
|||
|
|
lcd.display(img)
|
|||
|
|
#按键KEY按下。开启或关闭调试，并退出所有任务。
|
|||
|
|
if btn_debug.value() == 0:
|
|||
|
|
Flag_track = not Flag_track # 巡线任务退出
|
|||
|
|
QR_Flag = not QR_Flag # 二维码任务退出
|
|||
|
|
is_debug = not is_debug # 调试标志位取
|