Coche de preguntas C de deportes electrónicos 2022: OpenMV

---

prefacio

OpenMV es un potente módulo de visión artificial de código abierto . Muchas funciones básicas, como el reconocimiento de bloques de colores, se pueden realizar fácilmente llamando a las funciones. Al usar estas funciones hábilmente, OpenMV se puede usar para reemplazar otros periféricos. Los periféricos de nuestro coche provincial son muy sencillos, solo OpenMV, el teclado para configurar el modo, el zumbador para avisos sonoros, el coche de cabecera y el Bluetooth para comunicarse con el coche. Al principio, me preocupaba que el automóvil corriera demasiado rápido y que la tasa de retorno de OpenMV no fuera suficiente, pero la prueba encontró que la tasa de cuadros de OpenMV puede alcanzar 50 ~ 60 fps, lo cual es completamente suficiente.
Es decir, OpenMV realiza las tres funciones de seguimiento, reconocimiento de línea de parada y medición de distancia al mismo tiempo .

---

1. Pregunta de revisión C

1. Tarea

Diseñe un sistema de conducción de seguimiento de automóviles, utilizando MCU de TI, que consta de un automóvil líder y un automóvil siguiente. Se requiere que el automóvil tenga una función de seguimiento, y la velocidad es ajustable de 0.3 a 1 m / s, y puede completar la conducción en la ruta especificada Operación, la ruta del sitio de conducción se muestra en la Figura 1. Entre ellos, el punto A en el camino es el punto de inicio y el punto final de cada conducción del automóvil líder. Cuando el automóvil completa un viaje y llega al punto final, el automóvil líder y el automóvil siguiente emitirán indicaciones sonoras. El coche que va delante y el coche que le sigue pueden viajar a lo largo de la ruta del rectángulo redondeado ABFDE (referido como el círculo interior para abreviar), o a lo largo del camino del rectángulo redondeado ABCDE (referido como el círculo exterior para abreviar). Al conducir en la sección BFD del círculo interior, el carro debe emitir un indicador luminoso. Además, durante la prueba, el experto en pruebas puede colocar una señal de "instrucción de espera y parada" (ver el lado izquierdo de la Figura 1) en el área de línea recta en el camino donde el punto E está ubicado (ver el lado izquierdo de la Figura 1). Figura 1), indicando que el coche de cabeza debe detenerse aquí y esperar Continúe conduciendo después de 5 segundos.

2. Requisitos

1. Coloque el automóvil líder en el punto A en la posición inicial del camino, y coloque el automóvil siguiente 20 cm detrás de él, establezca la velocidad del automóvil líder en 0,3 m / sy conduzca un círculo a lo largo del círculo exterior para detenerse. : (20 puntos)
   (1) El error de velocidad promedio del automóvil líder no es superior al 10%;
   (2) El automóvil siguiente puede seguir al automóvil líder y no pueden ocurrir colisiones durante todo el proceso;
   (3) Después El automóvil líder se detiene en el punto A después de completar una vuelta, el automóvil siguiente debe detenerse a tiempo. Deténgase, la diferencia de tiempo de parada no es más de 1 segundo, y la distancia desde el automóvil líder es de 20 cm, y el error no es más de 6 cm. .
2. Coloque el automóvil líder en el punto A en la posición inicial de la trayectoria del camino, y coloque el automóvil siguiente en el área de línea recta en el camino donde se encuentra el punto E, en la posición designada por el experto de prueba, establezca la velocidad del líder coche a 0,5 m/s, y siga la trayectoria del círculo exterior Conduzca dos vueltas hasta detenerse, requisitos: (20 puntos)
   (1) El error de velocidad promedio del coche que va delante no es superior al 10 %;
   (2) El coche siguiente puede Alcance rápidamente al automóvil líder y luego siga al automóvil líder a una distancia de 20 cm, y no ocurrirá ningún automóvil durante todo el proceso Colisión:
   (3) Después de completar dos vueltas, el automóvil líder llega al punto A y se detiene. el auto debe detenerse a tiempo. La diferencia de tiempo entre los dos autos que se detienen no debe exceder 1 segundo, y la distancia entre el auto líder y el auto líder debe ser de 20 cm, y el error no debe ser mayor a 6 cm.
