commit 1d565476f82e7c99cdd12066a440c05fea7ce0fd Author: UnknownObject Date: Sat May 27 18:08:33 2023 +0800 first commit diff --git a/track.py b/track.py new file mode 100644 index 0000000..31ec023 --- /dev/null +++ b/track.py @@ -0,0 +1,387 @@ +''' + 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 + +#--------------LCD 滚动字符显示 START----------------- + +LCD_Current_Y = 0 + +def LCDDrawText(str_data): + global LCD_Current_Y + lcd.draw_string(0, LCD_Current_Y, str_data, lcd.GREEN, lcd.BLACK) + LCD_Current_Y += 15 + if LCD_Current_Y >= 200: + LCD_Current_Y = 0 + + +def LCDDrawTrackInfo(down_center,state_crossing,deflection_angle,_image): + lcd.display(_image) + lcd.draw_string(0, 0, "down_center = " + str(down_center), lcd.GREEN) + lcd.draw_string(0, 15, "state_crossing = " + str(state_crossing), lcd.GREEN) + lcd.draw_string(0, 30, "deflection_angle = " + str(deflection_angle), lcd.GREEN) + +#--------------LCD 滚动字符显示 STOP------------------ + +#--------------感光芯片配置 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 = True # 是否需要发送数据的信号标志 +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 ------------------- + + + +#---------------------MAIN----------------------- +last_cx = 0 +last_cy = 0 +#Flag_track = False +#调试，使设备始终处于巡线模式 +Flag_track = 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): # 启动识别 + print("巡线") + print("开始识别") + Flag_track = True + tim1.start() + if(data[3] == 0x02): + 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 + + + # 拍摄图片 + 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) + LCDDrawTrackInfo(down_center, state_crossing, deflection_angle, img) + 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 # 调试标志位取