''' 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.set_auto_exposure(1) # 设置自动曝光 sensor.set_auto_gain(0, gain_db = 8) # 设置画面增益 17 dB 影响实时画面亮度 #sensor.set_auto_whitebal(0, rgb_gain_db = (0,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语言解析待符号的数值。 ''' print("num = ", num) 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), # 偏航角符号 print输出是43 + 45- +向左转调整 -向右转调整 abs(int(deflection_angle)), # 偏航角 0xbb] global is_debug if is_debug: print("send_data = ", 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 = [(0, 60)] #找黑色 LINE_COLOR_BAISE=[(60, 255)] #找白色 #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), # 横向取样-下方 } #找黑色 ROIS_BAISE = { 'wleft':(110, 0, 100, 20), 'wright':(110, 220, 100, 20), 'wconte':(110, 45, 100, 150), #白色中心区域 } #找白色 def find_blobs_in_rois(img): ''' 说明：在ROIS中寻找色块，获取ROI中色块的中心区域与是否有色块的信息 ''' global ROIS # global定义全局变量 字典数组 global is_debug # Debug模式开关，打开后方便显示调试信息 canvas = img.copy() # 复制给 canvas 实时传输的图片 img类型 canvas_baise = img.copy() # 判断白色的图片 roi_blobs_result = {} # 在各个ROI中寻找色块的结果记录 空的 定义的 roi_blobs_baise = {} # 定义白色的色块寻找色块的结果记录 空的 定义的 for roi_direct in ROIS.keys(): # 赋值初始化键值 黑线 roi_blobs_result[roi_direct] = { 'cx':0, 'cy':0, 'w':0, 'blob_flag': False # 标志位 } for roi_baise in ROIS_BAISE.keys(): # 赋值初始化键值 白色 roi_blobs_result[roi_baise] = { 'wcx':0, 'wcy':0, 'ww':0, 'wblob_baise': False # 标志位 } for roi_direct, roi in ROIS.items(): # 返回键值对 赋值给前两个参数 第一个是键 第二个是值 blobs=canvas.find_blobs(LINE_COLOR_THRESHOLD, roi=roi,area_threshold=400,merge=True) # 查找图像中所有区域黑色色块，并返回一个包括每个色块的色块对象的列表 # 判断返回列表不为空 if len(blobs) != 0: largest_blob = max(blobs, key=lambda b: b.pixels()) if largest_blob.area() > 1000: # 判断色块返回面积是否大于1000 x,y,width,height = largest_blob[:4] # 将列表0到3 #roi_blobs_result[roi_direct]['cx'] = largest_blob.cy()-40 #存疑为什么-40 #此处XY轴互换 roi_blobs_result[roi_direct]['cx'] = largest_blob.cy() #存疑为什么-40 #此处XY轴互换，返回色块的外框的中心Y坐标 roi_blobs_result[roi_direct]['cy'] = largest_blob.cx() #存疑为什么-40 #此处XY轴互换，返回色块的外框的中心X坐标 roi_blobs_result[roi_direct]['w'] = largest_blob.h() roi_blobs_result[roi_direct]['blob_flag'] = True if is_debug: print('LINE_COLOR_THRESHOLD*****') #print(largest_blob[:4] ) #print('largest_blob内容*****') #print(largest_blob) #koimg.draw_rectangle((x,y,width, height), color=(0)) #img.draw_rectangle((0, 0, 180, 50), color=(255)) else: #blobs=canvas.find_blobs(LINE_COLOR_THRESHOLD, roi=roi, merge=True, pixels_area=10) continue blobs_baise = canvas.find_blobs(LINE_COLOR_BAISE, roi=(0, 0, 60, 240), merge=True)#车载摄像头屏幕下部找白色#宽度200修改为240#用途寻卡，y示例中40修改为0 blobs_dixing = canvas.find_blobs(LINE_COLOR_BAISE, roi=(125, 0, 60, 240), merge=True)#车载摄像头屏幕中间找白色 #宽度200修改为240，y示例中40修改为0 blobs_zuo = canvas.find_blobs(LINE_COLOR_THRESHOLD, roi=(0, 0, 180, 50), merge=True)#车载摄像头屏幕左下部找黑色 blobs_you = canvas.find_blobs(LINE_COLOR_THRESHOLD, roi=(0, 190, 180, 50), merge=True)#车载摄像头屏幕右下部找黑色 #if len(blobs_baise) != 0:#进行了修改先让摄像头中部区域先碰到白色 if len(blobs_baise) != 0: print("*********进入循环第1步*******") largest_baise = max(blobs_baise , key = lambda b:b.pixels()) wx,wy,wwidth,wwheight = largest_baise[:4] #print("宽度",largest_baise[2]) #print("高度",largest_baise[3]) #largest_blobs_zuo = max(blobs_zuo , key = lambda b:b.pixels()) #Bx_L,By_L,Bwidth_L,Bwheight_L = largest_blobs_zuo[:4] #arc_L=Bwidth_L*Bwheight_L #largest_blobs_you = max(blobs_you , key = lambda b:b.pixels()) #Bx_R,By_R,Bwidth_R,Bwheight_R = largest_blobs_you[:4] #arc_R=Bwidth_R*Bwheight_R if is_debug: #print('blobs_baise*****') img.draw_rectangle((0, 0, 60, 240), color=(255)) img.draw_rectangle((125, 0, 60, 240), color=(255)) #state_crossing = True #线上为白色准备跨越 #img.draw_rectangle((wx,wy,wwidth, wwheight), color=(255)) #标注 arc = wwidth * wwheight if arc >= 11000: print("*********进入循环第2步*******") #UsartSend(data_format_wrapper(0, 1, 0)) #img.draw_rectangle((wx,wy,wwidth, wwheight), color=(255))#左侧区域 if len(blobs_dixing) != 0: print("*********进入循环第3步*******") largest_dixing = max(blobs_dixing , key = lambda b:b.pixels()) wx,wy,wwidth,wwheight = largest_dixing[:4] #state_crossing = True #img.draw_rectangle((wx,wy,wwidth, wwheight), color=(255)) arc = wwidth * wwheight if arc >= 11000: #UsartSend(data_format_wrapper(0, 1, 0)) #img.draw_rectangle((wx,wy,wwidth, wwheight), color=(255))#右侧区域 print('kapian') #首先中部区域识别到白色进入判断地形还是卡片，接着判断下部，如果为白色判断为卡片。 UsartSend(data_format_wrapper(0, 1, 0))#地形停止命令 if len(blobs_zuo) != 0 and len(blobs_you) != 0: print("ka十字路口") UsartSend(data_format_wrapper(1, 1, 0))#地形停止命令 else: print('dixing') print(roi_blobs_result) #返回的是黑色色块，各区域中心位置 UsartSend(data_format_wrapper(0, 1, 0))#地形停止命令 return roi_blobs_result #返回的是黑色色块，各区域中心位置 def state_deflection_angle(roi_blobs_result): ''' 说明：偏转状态值返回 ''' # ROI区域权重值 #ROIS_WEIGHT = [0, 1, 0, 1] ROIS_WEIGHT = [1, 0, 0, 1] # ROIS对白色权重的数组 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: down_center=1#自行修改处 判断识别到十字路口 print("输出了十字路口标识") # down_center 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: 0 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([0x02])) 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-------------------# ################################################################################################### # 返回值 # 1、红灯（蒙的答案选项） # 2、绿灯 # 3、黄灯 # # 需要完善部分： # 1、过滤掉范围过小和过大部分--》提高识别效率 # 2、通信协议 ################################################################################################### def tick(timer): global times,isCut #print(times) if(times == 4): isCut = False tim.deinit() #tim对象反初始化 times = 0 else: times+=1; def drawTrafficLight(isDraw): #识别红绿灯******************* red = (90, 100, -40, -8, -10, 10) #红色阈值(75, 90, 0, 43, 0, 28) (49,76,25,60,-9,36) green = (91, 96, -33, -6, -11, 1) #绿色阈值(55, 94 ,-69, -22, -16 ,32) yellow = (90, 100, -5, 2, -4, 20) #黄色阈值(92, 100, -16, 43, -6, 17) (83, 100, 6, -28, 13, 73) hld = 0x01 #蒙答案 try: #sensor.set_windowing((0,0,280,100)) #tim.callback(tick) #回调函数的形参是用户函数或者匿名函数都必须有一个位置参数,才可以正常工作（位置参数可能就是一个实例），而且调用时不用些括号 red_blobs = img.find_blobs([red], merge=True) #寻找红色色块 合并所有重叠的blod green_blobs = img.find_blobs([green], merge=True) yellow_blobs = img.find_blobs([yellow],merge=True) if (len(red_blobs) != 0): x1 = red_blobs[0][0] y1 = red_blobs[0][1] w1 = red_blobs[0][2] h1 = red_blobs[0][3] print("红灯") if isDraw is True: img.draw_rectangle((x1,y1,w1,h1),color=(255,0,0)) hld = 0x01 elif (len(green_blobs) != 0): x1 = green_blobs[0][0] y1 = green_blobs[0][1] w1 = green_blobs[0][2] h1 = green_blobs[0][3] print("绿灯") if isDraw is True: img.draw_rectangle((x1,y1,w1,h1),color=(0,255,0)) hld = 0x02 elif (len(yellow_blobs) != 0): x1 = yellow_blobs[0][0] y1 = yellow_blobs[0][1] w1 = yellow_blobs[0][2] h1 = yellow_blobs[0][3] print("黄灯") if isDraw is True: img.draw_rectangle((x1,y1,w1,h1),color=(255,255,255)) hld = 0x03 # 串口发送交通灯信息 uart.write(bytes([0x55])) uart.write(bytes([0x92])) uart.write(bytes([0x03])) uart.write(bytes([hld])) uart.write(bytes([0x03])) uart.write(bytes([0x03])) uart.write(bytes([0x03])) uart.write(bytes([0xbb])) except Exception as e: print("识别红绿灯异常",e) # 串口发送交通灯信息 uart.write(bytes([0x55])) uart.write(bytes([0x92])) uart.write(bytes([0x03])) uart.write(bytes([hld])) uart.write(bytes([0x03])) uart.write(bytes([0x03])) uart.write(bytes([0x03])) uart.write(bytes([0xbb])) #======================交通灯识别 END===========================# #---------------------MAIN----------------------- last_cx = 0 last_cy = 0 Flag_track = False JTD = True #将蓝灯引脚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): # 启动识别 sensor.set_auto_gain(0, gain_db = 17) # 设置画面增益 17 dB 影响实时画面亮度 sensor.set_pixformat(sensor.GRAYSCALE) # 设置像素格式为灰色 Servo(-75) print("巡线") print("开始识别") Flag_track = True tim1.start() if(data[3] == 0x02): sensor.set_auto_gain(0, gain_db = 8) # 设置画面增益 17 dB 影响实时画面亮度 sensor.set_pixformat(sensor.RGB565) # 设置像素格式为彩色 RGB565 Servo(0) print("巡线") print("停止识别") 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("开始识别") tim1.start() JTD = True if(data[3] == 0x04): print("停止识别") tim1.stop() JTD = False # 拍摄图片 img = sensor.snapshot() # 去除图像畸变 #img.lens_corr(DISTORTION_FACTOR) # 二维码识别 QR_Check() #if JTD: #drawTrafficLight(img) ## 巡线 #Flag_track=1 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: dsd = data_format_wrapper(down_center, state_crossing, deflection_angle) UsartSend(dsd) print("下发指令") print("dsd = ", dsd) #print(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 # 调试标志位取