3. Coloque el automóvil líder en el punto A en la posición inicial del camino y coloque el automóvil siguiente 20 cm detrás de él. El automóvil líder y el automóvil siguiente completan tres vueltas del camino continuamente. En la primera vuelta, tanto el automóvil líder como el automóvil siguiente viajan a lo largo del camino del círculo exterior. En el segundo círculo, el automóvil que va en cabeza conduce a lo largo de la ruta del círculo exterior y sigue al automóvil a lo largo de la ruta del círculo interior para lograr adelantar y liderar. En la tercera vuelta, el automóvil siguiente conduce a lo largo del camino del círculo exterior, y el automóvil líder conduce a lo largo del camino del círculo interior, realizando adelantamientos y liderando nuevamente. Requisitos: (30 puntos)
   (1) Los dos autos funcionan sin problemas durante todo el proceso, completan con éxito dos adelantamientos y no se produce ninguna colisión;
   (2) Después de completar tres vueltas, el auto líder se detiene en el punto A y el siguiente auto debe detenerse a tiempo.La diferencia de tiempo entre los autos que se detienen
   no es más de 1 segundo, y la distancia desde el auto líder es de 20 cm, y el error no es más de 6 cm;
   (3) La velocidad de conducción del auto se puede configurar de forma independiente , pero no debe ser inferior a 0,3 m/s, y se deben completar las tres vueltas especificadas. Cuanto menor sea el tiempo necesario para recorrer la trayectoria, mejor.
4. El experto en pruebas designará una posición en el área de línea recta en el lado donde se encuentra el punto E del camino, y colocará el letrero de "Instrucción de espera y parada". Luego, coloque el automóvil líder en el punto A en la posición inicial del camino, y coloque el automóvil siguiente 20 cm detrás de él. Establezca la velocidad del automóvil líder en 1 m / s, y conduzca un círculo a lo largo del camino del círculo exterior. Los dos los coches no deben chocar durante la conducción
   . Requisitos: (20 puntos)
   (1) El error de velocidad promedio del automóvil líder no es superior al 10%;
   (2) El automóvil líder se detiene en el punto de "indicación de parada de espera", la posición de estacionamiento es precisa y el error es no más de 5 cm
   ; El tiempo de estacionamiento en la "indicación" es de 5 s, y el error no es más de 1 s.

2. OpenMV implementa ideas y códigos funcionales

1. Seguimiento

El código utilizado por OpenMV para el módulo de seguimiento ha sido compartido por muchos blogueros en Internet. La idea general es:
1. Divida el campo de visión de la imagen en tres partes, superior, media e inferior, y al mismo tiempo busque el bloque de color negro más grande (también puede encontrar otros bloques de color para ver qué línea de color se requiere ), es decir, llame a la función img que viene con OpenMV .find_blobs y luego filtre el bloque de color más grande.

2. Calcule el ángulo de compensación de la trayectoria actual en relación con el automóvil a través de los valores cx y cy ​​de los puntos centrales de los tres bloques de colores, para facilitar el cálculo de PID.

OpenMV的色块识别有很多细节方面，可以更加巧妙的利用，若有时间可以整理出一篇blog记录一下

