black_grayscale_line_following.py

# Esta obra está licenciada bajo la licencia MIT.
# Copyright (c) 2013-2023 OpenMV LLC. Todos los derechos reservados.
# https://github.com/openmv/openmv/blob/master/LICENSE
#
# Ejemplo de seguimiento de línea negra en escala de grises
#
# Crear un robot seguidor de líneas requiere bastante esfuerzo. Este script de ejemplo
# muestra cómo realizar la parte de visión artificial del robot seguidor de líneas. Puede
# usar la salida de este script para controlar un robot de tracción diferencial y así
# seguir una línea. Este script simplemente genera un único valor de giro que indica a
# su robot que gire a la izquierda o a la derecha.
#
# Para que este script funcione correctamente, debe apuntar la cámara hacia una línea con
# un ángulo aproximado de 45 grados. Asegúrese de que solo la línea esté dentro del
# campo de visión de la cámara.

import csi
import time
import math

# Rastrea una línea negra. Use [(128, 255)] para rastrear una línea blanca.
GRAYSCALE_THRESHOLD = [(0, 64)]

# Cada roi es (x, y, w, h). El algoritmo de detección de líneas intentará encontrar el
# centroide del blob más grande en cada roi. La posición x de los centroides
# se promediará después con distintos pesos, donde se asigna más peso
# al roi cercano a la parte inferior de la imagen y menos al siguiente roi, y así sucesivamente.
ROIS = [  # [ROI, peso]
    (0, 100, 160, 20, 0.7),  # Deberá ajustar los pesos para su aplicación
    (0, 50, 160, 20, 0.3),  # dependiendo de cómo esté configurado su robot.
    (0, 0, 160, 20, 0.1),
]

# Calcula el divisor de los pesos (lo calculamos para que usted no tenga que hacer que los pesos sumen 1).
weight_sum = 0
for r in ROIS:
    weight_sum += r[4]  # r[4] es el peso del roi.

# Configuración de la cámara...
csi0 = csi.CSI()
csi0.reset()  # Inicializar el sensor de la cámara.
csi0.pixformat(csi.GRAYSCALE)  # usar escala de grises.
csi0.framesize(csi.QQVGA)  # usar QQVGA por velocidad.
csi0.snapshot(time=2000)  # Deje que la nueva configuración surta efecto.
csi0.auto_gain(False)  # debe estar desactivado para el seguimiento de color
csi0.auto_whitebal(False)  # debe estar desactivado para el seguimiento de color
clock = time.clock()  # Registra los FPS.

while True:
    clock.tick()  # Registre los milisegundos transcurridos entre snapshots().
    img = csi0.snapshot()  # Tomar una foto y devolver la imagen.

    centroid_sum = 0

    for r in ROIS:
        blobs = img.find_blobs(
            GRAYSCALE_THRESHOLD, roi=r[0:4], merge=True
        )  # r[0:4] es la tupla del roi.

        if blobs:
            # Encontrar el blob con más píxeles.
            largest_blob = max(blobs, key=lambda b: b.pixels)

            # Dibujar un rectángulo alrededor del blob.
            img.draw_detection(largest_blob)

            centroid_sum += largest_blob.cx * r[4]  # r[4] es el peso del roi.

    center_pos = centroid_sum / weight_sum  # Determinar el centro de la línea.

    # Convierte center_pos en un ángulo de deflexión. Estamos usando una operación
    # no lineal para que la respuesta se vuelva más fuerte cuanto más nos alejamos de
    # la línea. Las operaciones no lineales son buenas para usar en la salida de algoritmos
    # como este para provocar un "disparador" de respuesta.
    deflection_angle = 0

    # El 80 proviene de la mitad de la resolución X, el 60 de la mitad de la resolución Y. La
    # ecuación siguiente simplemente calcula el ángulo de un triángulo donde el
    # cateto opuesto del triángulo es la desviación de la posición central
    # respecto al centro, y el cateto adyacente es la mitad de la resolución Y. Esto limita
    # la salida del ángulo a un rango aproximado de -45 a 45. (No es exactamente -45 y 45).
    deflection_angle = -math.atan((center_pos - 80) / 60)

    # Convertir el ángulo de radianes a grados.
    deflection_angle = math.degrees(deflection_angle)

    # Ahora tiene un ángulo que le indica cuánto girar el robot, el cual
    # incorpora la parte de la línea más cercana al robot y partes de
    # la línea más alejadas del robot para lograr una mejor predicción.
    print("Turn Angle: %f" % deflection_angle)

    print(clock.fps())  # Nota: Su OpenMV Cam funciona aproximadamente a la mitad de velocidad mientras
    # está conectada a su computadora. Los FPS deberían aumentar una vez desconectada.