立創(chuàng)·梁山派開發(fā)板-21年電賽F題-送藥小車-K210功能實現(xiàn)代碼講解

送藥小車代碼倉庫：https://gitee.com/lcsc/medical_car

更好的觀看體驗請去：https://lceda001.feishu.cn/wiki/ZDYbwqDfCiwVlckUEcScF0KSnRh

送藥小車立創(chuàng)開源平臺資料：https://oshwhub.com/li-chuang-kai-fa-ban/21-dian-sai-f-ti-zhi-neng-song-yao-xiao-che

K210功能實現(xiàn)代碼講解

具體實現(xiàn)代碼如下(在2_Code->application->sensor->k210->pyconde->main.py)：

# Hello World Example
#
# Welcome to the CanMV IDE! Click on the green run arrow button below to run the script!

import sensor, image, time, lcd, struct, ustruct
import gc

from machine import UART,Timer,PWM
from board import board_info
from fpioa_manager import fm
import KPU as kpu
from Maix import GPIO,utils
import gc
import machine

lcd.init()                          # Init lcd display
lcd.clear(lcd.RED)                  # Clear lcd screen.

sensor.reset(freq=22000000, dual_buff=1)                # 設(shè)置攝像頭頻率 24M 開啟雙緩沖模式 會提高幀率 但內(nèi)存占用增加
sensor.set_auto_exposure(0)         # 設(shè)置自動曝光
sensor.set_auto_gain(False) # 顏色跟蹤必須關(guān)閉自動增益
sensor.set_auto_whitebal(False) # 顏色跟蹤必須關(guān)閉白平衡
sensor.set_pixformat(sensor.RGB565) # Set pixel format to RGB565 (or GRAYSCALE)
sensor.set_framesize(sensor.QVGA)   # Set frame size to QVGA (320x240)

#kpu = KPU()
#kpu.load_kmodel("/sd/KPU/mnist/uint8_mnist_cnn_model.kmodel")
sensor.skip_frames(time = 2000)     # Wait for settings take effect.
clock = time.clock()                # Create a clock object to track the FPS.
sensor.set_auto_exposure(0)         # 設(shè)置自動曝光

sensor.set_vflip(1)
#______________________________________________________________________________________________________________________________
#打印內(nèi)存分配情況
print(utils.gc_heap_size())
print("stack mem:"+str(gc.mem_free() / 1024)) # stack mem
print("heap mem:"+str(utils.heap_free() / 1024)) # heap mem

#第一次用執(zhí)行一次下面這兩個語句
#utils.gc_heap_size(800*1024)
#machine.reset()

#______________________________________________________________________________________________________________________________
#程序運行選擇
is_need_debug = 0
is_patrol_line = 0
is_upacker_debug = 0
is_stack_heap_mem_debug = 0
is_upacker_recive_debug = 1
is_findflob_debug = 0

#______________________________________________________________________________________________________________________________
#程序運行狀態(tài)
work_mode = 0  #0是巡線模式，1是數(shù)字識別模式

#______________________________________________________________________________________________________________________________
#串口配置區(qū)

fm.register(6, fm.fpioa.UART1_TX, force=True)
fm.register(7, fm.fpioa.UART1_RX, force=True)

k210_uart = UART(UART.UART1, 115200, 8, 0, 0, timeout=1000, read_buf_len=4096)

uart_test_write_str = '1260808878'

#______________________________________________________________________________________________________________________________
#按鍵蜂鳴器配置區(qū)
#注冊IO，注意高速GPIO口才有中斷
fm.register(35, fm.fpioa.GPIO0)
fm.register(16, fm.fpioa.GPIOHS0)
#構(gòu)建案件對象
KEY=GPIO(GPIO.GPIOHS0, GPIO.IN, GPIO.PULL_UP)

#PWM通過定時器配置，接到IO15引腳
tim = Timer(Timer.TIMER1, Timer.CHANNEL0, mode=Timer.MODE_PWM)
beep = PWM(tim, freq=1000, duty=0, pin=9)

#按鍵標(biāo)志位
key_node = 0
key_press_long = 0

#中斷回調(diào)函數(shù)
def fun(KEY):
    global work_mode,key_node,key_press_long
    temp_count = 0

    time.sleep_ms(10) #消除抖動
    while KEY.value()== 0:
        key_node = 1
        time.sleep_ms(10) #長按延時
        #長按檢測計數(shù)
        temp_count=temp_count+1
        print(temp_count)
    if temp_count >= 50:

        beep.duty(50)
        time.sleep_ms(500)
        beep.duty(0)
        time.sleep_ms(100)

        print(temp_count)
        key_node = 0
        key_press_long = 1