def car_run():

    centroid_sum = 0

    #利用颜色识别分别寻找三个矩形区域内的线段
    for r in ROIS:
        blobs = img.find_blobs(GRAYSCALE_THRESHOLD, roi=r[0:4], merge=True)
        # r[0:4] is roi tuple.
        #找到视野中的线,merge=true,将找到的图像区域合并成一个

        #目标区域找到色块
        if blobs:
            # Find the index of the blob with the most pixels.
            most_pixels = 0
            largest_blob = 0
            for i in range(len(blobs)):
            #目标区域找到的颜色块（线段块）可能不止一个，找到最大的一个，作为本区域内的目标直线
                if blobs[i].pixels() > most_pixels:
                    most_pixels = blobs[i].pixels()
                    #merged_blobs[i][4]是这个颜色块的像素总数，如果此颜色块像素总数大于                     #most_pixels，则把本区域作为像素总数最大的颜色块。更新most_pixels和largest_blob
                    largest_blob = i

            # Draw a rect around the blob.
            img.draw_rectangle(blobs[largest_blob].rect())

            #将此区域的像素数最大的颜色块画矩形和十字形标记出来
            img.draw_cross(blobs[largest_blob].cx(),
                           blobs[largest_blob].cy())

            centroid_sum += blobs[largest_blob].cx() * r[4] # r[4] is the roi weight.
            #计算centroid_sum，centroid_sum等于每个区域的最大颜色块的中心点的x坐标值乘本区域的权值

    center_pos = (centroid_sum / weight_sum) # Determine center of line.
    #中间公式

    # Convert the center_pos to a deflection angle. We're using a non-linear
    # operation so that the response gets stronger the farther off the line we
    # are. Non-linear operations are good to use on the output of algorithms
    # like this to cause a response "trigger".
    deflection_angle = 0
    #机器人应该转的角度

    # The 80 is from half the X res, the 60 is from half the Y res. The
    # equation below is just computing the angle of a triangle where the
    # opposite side of the triangle is the deviation of the center position
    # from the center and the adjacent side is half the Y res. This limits
    # the angle output to around -45 to 45. (It's not quite -45 and 45).
    deflection_angle = -math.atan((center_pos-160)/120)
    #角度计算.80 60 分别为图像宽和高的一半，图像大小为QQVGA 160x120.
    #注意计算得到的是弧度值

    # Convert angle in radians to degrees.
    deflection_angle = math.degrees(deflection_angle)
    #将计算结果的弧度值转化为角度值
    A=deflection_angle+90
    return int(A)
    #由于数据需要传回给单片机，不适合传输负数，因此+90调节数据

Sin embargo, esta pregunta tiene un círculo interior y un círculo exterior. Obviamente, es inapropiado identificar un solo bloque de color en la bifurcación, así que identifique los dos bloques de color más grandes en cada área de las áreas superior, media e inferior. Al mismo tiempo, para evitar interferencias de fondo, se filtran los bloques de color identificados.Pequeñas manchas de color . De esta manera, hay dos situaciones:
1. No hay bifurcación, y solo hay un camino. En este momento, cada área tiene solo un bloque de color y el principio de cálculo es el mismo que el código anterior.

2. Si hay una bifurcación, aparecerán dos caminos. Puede haber un tenedor en el área frontal, o un tenedor en dos áreas, o un tenedor en las tres áreas. En este momento, los ángulos de desplazamiento de las dos rutas a la izquierda y a la derecha se calculan por separado, y las áreas donde aparecen dos bloques de color se calculan en los lados izquierdo y derecho, y las áreas con un solo bloque de color se calculan utilizando este color. bloquear.
Entrar en la bifurcación de la carretera:



salir de la bifurcación de la carretera (desde la perspectiva del círculo exterior, lo mismo es cierto para el círculo interior):

el código se muestra a continuación:

