Example: 02-Image-Processing/02-Color-Tracking/black_grayscale_line_following.py

# 本作品采用MIT许可证授权。
# 版权所有 (c) 2013-2023 OpenMV LLC。保留所有权利。
# https://github.com/openmv/openmv/blob/master/LICENSE
#
# 黑色 Grayscale Line Following Example
#
# 制作一个循线机器人需要很多努力。这个示例脚本
# 展示了如何完成循线机器人的机器视觉部分。你可以
# 使用这个脚本的输出来驱动差速驱动机器人以
# 沿着一条线行进。该脚本仅生成一个单次转向值，指示
# 你的机器人向左或向右移动。
#
# 为了使此脚本正常工作，应将摄像头以大约
# 45度的角度对准线条。请确保只有线条位于
# 摄像头的视野范围内。

import sensor
import time
import math

# Tracks a black line. Use [(128, 255)] for a tracking a white line.
GRAYSCALE_THRESHOLD = [(0, 64)]

# 每个感兴趣区域(ROI)表示为(x, y, w, h)。线条检测算法将尝试找到
# 每个ROI内最大斑块的质心。随后，这些质心的x位置
# 会以不同权重进行平均计算，其中最大的权重分配给
# 靠近图像底部的ROI，次之的权重给下一个ROI，依此类推。
ROIS = [  # [ROI, 权重]
    (0, 100, 160, 20, 0.7),  # 你需要根据应用调整这些权重。
    (0, 50, 160, 20, 0.3),  # 根据你的机器人设置方式。
    (0, 0, 160, 20, 0.1),
]

# 计算权重除数（我们预先计算，这样你就不必手动调整权重总和为1）。
weight_sum = 0
for r in ROIS:
    weight_sum += r[4]  # r[4]代表ROI权重。

# 摄像头配置中...
sensor.reset()  # 初始化相机传感器。
sensor.set_pixformat(sensor.GRAYSCALE)  # 使用灰度模式。
sensor.set_framesize(sensor.QQVGA)  # 采用QQVGA分辨率以提升速度。
sensor.skip_frames(time=2000)  # 让新设置生效。
sensor.set_auto_gain(False)  # 必须关闭以进行颜色跟踪
sensor.set_auto_whitebal(False)  # 必须关闭以进行颜色跟踪
clock = time.clock()  # 跟踪FPS。

while True:
    clock.tick()  # Track elapsed milliseconds between snapshots().
    img = sensor.snapshot()  # 拍照并返回图像。

    centroid_sum = 0

    for r in ROIS:
        blobs = img.find_blobs(
            GRAYSCALE_THRESHOLD, roi=r[0:4], merge=True
        )  # r[0:4]为ROI区域元组。

        if blobs:
            # 寻找像素最多的色块。
            largest_blob = max(blobs, key=lambda b: b.pixels())

            # 在色块周围绘制矩形框。
            img.draw_rectangle(largest_blob.rect())
            img.draw_cross(largest_blob.cx(), largest_blob.cy())

            centroid_sum += largest_blob.cx() * r[4]  # r[4]代表ROI权重。

    center_pos = centroid_sum / weight_sum  # 确定线的中心。

    # 将center_pos转换为偏转角度。我们使用非线性
    # 操作，使得响应在我们偏离线越远时
    # 越强。非线性操作非常适合用于此类算法的输出
    # 以引发“触发”响应。
    deflection_angle = 0

    # 80来自X分辨率的一半，60来自Y分辨率的一半。
    # 下面的方程只是计算一个三角形的角度，其中
    # 三角形的对边是中心位置与中心的偏差，
    # 邻边是Y分辨率的一半。这将角度输出限制在
    # 大约-45到45之间。（并不完全是-45和45）。
    deflection_angle = -math.atan((center_pos - 80) / 60)

    # 将弧度转换为角度。
    deflection_angle = math.degrees(deflection_angle)

    # 现在你有了一个角度，告诉你需要转动机器人多少度
    # 包含离机器人最近的线部分以及离机器人较远的
    # 线部分，以获得更好的预测。
    print("Turn Angle: %f" % deflection_angle)

    print(clock.fps())  # 注意：您的 OpenMV Cam 在连接到
    # 计算机时运行速度会减半。断开连接后，FPS 应该会增加。