23年电赛E题，k210智能识别与定位

#=============================================================
import sensor
import image
import lcd
import time
import machine
from machine import UART, Timer
from fpioa_manager import fm
import ustruct

import sensor, image, lcd, time
import KPU as kpu

import utime
from Maix import GPIO
from board import board_info

import math






# ************************* 一些变量的设定 *************************

min_blob_area = 1  # 最小色块面积阈值

x_offset = 0
y_offset = 0
position = "Center"  # 初始化 position

red_x = 160
red_y = 120

green_x = 160
green_y = 120

black_x = 160
black_y = 120

green_threshold =  (85, 97, -128, -22, 124, -128)  # 设定颜色色块参数
red_threshold = (75, 66, 10, 118, 89, -39)
red_threshold = (72, 100, -50, 127, -128, 127)
black_threshold = (39, 1, -128, 127, -41, 57)


#labels = ['1', '2', '3', '4', '5', '6', '7', '8']
labels = ['0', '1']
color_R = (255, 0, 0)
color_G = (0, 255, 0)
color_B = (0, 0, 255)
color_BLACK = (0,0,0)
color_WHITE = (255,255,255)
# ************************* 摄像头画面的初始化 *************************
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
# 如果对输入的镜头有格式要求
img_width = 240
img_height = 240
sensor_window = (img_width,img_height)
sensor_hmirror = False
sensor_vflip = True
sensor.set_windowing(sensor_window)
sensor.set_hmirror(sensor_hmirror)
sensor.set_vflip(sensor_vflip)
sensor.run(1)
#sensor.skip_frames()   #跳过一些图片让摄像头稳定


# ************************* lcd画面的初始化 *************************
lcd.init(type=1)  #type=1正常，type=2反色
#lcd.init(freq = 1500000)
lcd.rotation(0)   #旋转方向  0-0°  1-90°  2-180° 3-270°

#lcd.display(img)  # 显示图像   #在后续完成了位置的标注后，再进行显示

#img.draw_string(50, 50, "img") #在图片上写字
#lcd.draw_string(0, 0, "lcd")   #在lcd上写字 #最好在图片上写字，因为这种翻转是有用的

# ************************* 串口的初始化设置 *************************
fm.register(24, fm.fpioa.UART1_TX, force=True)
fm.register(25, fm.fpioa.UART1_RX, force=True)
uart_A = UART(UART.UART1, 9600, 8, 0, 0, timeout=1000, read_buf_len=4096)
uart_A.write(b'ready\r\n')


# ************************* 定时器的中断 *************************
def send_data(timer):
    data = "none\r\n".encode()
    uart_A.write(data)
'''
    if classID != -1:
        abs_x_offset_str = str(abs(x_offset))
        data = labels[classID].encode() + ":".encode() + position.encode() + abs_x_offset_str.encode() + "\r\n".encode()
    else:
        data = "none\r\n".encode()
'''
tim = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PERIODIC, period=500, unit=Timer.UNIT_MS, callback=send_data)

# ************************ 循迹黑色的线 **************************
def track_task():
    red_roi_size = 30  # 十字架的尺寸，根据需要调整
    # 计算子区域的左上角和右下角坐标
    roi_left = max(red_x - red_roi_size, 0)  # 左上角x坐标，不能小于0
    roi_top = max(red_y - red_roi_size, 0)   # 左上角y坐标，不能小于0
    roi_right = min(red_x + red_roi_size, img_width)    # 右下角x坐标，不能大于图像宽度
    roi_bottom = min(red_y + red_roi_size, img_height)  # 右下角y坐标，不能大于图像高度
    #print("left:",roi_left," top:",roi_top," right:",roi_right," bottom:",roi_bottom)
    # 处理边界情况，确保子区域不超出图像范围
    red_roi_region = img.copy(roi=(roi_left, roi_top, roi_right, roi_bottom))

    blobs = red_roi_region.find_blobs([black_threshold])  # 在img中找色块
    #blobs = img.find_blobs([black_threshold])  # 在img中找色块
    if blobs:
        # 对色块按面积进行排序，从大到小
        blobs = sorted(blobs, key=lambda b: b.area(), reverse=True)

        #for b in blobs:
        for i, b in enumerate(blobs):
            if i > 0:  # 只处理前三个最大的目标
                break


            if b.area() > min_blob_area:  # 判断色块面积是否大于阈值
                # 进行框图表示
                itemROL = b.rect()
                x, y, w, h = itemROL
                print("x:",x," y:",y," w=",w," h=",h)
                abs_x = x+roi_left
                abs_y = y+roi_top
                itemROL = (abs_x, abs_y, w, h)
                #print("x:",abs_x," y:",abs_y," w=",w," h=",h)
                img.draw_rectangle(itemROL,scale=2, color=color_WHITE)     # 画一个矩形框出目标
                img.draw_cross(roi_left+b.cx(), roi_top+b.cy(), color=color_WHITE)  # 在图像中心画十字叉
                img.draw_string(abs_x, abs_y-20,"B", scale=2, color=color_WHITE)

                global black_x
                global black_y
                black_x = abs_x
                black_y = abs_y







# ************************* 寻找色块 *************************