def car_run():
    centroid_sum = [0,0]
    left_center=[-1,-1,-1]
    right_center=[-1,-1,-1]
    for r in range(3):
        blobs = img.find_blobs(GRAYSCALE_THRESHOLD, roi=ROIS[r][0:4], merge=True,area_threshold=100,margin=3)
        if blobs:
            max_ID=[-1,-1]#保存两个最大色块的ID
            max_ID=find_max(blobs)
            img.draw_rectangle(blobs[max_ID[0]].rect())
            img.draw_cross(blobs[max_ID[0]].cx(),
                           blobs[max_ID[0]].cy())
            if max_ID[1]!=-1:#如果识别到两个色块
                img.draw_rectangle(blobs[max_ID[1]].rect())
                img.draw_cross(blobs[max_ID[1]].cx(),
                               blobs[max_ID[1]].cy())
                #区分左边和右边
                if blobs[max_ID[0]].cx()<blobs[max_ID[1]].cx():
                    left_center[r]=blobs[max_ID[0]].cx()
                    right_center[r]=blobs[max_ID[1]].cx()
                else:
                    left_center[r]=blobs[max_ID[1]].cx()
                    right_center[r]=blobs[max_ID[0]].cx()
            else:
            	left_center[r]=right_center[r]=blobs[max_ID[0]].cx()
            	centroid_sum[0] += left_center[r] * ROIS[r][4]
            	centroid_sum[1] += right_center[r] * ROIS[r][4]
    center_pos =[0,0]
    center_pos[0] = (centroid_sum[0] / weight_sum)
    center_pos[1] = (centroid_sum[1] / weight_sum)
    deflection_angle = [0,0]
    deflection_angle[0] = -math.atan((center_pos[0]-80)/60)
    deflection_angle[1] = -math.atan((center_pos[1]-80)/60)
    #使用的QQVGA像素是160*120，因此中心点是（80,60）
    deflection_angle[0] = math.degrees(deflection_angle[0])
    deflection_angle[1] = math.degrees(deflection_angle[1])
    if center_pos[0]==center_pos[1]==0:
        deflection_angle[1]=deflection_angle[0]=0
    A=[int(deflection_angle[0])+90,int(deflection_angle[1])+90]
    return A

Una vez que se procesan los datos, se envían a la microcomputadora de un solo chip y el resto se entrega a la microcomputadora de un solo chip para su procesamiento. Aquí también hay una breve explicación de la idea del código MCU: use la perspectiva del automóvil para caminar por los carriles internos y externos, y descubra que los datos de la derecha se toman al tomar el carril exterior, y los datos de la izquierda se usan cuando caminando por el carril interior . De esta manera, si los lados izquierdo y derecho de los datos devueltos son iguales al caminar en línea recta, entonces la microcomputadora de un solo chip solo necesita seleccionar el ángulo de desplazamiento izquierdo o derecho de acuerdo con la selección del carril interior y exterior. modo, y luego se puede realizar la conducción del carril interior y el carril exterior.

2. Identifique la línea de parada

La línea de parada tiene un ancho de 5 cm. El uso de esta función resuelve directamente el problema de identificar la línea de parada al identificar el ángulo de compensación. Más tarde, de acuerdo con esta idea, también se resolvió el problema de esperar la línea de parada.
En pocas palabras, si el área de píxeles de un bloque de color es mayor que un cierto valor, se considera una línea de parada .

1. Primero, la identificación de la línea de parada se realiza cuando el ángulo de desviación izquierdo es igual al ángulo de desviación derecho, es decir, cuando no hay bifurcación, para evitar la influencia de la bifurcación.

2. Lo que se usa no es el tamaño del área del bloque de color, lo que provocará un error de juicio. Lo que se debe usar es el tamaño del área de píxeles, función blob.pixels() . (Habrá muchas ganancias inesperadas si tiene tiempo para leer más el manual)
3. El ángulo de inclinación de la cámara y la altura desde el suelo afectarán el valor de blob. El tamaño de píxel de la línea de parada no es el mismo, por lo que se deben establecer tres umbrales.
4. Es un poco demasiado simple y tosco identificar la línea de parada de esta manera, y se producirá un error de juicio, por lo que también se requiere la cooperación de la microcomputadora de un solo chip. El código describirá brevemente la idea de la microcomputadora de un solo chip.