#開啟中斷，下降沿觸發(fā)
KEY.irq(fun, GPIO.IRQ_FALLING)

#______________________________________________________________________________________________________________________________
#要傳給梁山派的數(shù)據(jù)

#work_mode：0是巡線模式，1是數(shù)字識別模式
#recognition：0是未知（巡線狀態(tài)下），1是藥房門口區(qū)域（停車線）,十字路口和T字路口由

class uart_send_data_t:
   def __init__( self, work_mode=0, recognition=0,top_block_offset=0,center_block_offset=0,left_block_offset=0,right_block_offset=0,left_number = 0,right_number = 0):
      self.work_mode = work_mode
      self.recognition = recognition
      self.top_block_offset = top_block_offset
      self.center_block_offset = center_block_offset
      self.left_block_offset = left_block_offset
      self.right_block_offset = right_block_offset
      self.left_number = left_number
      self.right_number = right_number


global_uart_send_data = uart_send_data_t()

#______________________________________________________________________________________________________________________________
#感興趣區(qū)配置
roi_area_width = 30
roi_area_color = (0, 0, 200)
roi_area_thickness = 2


left_roi = [50,40,roi_area_width,sensor.height()-40]
right_roi = [sensor.width()-roi_area_width-50,40,roi_area_width,sensor.height()-40]
top_roi = [0,10,sensor.width(),roi_area_width]
center_roi = [0,int(sensor.height()/2)-int(roi_area_width/2),sensor.width(),roi_area_width]
#bottom_roi = [0,sensor.height()- roi_area_width,sensor.width(),roi_area_width]

black_block_roi = [0,100,sensor.width(),100]

def draw_roi(img):
    img.draw_rectangle(left_roi, color = roi_area_color, thickness = roi_area_thickness, fill = False)
    img.draw_rectangle(right_roi, color = roi_area_color, thickness = roi_area_thickness, fill = False)
    img.draw_rectangle(top_roi, color = roi_area_color, thickness = roi_area_thickness, fill = False)
    img.draw_rectangle(center_roi, color = roi_area_color, thickness = roi_area_thickness, fill = False)
    img.draw_rectangle(black_block_roi, color = (0, 255, 0), thickness = roi_area_thickness, fill = False)
    return

#______________________________________________________________________________________________________________________________
#尋找色塊區(qū)配置
red_threshold =[12, 68, 11, 63, 5, 81]
black_threshold =[0, 50, -24,-1, -18, 6]

class red_blob_location_t:
   def __init__( self, x=0, y=0,area = 0):
      self.x = x
      self.y = y
      self.area = area


#______________
#左部blob位置信息
left_blob_location = red_blob_location_t()
left_pixels_threshold = 20     #若一個色塊的像素數(shù)小于 pixel_threshold ，則會被過濾掉。
left_area_threshold = 20       #若一個色塊的邊界框區(qū)域小于 area_threshold ，則會被過濾掉。
left_x_stride = 20               #是查找某色塊時需要跳過的x像素的數(shù)量。找到色塊后，直線填充算法將精確像素。 若已知色塊較大，可增加 x_stride 來提高查找色塊的速度。
left_y_stride = 20               #是查找某色塊時需要跳過的y像素的數(shù)量。找到色塊后，直線填充算法將精確像素。 若已知色塊較大，可增加 y_stride 來提高查找色塊的速度。

#______________
#右部blob位置信息
right_blob_location = red_blob_location_t()
right_pixels_threshold = 20     #若一個色塊的像素數(shù)小于 pixel_threshold ，則會被過濾掉。
right_area_threshold = 20       #若一個色塊的邊界框區(qū)域小于 area_threshold ，則會被過濾掉。
right_x_stride = 20               #是查找某色塊時需要跳過的x像素的數(shù)量。找到色塊后，直線填充算法將精確像素。 若已知色塊較大，可增加 x_stride 來提高查找色塊的速度。
right_y_stride = 20               #是查找某色塊時需要跳過的y像素的數(shù)量。找到色塊后，直線填充算法將精確像素。 若已知色塊較大，可增加 y_stride 來提高查找色塊的速度。