while True:
    img = sensor.snapshot()

    # ----------------------- 黑色方块 ------------------------

    #img = img.to_grayscale(copy=False)
    # 将图像转换为灰度图像
    '''
    img_gray = img.to_grayscale(copy=True)

    # 找到边缘中的直线段
    lines = img_gray.find_lines()
    '''

    lines = img.find_lines()
    # 在图像上绘制直线
    i=0
    for i,l in lines:
        # 只处理较长的直线段
        if l.length() > 100:
            # 获取直线段的两个端点坐标
            x1, y1, x2, y2 = l.line()

            # 在图像上画线
            img.draw_line((x1, y1, x2, y2), color=color_B, thickness=5)
            print("i="i,"  x1=",x1," y1=",y1,"   x2=",x2," y2=",y2)

    # 计算直线之间的交点
    i = 0
    for i in range(len(lines)):
        for j in range(i + 1, len(lines)):
            line1 = lines[i]
            line2 = lines[j]

            # 获取直线参数
            theta1 = line1.theta()  # 直线1的角度
            rho1 = line1.rho()      # 直线1的p值

            theta2 = line2.theta()  # 直线2的角度
            rho2 = line2.rho()      # 直线2的p值

            # 判断两条直线是否平行
            if abs(theta1 - theta2) < 1e-2:
                continue

            # 计算交点坐标
            x = (rho2 * math.sin(math.radians(theta1)) - rho1 * math.sin(math.radians(theta2))) / (math.cos(math.radians(theta1 - theta2))+0.1)
            y = (rho1 - x * math.cos(math.radians(theta1))) / (math.sin(math.radians(theta1))+0.1)

            # 在图像上画交点
            img.draw_circle(int(x), int(y), 5, color=(0, 0, 255), fill=True)

    # ----------------------- 红色色块 ------------------------
    blobs = img.find_blobs([red_threshold])  # 在img中找色块
    if blobs:
        # 对色块按面积进行排序，从大到小
        blobs = sorted(blobs, key=lambda b: b.area(), reverse=True)

        #for b in blobs:
        for i, b in enumerate(blobs):
            if i > 0:  # 只处理前三个最大的目标
                break


            if b.area() > min_blob_area:  # 判断色块面积是否大于阈值
                # 进行框图表示
                itemROL = b.rect()
                x, y, w, h = itemROL
                img.draw_rectangle(itemROL,scale=2, color=color_R)     # 画一个矩形框出目标
                img.draw_cross(b.cx(), b.cy(),color=color_R)  # 在图像中心画十字叉
                img.draw_string(x, y-20,"R", scale=2, color=color_R)

                red_x = b.cx()
                red_y = b.cy()

                #识别红点周围的黑色线
                #track_task()



    # ----------------------- 绿色色块 ------------------------
    blobs = img.find_blobs([green_threshold])  # 在img中找色块
    if blobs:
        # 对色块按面积进行排序，从大到小
        blobs = sorted(blobs, key=lambda b: b.area(), reverse=True)

        #for b in blobs:
        i = 0
        for i, b in enumerate(blobs):
            if i > 0:  # 只处理前三个最大的目标
                break


            if b.area() > min_blob_area:  # 判断色块面积是否大于阈值
                # 进行框图表示
                itemROL = b.rect()
                x, y, w, h = itemROL
                img.draw_rectangle(itemROL,scale=2, color=color_G)     # 画一个矩形框出目标
                img.draw_cross(b.cx(), b.cy(),color=color_G)  # 在图像中心画十字叉
                img.draw_string(x, y-20,"G", scale=2, color=color_G)

                green_x = b.cx()
                green_y = b.cy()


    lcd.display(img)  # 先显示图像

    lcd.draw_string(10, 10, " RED :" + str(red_x) +','+ str(red_y), lcd.RED, lcd.BLACK)  # 再显示文字
    lcd.draw_string(10, 30, "BLACK:" + str(black_x) +','+ str(black_y), lcd.WHITE, lcd.BLACK)  # 再显示文字
    lcd.draw_string(10, 50, "GREEN:" + str(green_x) +','+ str(green_y), lcd.GREEN, lcd.BLACK)  # 再显示文字


# ************************* yolo v2 *************************
'''
my_model = kpu.load(0x300000)
#anchors = [0.84, 1.22, 1.66, 2.34, 1.31, 1.75, 1.88, 2.59, 1.47, 2.09]
anchors = [3.8125, 3.8125, 5.375, 5.375, 7.1875, 7.1875, 11.25, 11.3125, 9.125, 9.125]
_ = kpu.init_yolo2(my_model, 0.5, 0.3, 5, anchors)
while True:
    img = sensor.snapshot()
    objects = kpu.run_yolo2(my_model,img)
    if objects:
        for obj in objects:
            confidence = obj.value()
            itemROL = obj.rect()
            classID = int(obj.classid())
            x, y, w, h = itemROL

            # 在相应位置，标注
            img.draw_rectangle(itemROL)     # 画一个矩形框出目标
            img.draw_cross(x+w//2, y+h//2)  # 在图像中心画十字叉
            img.draw_string(x, y-20,"%s : %.2f" %(labels[classID], confidence), scale=2, color=color_R)

            # 标注出位置
            x_offset = x+w//2 - img.width()//2  # X轴偏移量
            y_offset = y+h//2 - img.height()//2  # Y轴偏移量
            if x_offset > 50:
                position = "right"
            elif x_offset < -50:
                position = "left"
            else:
                position = "Center"

            img.draw_string(x, y+10, "%s : %d"%(position,x_offset))
    else:
        classID = -1

    lcd.display(img)  # 先显示图像
    #lcd.draw_string(10, 10, "Position: " + position, lcd.WHITE, lcd.BLACK)  # 再显示文字
    #lcd.draw_string(10, 30, "X Offset: " + str(x_offset), lcd.WHITE, lcd.BLACK)  # 显示X轴偏移
    #lcd.draw_string(10, 50, "Y Offset: " + str(y_offset), lcd.WHITE, lcd.BLACK)  # 显示Y轴偏移
'''