def car_run():
    centroid_sum = [0,0]
    left_center=[-1,-1,-1]
    right_center=[-1,-1,-1]
    flag_cross=0
    flag_Stop=0	#停止线标志
    flag_Wait=[0,0]#等停线标志
    for r in range(3):
        blobs = img.find_blobs(GRAYSCALE_THRESHOLD, roi=ROIS[r][0:4], merge=True,area_threshold=100,margin=3)
        if blobs:
            max_ID=[-1,-1]
            max_ID=find_max(blobs)
            img.draw_rectangle(blobs[max_ID[0]].rect())
            img.draw_cross(blobs[max_ID[0]].cx(),
                           blobs[max_ID[0]].cy())
            if max_ID[1]!=-1:
                img.draw_rectangle(blobs[max_ID[1]].rect())
                flag_cross=1
                img.draw_cross(blobs[max_ID[1]].cx(),
                               blobs[max_ID[1]].cy())
                if blobs[max_ID[0]].cx()<blobs[max_ID[1]].cx():
                    left_center[r]=blobs[max_ID[0]].cx()
                    right_center[r]=blobs[max_ID[1]].cx()
                else:
                    left_center[r]=blobs[max_ID[1]].cx()
                    right_center[r]=blobs[max_ID[0]].cx()
            else:
                #print(blobs[max_ID[0]].pixels(),blobs[max_ID[0]].w())
                if flag_cross==0:
                    if blobs[max_ID[0]].pixels()>range_stop[r]:#range_stop为三个区域停止线的像素阈值
                        flag_Stop=r+1	#停止线标志的值为3,2，1，表示所在区域
                    if blobs[max_ID[0]].w()>range_wait[r]:
                        flag_Wait[0]=flag_Wait[0]+1#等待停止线同理
                left_center[r]=right_center[r]=blobs[max_ID[0]].cx()
            centroid_sum[0] += left_center[r] * ROIS[r][4]
            centroid_sum[1] += right_center[r] * ROIS[r][4]
    center_pos =[0,0]
    center_pos[0] = (centroid_sum[0] / weight_sum)
    center_pos[1] = (centroid_sum[1] / weight_sum)
    if flag_Wait[0]==2:
        flag_Wait[1]=1
    deflection_angle = [0,0]
    K=0.8
    deflection_angle[0] = -math.atan((center_pos[0]-80)/60)
    deflection_angle[1] = -math.atan((center_pos[1]-80)/60)
    deflection_angle[0] = math.degrees(deflection_angle[0])
    deflection_angle[1] = math.degrees(deflection_angle[1])
    if center_pos[0]==center_pos[1]==0:
        deflection_angle[1]=deflection_angle[0]=0
    A=[int(deflection_angle[0])+90,int(deflection_angle[1])+90,flag_Stop,flag_Wait[1]]
    return A

Idea de microcomputadora de un solo chip: el automóvil se acerca gradualmente a la línea de parada, por lo que la posición de la línea de parada es de lejos a cerca, por lo que el valor devuelto es 3-> 2-> 1, cuando recibe 3, 2, 1 junto el camino, se considera que se ha detenido la línea, detenerse inmediatamente . Recibir a lo largo de la línea no significa recibir continuamente, porque algunas áreas de visión no reconocen bloques de color, es decir, áreas ciegas de visión, y la línea de parada pasará por las áreas ciegas de visión y devolverá 0. Y este método también puede evitar errores de juicio en el tenedor.
El principio de esperar la línea de parada es similar al de la línea de parada. Cuando se identifica la línea de parada de espera, también se reconocerá la línea de parada. Por lo tanto, es una coincidencia que cuando se requieren cuatro veces, cuando la línea de parada de espera La línea y la línea de parada se reconocen al mismo tiempo, la línea de parada en espera se procesa. Incluso si la vuelve a ver después de detener la línea, no la trate. Es decir, la primera parada está esperando una parada y la segunda parada es una parada.

3. Reconocimiento de distancia

De hecho, quería usar OpenMV para la medición de distancias desde el principio, usando el seguimiento de marcadores AprilTag integrado, que es equivalente a identificar un marcador para obtener la distancia. En ese momento, se pegó un trozo de cartón en la parte trasera del automóvil y luego se pegó el logotipo.