#______________
#頂部blob位置信息
top_blob_location = red_blob_location_t()
top_pixels_threshold = 20     #若一個色塊的像素數(shù)小于 pixel_threshold ，則會被過濾掉。
top_area_threshold = 20       #若一個色塊的邊界框區(qū)域小于 area_threshold ，則會被過濾掉。
top_x_stride = 15               #是查找某色塊時需要跳過的x像素的數(shù)量。找到色塊后，直線填充算法將精確像素。 若已知色塊較大，可增加 x_stride 來提高查找色塊的速度。
top_y_stride = 20               #是查找某色塊時需要跳過的y像素的數(shù)量。找到色塊后，直線填充算法將精確像素。 若已知色塊較大，可增加 y_stride 來提高查找色塊的速度。

#______________
#中部blob位置信息
center_blob_location = red_blob_location_t()
center_pixels_threshold = 20     #若一個色塊的像素數(shù)小于 pixel_threshold ，則會被過濾掉。
center_area_threshold = 20       #若一個色塊的邊界框區(qū)域小于 area_threshold ，則會被過濾掉。
center_x_stride = 20               #是查找某色塊時需要跳過的x像素的數(shù)量。找到色塊后，直線填充算法將精確像素。 若已知色塊較大，可增加 x_stride 來提高查找色塊的速度。
center_y_stride = 20               #是查找某色塊時需要跳過的y像素的數(shù)量。找到色塊后，直線填充算法將精確像素。 若已知色塊較大，可增加 y_stride 來提高查找色塊的速度。


blob_location=[0,0]

def find_blob(img):

    global global_uart_send_data
    #top_blob_location.x = -1
    #top_blob_location.y = -1
    #center_blob_location.x = -1
    #center_blob_location.y = -1
    left_blob_location.x = -1
    left_blob_location.y = -1
    right_blob_location.x = -1
    right_blob_location.y = -1
    center_blob_location.area = 0
    for blob in img.find_blobs([red_threshold], roi = left_roi, x_stride = left_x_stride, y_stride = left_y_stride, pixels_threshold=left_pixels_threshold, area_threshold=left_area_threshold, merge=False, margin=10):
        img.draw_rectangle(blob.rect())
        img.draw_cross(blob.cx(), blob.cy())
        left_blob_location.x = blob.cx()
        left_blob_location.y = blob.cy()
        left_blob_location.area = blob.area()

    for blob in img.find_blobs([red_threshold], roi = right_roi,x_stride = right_x_stride, y_stride = right_y_stride, pixels_threshold=right_pixels_threshold, area_threshold=right_area_threshold, merge=True, margin=10):
        img.draw_rectangle(blob.rect())
        img.draw_cross(blob.cx(), blob.cy())
        right_blob_location.x = blob.cx()
        right_blob_location.y = blob.cy()
        right_blob_location.area = blob.area()

    for blob in img.find_blobs([red_threshold], roi = top_roi,x_stride = top_x_stride, y_stride = top_y_stride, pixels_threshold=top_pixels_threshold, area_threshold=top_area_threshold, merge=True, margin=10):
        img.draw_rectangle(blob.rect())
        img.draw_cross(blob.cx(), blob.cy())
        #print("-------"+str(blob.area()))
        top_blob_location.x = blob.cx()
        top_blob_location.y = blob.cy()
        top_blob_location.area = blob.area()

    for blob in img.find_blobs([red_threshold], roi = center_roi,x_stride = center_x_stride, y_stride = center_y_stride, pixels_threshold=center_pixels_threshold, area_threshold=center_area_threshold, merge=True, margin=10):
        img.draw_rectangle(blob.rect())
        img.draw_cross(blob.cx(), blob.cy())
        center_blob_location.x = blob.cx()
        center_blob_location.y = blob.cy()
        center_blob_location.area = blob.area()
        #print(blob.pixels())


    #for blob in img.find_blobs([black_threshold], roi = black_block_roi, pixels_threshold=30,merge=True,margin=50):
        #img.draw_rectangle(blob.rect())
        #img.draw_cross(blob.cx(), blob.cy())
        ##print(blob.count())
        #if(blob.count() >= 5):
            #print("tinche"+str(blob.count()))
            #global_uart_send_data.recognition = 1
        #else:
            #global_uart_send_data.recognition = 0

    if(center_blob_location.area == 0):
        print("tinche")
        global_uart_send_data.recognition = 1
    else:
        global_uart_send_data.recognition = 0

    return

#______________________________________________________________________________________________________________________________
#色塊連線區(qū)配置
lines_color = (0, 200, 0)
lines_thickness = 1

def draw_lines(img):
    img.draw_line(top_blob_location.x, top_blob_location.y, center_blob_location.x, center_blob_location.y, color = lines_color, thickness = lines_thickness)
    img.draw_line(left_blob_location.x, left_blob_location.y, right_blob_location.x, right_blob_location.y, color = lines_color, thickness = lines_thickness)
    return


