black_grayscale_line_following.py

# Este trabalho está licenciado sob a licença MIT.
# Copyright (c) 2013-2023 OpenMV LLC. Todos os direitos reservados.
# https://github.com/openmv/openmv/blob/master/LICENSE
#
# Exemplo de Seguidor de Linha Preta em Escala de Cinza
#
# Construir um robô seguidor de linha requer muito esforço. Este script de exemplo
# mostra como fazer a parte de visão computacional do robô seguidor de linha. Você
# pode usar a saída deste script para acionar um robô de tração diferencial para
# seguir uma linha. Este script apenas gera um único valor de curva que indica
# ao seu robô para ir para a esquerda ou direita.
#
# Para que este script funcione corretamente, você deve apontar a câmera para uma linha em
# um ângulo de aproximadamente 45 graus. Certifique-se de que apenas a linha esteja no
# campo de visão da câmera.

import csi
import time
import math

# Rastreia uma linha preta. Use [(128, 255)] para rastrear uma linha branca.
GRAYSCALE_THRESHOLD = [(0, 64)]

# Cada roi é (x, y, w, h). O algoritmo de detecção de linha tentará encontrar o
# centroide do maior blob em cada roi. A posição x dos centroides
# será então calculada com uma média ponderada, onde o maior peso é atribuído
# ao roi próximo à parte inferior da imagem, e menos ao próximo roi, e assim por diante.
ROIS = [  # [ROI, peso]
    (0, 100, 160, 20, 0.7),  # Você precisará ajustar os pesos para sua aplicação
    (0, 50, 160, 20, 0.3),  # dependendo de como seu robô está configurado.
    (0, 0, 160, 20, 0.1),
]

# Calcula o divisor de peso (fazemos isso para que você não precise fazer os pesos somarem 1).
weight_sum = 0
for r in ROIS:
    weight_sum += r[4]  # r[4] é o peso do roi.

# Configuração da câmera...
csi0 = csi.CSI()
csi0.reset()  # Inicializa o sensor da câmera.
csi0.pixformat(csi.GRAYSCALE)  # usar escala de cinza.
csi0.framesize(csi.QQVGA)  # usar QQVGA para velocidade.
csi0.snapshot(time=2000)  # Aguarda as novas configurações fazerem efeito.
csi0.auto_gain(False)  # deve ser desativado para o rastreamento por cor
csi0.auto_whitebal(False)  # deve ser desativado para o rastreamento por cor
clock = time.clock()  # Rastreia FPS.

while True:
    clock.tick()  # Rastreia os milissegundos decorridos entre snapshots().
    img = csi0.snapshot()  # Tira uma foto e retorna a imagem.

    centroid_sum = 0

    for r in ROIS:
        blobs = img.find_blobs(
            GRAYSCALE_THRESHOLD, roi=r[0:4], merge=True
        )  # r[0:4] é a tupla do roi.

        if blobs:
            # Encontra o blob com mais pixels.
            largest_blob = max(blobs, key=lambda b: b.pixels)

            # Desenha um retângulo ao redor do blob.
            img.draw_detection(largest_blob)

            centroid_sum += largest_blob.cx * r[4]  # r[4] é o peso do roi.

    center_pos = centroid_sum / weight_sum  # Determina o centro da linha.

    # Converte center_pos em um ângulo de deflexão. Estamos usando uma operação
    # não linear para que a resposta fique mais forte quanto mais longe da linha
    # estivermos. Operações não lineares são boas para usar na saída de algoritmos
    # como este para causar um "gatilho" de resposta.
    deflection_angle = 0

    # O 80 vem da metade da resolução X, o 60 vem da metade da resolução Y. A
    # equação abaixo apenas calcula o ângulo de um triângulo onde o
    # cateto oposto do triângulo é o desvio da posição central
    # em relação ao centro, e o cateto adjacente é metade da resolução Y. Isso limita
    # a saída do ângulo a aproximadamente -45 a 45. (Não é exatamente -45 e 45).
    deflection_angle = -math.atan((center_pos - 80) / 60)

    # Converte o ângulo de radianos para graus.
    deflection_angle = math.degrees(deflection_angle)

    # Agora você tem um ângulo que indica quanto girar o robô, o que
    # incorpora a parte da linha mais próxima do robô e partes
    # da linha mais distantes do robô, para uma melhor previsão.
    print("Turn Angle: %f" % deflection_angle)

    print(clock.fps())  # Nota: Sua OpenMV Cam roda cerca de metade da velocidade enquanto
    # conectada ao seu computador. O FPS deve aumentar após a desconexão.