El experimento encontró que no es factible.Para nuestro automóvil, la cámara se coloca en la parte superior. (Por supuesto, si es un auto con muchas cámaras instaladas en diferentes lugares, está bien, pero no tenemos dinero.) Este ángulo es muy complicado Incluso si se usa un AprilTag pequeño, la distancia entre los dos autos es de 20 cm. para apenas ver todo. Cuando lo vi, ya estaba a 20 cm de distancia, y estaba relativamente cerca, si no lo hubiera visto, me habría topado con él directamente.
Basado en esta idea, ya que se puede ver, está relativamente cerca, por lo que se involucra directamente en un reconocimiento de bloque de color. El reconocimiento de bloque de color de OpenMV realmente me hace odiarlo y amarlo. Qué doloroso es ajustar el umbral, qué fácil es este reconocimiento de bloque de color. Adjuntamos un tablero azul completo a la parte trasera del automóvil y, por cierto, cubrimos las ruedas del automóvil, para evitar que el automóvil trasero reconozca la rueda delantera como una pista.
También pensé en muchas formas de identificar el bloque de color y juzgar la distancia, como el área del bloque de color, el área del píxel, la longitud del bloque de color, el punto central del bloque de color, etc. Finalmente, me di unas palmaditas en la cabeza y usé directamente el valor y del borde inferior del bloque de color, y luego configuré los parámetros de acuerdo con la medida real k, ajustado a un valor de distancia . El lado negativo de B en la gama de colores LAB es azul. En ese momento, el área azul estaba agotada y L y A también estaban agotadas, por lo que no había necesidad de preocuparse por la influencia de los cambios de luz en el color.
LAB这里简单说一下：L*代表明度，取值0~100， a*代表从绿色到红色的分量 ，取值-128~127，b*代表从蓝色到黄色的分量，取值-128~127

Probablemente solo haya una medida de distancia limitada, y el resto del ajuste de velocidad se deja en manos de la microcomputadora de un solo chip.
código:

blobs=img.find_blobs([(30,60,-30,-10,-25,-12)],pixels_threshold=300,area_threshold=300,merge=False)
max_size=0
if blobs:
    for blob in blobs:
        if blob.cy()+0.5*blob.h() > max_size:
            img.draw_rectangle(blob.rect(),(255,0,0))
            max_size = blob.cy()+0.5*blob.h()
    row_data[2]=int(k*(120-max_size))

Resumir

Los documentos de referencia de OpenMV deben leerse más, usarse más y llamarse funciones miembro de manera flexible.Muchos problemas en realidad no son muy complicados. El otro es hacer un buen uso de la potencia informática de la microcomputadora de un solo chip para procesar los datos transmitidos por OpenMV.
Agregar código MSP: https://download.csdn.net/download/weixin_52385589/86393773?spm=1001.2014.3001.5503

import sensor, image, time, math
from pyb import UART,LED
LED(3).on()
uart = UART(3, 115200, timeout_char=1000)
u_start=bytearray([0xb3,0xb3])
u_over=bytearray([0x0d,0x0a])
GRAYSCALE_THRESHOLD = [(-125, 20, -21, 13, -28, 14)]#巡线的阈值
ROIS = [
        (0, 90, 160, 20, 0.7),
        (0, 050, 160, 20, 0.4),
        (0, 000, 160, 20, 0.05)
       ]#三个区域
weight_sum = 0
range_stop=[390,190,100]#停止线像素最小值
range_wait=[60,40,0]    #等待停止线像素最小值
for r in ROIS: weight_sum += r[4]
#摄像头设置
sensor.reset()
sensor.set_contrast(1)
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(30)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
clock = time.clock()
sensor.set_vflip(True)
sensor.set_hmirror(True)
thresholds=[(-100, 72, -128, -16, -128, 127)]
#该阈值用于测距

#寻找两个最大的色块，ID存在max_ID中，便于调用
def find_max(blobs):
    max_size=[0,0]
    max_ID=[-1,-1]
    for i in range(len(blobs)):
        if blobs[i].pixels()>max_size[0]:
            max_ID[1]=max_ID[0]
            max_size[1]=max_size[0]
            max_ID[0]=i
            max_size[0]=blobs[i].pixels()
        elif blobs[i].pixels()>max_size[1]:
            max_ID[1]=i
            max_size[1]=blobs[i].pixels()
    return max_ID