#______________________________________________________________________________________________________________________________
#upacker python實現(xiàn)代碼 ：https://github.com/aeo123/upacker/blob/master/python/upacker.py
#介紹請看這里：https://github.com/aeo123/upacker

class Upacker():
    def __init__(self):
        self._STX_L = 0x55
        self._MAX_PACK_SIZE = 1024 + 4 + 128
        self._calc = 0
        self._check = 0
        self._cnt = 0
        self._flen = 0
        self._state = 0
        self._data = bytearray()

    def _decode(self, d):
        if (self._state == 0 and d == self._STX_L):
            self._state = 1
            self._calc = self._STX_L
        elif self._state == 1:
            self._flen = d & 0xff
            self._calc ^= d & 0xff
            self._state = 2
        elif self._state == 2:
            self._flen |= (d & 0xff)  self._MAX_PACK_SIZE):
                self._state = 0
                return -1
            else:
                self._data = bytearray(self._flen & 0x3FFF)
            self._state = 3
            self._cnt = 0
        elif self._state == 3:
            header_crc = ((d & 0x03) > 12)
            self._check = d
            if (header_crc != (self._calc & 0X3C)):
                self._state = 0
                return -1
            self._state = 4
            self._flen &= 0x3FFF
        elif self._state == 4:
            self._data[self._cnt] = d
            self._cnt += 1
            self._calc ^= d
            if self._cnt == self._flen:
                self._state = 0
                #接收完，檢查check
                if ((self._calc & 0xFC) == (self._check & 0XFC)):
                    return 0
                else:
                    return -1
        else:
            self._state = 0

        return 1

    # 解包
    def unpack(self, bytes_data, callback):
        ret = 0
        for i in bytes_data:
            ret = self._decode(i)
            if ret == 0:
                callback(self._data)
                if(is_upacker_debug):
                    print(self._data)
            elif ret == -1:
                # callback(None)
                print("err")

    # 打包
    def enpack(self, data):
        tmp = bytearray(4)
        tmp[0] = 0x55
        tmp[1] = len(data) & 0xff
        tmp[2] = (len(data) >> 8) & 0xff
        crc = tmp[0] ^ tmp[1] ^ tmp[2]
        tmp[2] |= (crc & 0x0c) > 4)

        for i in range(len(data)):
            crc ^= data[i]
        tmp[3] |= (crc & 0xfc)

        frame = struct.pack("BBBB%ds" % len(data), tmp[0], tmp[1], tmp[2],
                            tmp[3], data)
        #python3.5之前bytes數(shù)據(jù)沒有hex()屬性，所以修改了下面這個
        #print(frame.hex())
        if(is_upacker_debug):
            print(''.join(map(lambda x:('' if len(hex(x))>=4 else '/x0')+hex(x)[2:],frame)))

        return frame

#____________
#串口命令切換模式
uart_cmd_need_change_mode = 0

def print_hex(bytes):
    global work_mode,uart_cmd_need_change_mode

    hex_byte = [hex(i) for i in bytes]
    if is_upacker_recive_debug:
        print("-----"+" ".join(hex_byte))
    if bytes[0] == 0x00:
        work_mode = 0
    if bytes[0] == 0x01:
        work_mode = 1
    uart_cmd_need_change_mode = 1

#if __name__ == '__main__':
    #buf = bytearray([0x00, 0x01, 0x02,0x03,0x77])
    #pack = Upacker()
    #pkt = pack.enpack(buf)
    #pack.unpack(pkt, print_hex)

#______________________________________________________________________________________________________________________________
#upacker python實現(xiàn)代碼結(jié)束



def upacker_init():
    pack = Upacker()

    return pack

yzh = 10

#______________________________________________________________________________________________________________________________
#發(fā)送數(shù)據(jù)到MCU，gd32是小端字節(jié)序
#pack各字母對應(yīng)類型
#x   pad byte        no value            1
#c   char            string of length 1  1
#b   signed char     integer             1
#B   unsigned char   integer             1
#?   _Bool           bool                1
#h   short           integer             2
#H   unsigned short  integer             2
#i   int             integer             4
#I   unsigned int    integer or long     4
#l   long            integer             4
#L   unsigned long   long                4
#q   long long       long                8
#Q   unsilong long   long                8
#f   float           float               4
#d   double          float               8
#s   char[]          string              1
#p   char[]          string              1
#P   void *          long

def send_data_to_mcu(pack,global_uart_send_data):
    hex_data = ustruct.pack("