# vision distance - By: SingTown - Wed Jul 30 2025

import sensor
import time


# 设置VGA画面，并裁剪中间的画面
sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_framesize(sensor.VGA)
sensor.set_windowing((200,240))
sensor.skip_frames(time=2000)
clock = time.clock() # Tracks FPS.

CENTER_X = 200/2
CENTER_Y = 240//2

# 边框的真实长度，单位mm
FRAME_WIDTH_MM = 170
FRAME_HIGHT_MM = 245

# 校准的数据
# 例如： 当距离是1400mm时，边框宽度为82像素

# DISTANCE_MM_1 = 1100
# FRAME_WIDTH_PIXEL_1 = 104

DISTANCE_MM_2 = 1520
FRAME_WIDTH_PIXEL_2 = 69
FRAME_HEIGHT_PIXEL_2 = 100·
# DISTANCE_MM_3 = 1800
# FRAME_WIDTH_PIXEL_3 = 64

def find_center_min_blob(blobs):
    # 找中间最小的色块
    blob = None
    min_area = 100000
    for b in blobs:
        if abs(b.cx()-CENTER_X)+ abs(b.cy()-CENTER_Y) > 50:
            continue
        if b.area() > min_area:
            continue
        blob = b
        min_area = b.area()
    return blob

def find_center_max_blob(blobs):
    # 找中间最大的色块
    blob = None
    max_area = 0
    for b in blobs:
        if abs(b.cx()-CENTER_X)+ abs(b.cy()-CENTER_Y) > 50:
            continue
        if b.area() < max_area:
            continue
        blob = b
        max_area = b.area()
    return blob

while True:
    clock.tick()
    img = sensor.snapshot()

    # 找白色色块，黑色边框内部
    frames = img.find_blobs([(150,256)])
    frame_blob = find_center_min_blob(frames)
    if not frame_blob:
        print("NO FRAME")
        continue

    # 计算距离
    distance = DISTANCE_MM_2*FRAME_HEIGHT_PIXEL_2/frame_blob.h()

    # 缩小roi，避免黑框的黑边
    frame_roi = (frame_blob.x()+5, frame_blob.y()+5, frame_blob.w()-10, frame_blob.h()-10)
    if frame_roi[2] <= 0 or frame_roi[3] <= 0:
        print("ROI ERROR")
        continue
    print(frame_roi)

    # 找黑色色块，目标物体
    objs = img.find_blobs([(0,150)],roi=frame_roi)
    obj_blob = find_center_max_blob(objs)
    if not obj_blob:
        print("NO OBJS")
        continue

    # 计算物体边长
    obj_w_mm = obj_blob.w()/frame_blob.w()*FRAME_WIDTH_MM

    print(frame_blob.w(), frame_blob.h())
    print(obj_blob.density())
    if 0.9 < obj_blob.density() :
        print("矩形")
    elif 0.6 < obj_blob.density():
        print("圆形")
    elif 0.4 < obj_blob.density():
        print("三角形形")
    else:
        print("无法识别到形状")

    img.draw_string(10,10, "length:"+str(obj_w_mm)+"mm")
    img.draw_string(10,20, "distance:"+str(distance)+"mm")
    img.draw_rectangle(frame_blob.rect())
    img.draw_rectangle(obj_blob.rect())

import time
from machine import UART

from pyb import LED

#from pyb import UART

OpenMV4 H7 Plus, OpenMV4 H7, OpenMV3 M7, OpenMV2 M4 的UART(3)是P4-TX P5-RX

uart = UART(3,115200) #OpenMV RT 注释掉这一行，用下一行UART(1)
#uart = UART(1, 19200) #OpenMV RT 用UART(1)这行，注释掉上一行UART(3)

OpenMV RT 只有串口UART(1)，对应P4-TX P5-RX; OpenMV4 H7 Plus, OpenMV4 H7, OpenMV3 M7 的UART(1)是P0-RX P1-TX

red_led = LED(1)
green_led = LED(2)
res = 0

while(True):
uart.write('3')
time.sleep_ms(1000)
if uart.any():
res = uart.read()
k= int(res)
if k >= 1:
red_led.on()
time.sleep_ms(1000)
red_led.off()
else:
green_led.on()
time.sleep_ms(1000)
green_led.off()
# green_led.on()
# time.sleep_ms(5000)
# green_led.off()