def car_run():
    centroid_sum = [0,0]
    left_center=[-1,-1,-1]  #存放左边色块的中心cx值用于计算左边的偏移角
    right_center=[-1,-1,-1] #存放右边色块的中心cx值用于计算右边的偏移角
    flag_cross=0    #是否有分岔口
    flag_Stop=0     #停止标志
    flag_Wait=[0,0] #等待停止标志
    for r in range(3):  #三个区域分别寻找色块
        blobs = img.find_blobs(GRAYSCALE_THRESHOLD, roi=ROIS[r][0:4], merge=True,area_threshold=100,margin=3)
        if blobs:
            max_ID=[-1,-1]
            max_ID=find_max(blobs)  #找最大色块
            img.draw_rectangle(blobs[max_ID[0]].rect())
            img.draw_cross(blobs[max_ID[0]].cx(),
                           blobs[max_ID[0]].cy())
            if max_ID[1]!=-1:   #如果有两个色块，即有分岔口，分成左边右边存入数组
                img.draw_rectangle(blobs[max_ID[1]].rect())
                flag_cross=1
                img.draw_cross(blobs[max_ID[1]].cx(),
                               blobs[max_ID[1]].cy())
                if blobs[max_ID[0]].cx()<blobs[max_ID[1]].cx():
                    left_center[r]=blobs[max_ID[0]].cx()
                    right_center[r]=blobs[max_ID[1]].cx()
                else:
                    left_center[r]=blobs[max_ID[1]].cx()
                    right_center[r]=blobs[max_ID[0]].cx()
            else:   #只有一个色块
                #print(blobs[max_ID[0]].pixels(),blobs[max_ID[0]].w())
                if flag_cross==0:   #没有分岔口，进行判断停止线
                    if blobs[max_ID[0]].pixels()>range_stop[r]:
                        flag_Stop=r+1
                    if blobs[max_ID[0]].w()>range_wait[r]:
                        flag_Wait[0]=flag_Wait[0]+1
                left_center[r]=right_center[r]=blobs[max_ID[0]].cx()
            centroid_sum[0] += left_center[r] * ROIS[r][4] #乘权值
            centroid_sum[1] += right_center[r] * ROIS[r][4]
    center_pos =[0,0]
    center_pos[0] = (centroid_sum[0] / weight_sum)
    center_pos[1] = (centroid_sum[1] / weight_sum)
    if flag_Wait[0]==2:
        flag_Wait[1]=1
    deflection_angle = [0,0]
    deflection_angle[0] = -math.atan((center_pos[0]-80)/60)#计算角度
    deflection_angle[1] = -math.atan((center_pos[1]-80)/60)
    deflection_angle[0] = math.degrees(deflection_angle[0])#弧度制换成角度制
    deflection_angle[1] = math.degrees(deflection_angle[1])
    if center_pos[0]==center_pos[1]==0:
        deflection_angle[1]=deflection_angle[0]=0
    A=[int(deflection_angle[0])+90,int(deflection_angle[1])+90,flag_Stop,flag_Wait[1]]
    return A

def degrees(radians):
    return (180 * radians) / math.pi
k=1
while(True):
    times=0
    clock.tick()
    img = sensor.snapshot().lens_corr(strength = 1.8, zoom = 1.0)#不断拍照，进行鱼眼校正
    row_data=[0,0,0,0,0]
    row_data[0],row_data[1],row_data[3],row_data[4]=car_run()
    blobs=img.find_blobs([(30,60,-30,-10,-25,-12)],pixels_threshold=300,area_threshold=300,merge=False)
    max_size=0
    if blobs:
        for blob in blobs:
            if blob.cy()+0.5*blob.h() > max_size:
                img.draw_rectangle(blob.rect(),(255,0,0))
                max_size = blob.cy()+0.5*blob.h()
        row_data[2]=int(k*(120-max_size))#计算距离，k可调
    print(row_data)
    #传输数据给单片机
    uart_buf = bytearray(row_data)
    uart.write(u_start)
    uart.write(uart_buf)
    uart.write(u_over)