@zgc6 注：在脱机时可以正常向单片机发信息

# Untitled - By: 21980 - Mon Jul 14 2025

import sensor, image, time, pyb, math
from pyb import UART, Pin, LED, Servo


uart = UART(3, 115200)      #用于接收飞控的信息

black_threshold = (1, 8, -6, 3, -4, 3)    #杆子的颜色阈值，需实测
yellow_threshold = (39, 90, -13, 4, 16, 69)    #线上黄色物体的颜色阈值，需实测

K1 = 700
K2 = 1100       #用于测距用的比例系数（K=物体离镜头实际距离*物体在镜头里直径像素数），需实测

#面积阈值
MIN_PIX = 500
MAX_PIX = 1000
#x坐标相差阈值
x_diff = 20

#上下五等分屏幕
ROIS = {
    'up1': (0, 0, sensor.width(), sensor.height() // 5),
    'up2': (0, sensor.height() // 5, sensor.width(), sensor.height() // 5),
    'mid': (0, sensor.height() // 5 * 2, sensor.width(), sensor.height() // 5),
    'down1': (0, sensor.height() // 5 * 3, sensor.width(), sensor.height() // 5),
    'down2': (0, sensor.height() // 5 * 4, sensor.width(), sensor.height() // 5)
}

led1 = LED(1)       #红绿蓝三种工作状态灯
led2 = LED(2)
led3 = LED(3)

photo_taken1 = 0     #条码的拍照次数限制，一共只能拍三次，拍完变为1
photo_taken2 = 0

Servo(1).calibration(500,2500,500)  #初始化舵机
Servo(2).calibration(500,2500,500)
Servo(1).angle(90)  #固定云台角度
Servo(2).angle(90)

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time=2000)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)


clock = time.clock()

# 用于处理接收到的接收串口数据——接收到的串口数据总共为2位：0：帧头0xAA，1：工作模式兼帧尾
class Receive(object):
    def __init__(self,mode,uart):       #就是构造函数（初始化），__xx__表示特殊变量
        self.uart_buf  = []
        self.state = 0
        self.uart = uart
        self.WorkMode = mode
    #读取串口缓存
    def UartReadBuffer(self):
        i = 0
        Buffer_size = self.uart.any()
        while i < Buffer_size:
            self.ReceivePrepare(self.uart.readchar())
            i = i + 1
    #串口通信协议接收
    def ReceivePrepare(self,data):
        if self.state==0:       #应该是指未读入
            if data == 0xAA:#帧头
                self.uart_buf.append(data)
                self.state = 1
            else:
                self.state = 0
        elif self.state==1:     #读入
            if data == 0x1A or data == 0x1B or data == 0x2A or data == 0x2B or data == 0x3A or data == 0x3B or data == 0x4A or data == 0x4B:    #工作模式，兼为帧尾（0x1x为找杆子，0x2x为找黄色异物，0x3x为扫条形码，0x4x为扫二维码）
                self.uart_buf.append(data)
                self.state = 0
                self.WorkMode = self.uart_buf[1]        #规定工作模式为第1位（第0位起始）
                self.uart_buf = []#清空缓冲区，准备下次接收数据
            else:
                self.state = 0
        else:
            self.state = 0
    #串口数据解析
    def ReceiveAnl(self):
        self.WorkMode = self.uart_buf[1]

Receive_ctrl = Receive(1,uart)

class target_check(object):
    x = 0
    y = 0
    flag = 0
    distance = 0
    num = 0

target = target_check()

#滤出最大的色块
def find_max(blobs):
    max_blob = None
    max_size = 0
    for blob in blobs:
        if blob.pixels() > max_size:
            max_blob = blob
            max_size = blob.pixels()
    return max_blob


#找杆子
'''
def find_pole():
    img = sensor.snapshot()
    blobs = img.find_blobs([black_threshold])
    max_blob = find_max(blobs)
    if max_blob:
        target.x = max_blob.cx() - sensor.width() // 2       #x偏移量
        #target.y = max_blob.cy() - sensor.height() // 2      #y偏移量
        target.flag = 1
        Lm = max_blob[2]
        target.distance = int(K / Lm)       #测距
        img.draw_rectangle(max_blob.rect())
        img.draw_cross(max_blob.cx(), max_blob.cy())
        img.draw_string(10, 10, str(target.distance), scale = 5, color = (255, 0, 0))
    else:
        target.flag = 0
'''
def find_pole():
    img = sensor.snapshot()
    blobs = []
    #将所有roi中找到的小色块放进一个列表
    for roi_direct in ROIS.keys():
        blobs.extend(img.find_blobs([black_threshold], roi = ROIS[roi_direct], area_threshold = MIN_PIX, merge = True, margin = 1))
    if len(blobs):
        #先面积初筛
        for blob in blobs:
            if blob.pixels() > MAX_PIX:
                del blob
            else:
                img.draw_rectangle(blob.rect())
                img.draw_cross(blob.cx(), blob.cy())

        #根据blob的y坐标大小，从小到大冒泡排序
        for i in range(len(blobs)):
            swapped = 0
            for j in range(i):
                #把y大的色块放在后面
                if blobs[j].cy() > blobs[j + 1].cy():
                    blobs[j + 1], blobs[j] = blobs[j], blobs[j + 1]
                    swapped = 1
            if not swapped:
                break

        #剩余面积大小相近的，比较x坐标有没有相差过大（只要底部没有误识别的色块，就基本准确）
        filted_blobs = []           #滤波后的色块
        last_x = blobs[0].cx()      #如果这个是正确的，就没有大问题
        for blob in blobs:
            if abs(blob.cx() - last_x) <= x_diff:
                filted_blobs.append(blob)
                last_x = blob.cx()  #正确的坐标记为上次坐标
                img.draw_rectangle(blob.rect(), color = (255, 0, 0))
                img.draw_cross(blob.cx(), blob.cy(), color = (255, 0, 0))
        sum_x = 0
        if filted_blobs:
            target.flag = 1
            for c in filted_blobs:
                sum_x = sum_x + c.cx()
            avg_x = sum_x // len(filted_blobs)      #x坐标
            target.x = avg_x - sensor.width() // 2  #x偏移量
            Lm = max(b.w() for b in filted_blobs)   #宽
            target.distance = K1 // Lm                           #测距
            img.draw_string(10, 10, str(target.distance), scale = 5, color = (255, 0, 0))
            img.draw_string(10, sensor.height() - 50, str(target.x), scale = 5, color = (0, 255, 0))
        else:
            target.flag = 0
    else:
        target.flag = 0
#找黄色异物
def find_yellow_rect():
    img = sensor.snapshot()
    blobs = img.find_blobs([yellow_threshold], merge = True)
    if blobs:
        x_min = min([b.x() for b in blobs])
        y_min = min([b.y() for b in blobs])
        x_max = max([b.x() + b.w() for b in blobs])
        y_max = max([b.y() + b.h() for b in blobs])
        img.draw_rectangle([x_min, y_min, x_max - x_min, y_max - y_min], color=(0, 255, 0))
        img.draw_cross((x_min + x_max) // 2, (y_min + y_max) // 2, color=(255, 0, 0))
        target.x = (x_min + x_max) // 2 - sensor.width() // 2
        target.y = (y_min + y_max) // 2 - sensor.height() // 2
        target.distance = K2 // (x_max - x_min)
        img.draw_string(10, 10, str(target.distance), scale = 5, color = (255, 0, 0))
        img.draw_string(10, sensor.height() - 50, str(target.x), scale = 5, color = (0, 255, 0))
        target.flag = 1
    else:
        target.flag = 0

#拍照存在SD卡
def take_photo(name, photo_count, img):
    #led1.on()       #亮红灯，准备拍照
    #sensor.skip_frames(time = 1000) #停顿1s准备
    #led1.off()
    #led3.on()       #转蓝灯，拍照
    print(f"Taking photo {photo_count}……")
    img.save(name + f"{photo_count}.jpg")
    #led3.off()      #灭灯，拍照1次
    print("Done!")

#找条形码（拍照+识别）
def find_barcode():
    global photo_taken1
    #临时更改为高分辨率，不然识别不出
    sensor.set_framesize(sensor.FHD)
    sensor.set_windowing((500, 150))
    img = sensor.snapshot()
    target_code = None
    codes = img.find_barcodes()
    for code in codes:
        if code:
            target_code = code
            break
    #找到之后拍照
    if target_code:
        img.draw_string(10, 10, "B", scale = 2, color = (255, 0, 0))
        if not photo_taken1:                 #如果没拍过照就拍照，拍过照就不拍
            for photo_count in range(1, 4):
                take_photo("BP", photo_count, img)
            photo_taken1 = 1
        target.flag = 1
        target.num = int(target_code.payload())
    else:
        target.flag = 0
        target.num = 0

#找二维码（这里只需拍照，无需识别）
def find_qrcode():
    global photo_taken2
    sensor.set_framesize(sensor.HD)
    sensor.set_windowing((400, 300))
    img = sensor.snapshot()
    target_code = None
    codes = img.find_qrcodes()
    for code in codes:
        if code:
            target_code = code
            break
    #找到之后拍照
    if target_code:
        img.draw_string(10, 10, "Q", scale = 5, color = (255, 0, 0))
        if not photo_taken2:
            for photo_num in range(1, 4):
                take_photo("QP", photo_num, img)
            photo_taken2 = 1
        target.flag = 1
    else:
        target.flag = 0

while True:
    clock.tick()
    led3.on()               #脱机时，会一直停在此状态，无法接收到单片机发送的0x1A，但连接IDE时可以
    #img = sensor.snapshot()
    data = bytearray()
    Receive_ctrl.UartReadBuffer()
    print(Receive_ctrl.WorkMode)
    #Receive_ctrl.WorkMode = 0x1A
    if Receive_ctrl.WorkMode == 0x1A:       #找杆子
        led3.off()
        led1.on()           #进入工作状态，红灯
        find_pole()
        if target.flag:     #找到了就转绿灯
            led1.off()
            led2.on()
        data = bytearray([0xAA, 0x10, target.flag, target.x // 256, target.x % 256, target.distance // 256, target.distance % 256, 0xFF])
        print("find pole: ", data)
        #print(target.flag, target.x, target.distance)
    elif Receive_ctrl.WorkMode == 0x1B:     #停止找杆子
        data = bytearray([0xAA, 0x10, 0, 0, 0, 0, 0, 0xFF])
        print("stop finding pole: ", data)
    elif Receive_ctrl.WorkMode == 0x2A:     #找黄色异物
        find_yellow_rect()
        data = bytearray([0xAA, 0x20, target.flag, target.x // 256, target.x % 256, target.y // 256, target.y % 256, target.distance // 256, target.distance % 256, 0xFF])
        print("find yellow rect: ", data)
    elif Receive_ctrl.WorkMode == 0x2B:     #停止找黄色异物
        sensor.set_framesize(sensor.QVGA)   #扫码之后要重置分辨率
        data = bytearray([0xAA, 0x20, 0, 0, 0, 0, 0, 0, 0, 0xFF])
        print("stop finding yellow rect: ", data)
    elif Receive_ctrl.WorkMode == 0x3A:     #扫条形码
        find_barcode()
        data = bytearray([0xAA, 0x30, target.flag, target.num // 256, target.num % 256, 0xFF])
        print("find barcode: ", data)
    elif Receive_ctrl.WorkMode == 0x3B:     #停止扫条形码
        sensor.set_framesize(sensor.QVGA)
        data = bytearray([0xAA, 0x30, 0, 0, 0, 0xFF])
        print("stop finding barcode: ", data)
    elif Receive_ctrl.WorkMode == 0x4A:     #扫二维码
        find_qrcode()
        data = bytearray([0xAA, 0x40, target.flag, 0xFF])
        print("find qrcode: ", data)
    elif Receive_ctrl.WorkMode == 0x4B:     #停止扫二维码
        sensor.set_framesize(sensor.QVGA)   #扫码之后要重置分辨率
        data = bytearray([0xAA, 0x40, 0, 0xFF])
        print("stop finding qrcode: ", data)
    else:                                   #未工作
        data = bytearray([0xAA, 0x00, 0xFF])
        print("not working: ", data)
    #data = bytearray([0xAA, 0x10, 1, 0, 0, 0, 0, 0xFF])        #脱机向单片机发送此数据，能够正常接收
    uart.write(data)
    print(clock.fps())