2023人形全能赛openmv巡线代码

• openmv

import sensor, image, time, math
from pyb import LED, millis, UART

class RobotControl:
    flag = 0  # 代表是否使用更新的区域来进行下一次检测

    def __init__(self):
        # 获取三种颜色的灯
        self.red_led = LED(1)
        self.green_led = LED(2)
        self.blue_led = LED(3)
        # 图像翻转
        sensor.reset()
        #sensor.set_vflip(True)
        #sensor.set_hmirror(True)
        sensor.set_pixformat(sensor.RGB565)  # 读入彩色图片
        sensor.set_framesize(sensor.QQVGA)  # 160×120
        sensor.skip_frames(time=2000)  # 跳过2000张图片
        # 线的阈值
        self.THRESHOLD = (29, 0, 61, -86, -69, 55)  # 黑色
        self.red_threshold = (0, 100, 19, -62, -81, 61)
        # 通信初始化
        self.uart = UART(3, 19200)  # 注意波特率
        # 计算差值
        self.middle = 0
        # 每次检测区域 （6块）  (x, y, w, h)
        self.init_roi = [(5, 50, 70, 10), (85, 50, 70, 10),
                         (5, 70, 70, 10), (85, 70, 70, 10),
                         (5, 90, 70, 10), (85, 90, 70, 10), ]
        self.roi = []
        self.py = 5  # 找的正方形的竖直偏差
        self.img = None  # 每次的图片存在这，方便操作
        self.count_turn_left = 0
        self.is_cross_barr = 0
    # 关灯
    def close_rgb(self):
        self.red_led.off()
        self.green_led.off()
        self.blue_led.off()

    # 开白灯
    def while_rgb(self):
        self.close_rgb()
        self.red_led.on()
        self.green_led.on()
        self.blue_led.on()

    # 开红灯
    def red_rgb(self):
        self.close_rgb()
        self.red_led.on()

    # 开绿灯
    def green_rgb(self):
        self.close_rgb()
        self.green_led.on()

    # 开蓝灯
    def blue_rgb(self):
        self.close_rgb()
        self.blue_led.on()

    # 数据发送
    def send_to_arduino(self, mes):
        print("send_message:", mes)  # 调试用
        #time.sleep(0.1)
        send_str = "{" + str(mes) + "&}"
        self.uart.write(send_str)

    # 数据接收
    def recv_from_arduino(self):
        recv = self.uart.read()
        if recv:
            return recv
        else:
            return None

    # 获取边界点（6个）
    def get_dot(self, roi):
        most_pixels = 0
        max_x = 0
        if self.img:
            blob0 = self.img.find_blobs([self.THRESHOLD], roi=roi, pixels_threshold=2, area_threshold=2, merge=True)
            if blob0:
                for n in range(len(blob0)):
                    if blob0[n].pixels() > most_pixels:
                        most_pixels = blob0[n].pixels()
                        max_x = n
                # 将矩形标出
                self.img.draw_rectangle(blob0[max_x].x(), blob0[max_x].y(), blob0[max_x].w(), blob0[max_x].h(),
                                        color=(0, 0, 255), thickness=1, fill=False)
                # 将矩形中点标出
                self.img.draw_cross(blob0[max_x].x() + int(blob0[max_x].w() / 2), blob0[max_x].y() + int(
                    blob0[max_x].h() / 2), color=(0, 0, 255), size=1, thickness=1)
                return blob0[max_x].x() + int(blob0[max_x].w() / 2), \
                       blob0[max_x].y() + int(blob0[max_x].h() / 2)  # 矩形中心
            else:
                return 0
        else:
            return 0

    def get_dx(self):
        left_dot_dic = {}
        right_dot_dic = {}
        middle_dot = []
        # 检测区域动态更新
        if not RobotControl.flag:
            RobotControl.flag = 1
            roi = self.init_roi
        else:
            roi = self.roi
        roi = self.init_roi
        # 获取边界坐标
        for i in range(6):
            roii = roi[i]
            dot = self.get_dot(roii)
            if dot:
                dot = list(dot)
                if 50 <= dot[1] <= 59:
                    dot[1] = 55
                if 70 <= dot[1] <= 79:
                    dot[1] = 75
                if 90 <= dot[1] <= 99:
                    dot[1] = 95
                if i % 2 == 0:
                    left_dot_dic[dot[1]] = dot[0]
                else:
                    right_dot_dic[dot[1]] = dot[0]

        # 获取中点坐标
        for y1, x1 in left_dot_dic.items():
            if right_dot_dic.get(y1, -1) != -1:
                x2 = right_dot_dic.get(y1, -1)
                middle_dot.append(((x1 + x2) // 2, y1))
                # 将路中点标出
                self.img.draw_cross((x1 + x2) // 2, y1, color=(0, 255, 0), size=1, thickness=1)

        # 中点排个序
        middle_dot = sorted(middle_dot, key=lambda x: x[1], reverse=True)

        # 清空变动的坐标列表
        self.roi = []

        # 若三个中点都找到
        if len(middle_dot) == 3:
            numy = 3
            # 更新下一个区域
            for y1, x1 in left_dot_dic.items():
                x2 = right_dot_dic.get(y1, -1)
                if x1 >= 5:
                    x1 -= 5
                else:
                    x1 = 0
                if x2 >= 160 - 70 - 1:
                    x2 = 89
                self.roi.append((x1, y1 - self.py, 60, 10))
                self.roi.append((x2, y1 - self.py, 60, 10))
            # 获取dx middle_x并返回
            dx = middle_dot[0][0] - middle_dot[2][0]
            middle_x = (middle_dot[0][0] + middle_dot[1][0] + middle_dot[2][0]) // 3

        elif len(middle_dot) == 2:
            numy = 2
            RobotControl.flag = 0

            dx = middle_dot[0][0] - middle_dot[1][0]
            middle_x = (middle_dot[0][0] + middle_dot[1][0]) // 2

        elif len(middle_dot) == 1:
            numy = 1
            RobotControl.flag = 0
            dx = 0
            try:
                middle_x = middle_dot[0][0]
            except:
                middle_x = 80  # 如果没找到中线的话返回中线值即可
            if len(right_dot_dic) >= len(left_dot_dic):
                num_list = right_dot_dic.values()
                num_list = sorted(num_list, reverse=True)  # 降序
            else:
                num_list = left_dot_dic.values()
                num_list = sorted(num_list, reverse=True)  # 降序
            if len(num_list) == 1:
                    dx = 0
            else:
                dx = num_list[0] - num_list[-1]

        # 没有找到一个中点的情况，需要进行更多的判断
        else:
            RobotControl.flag = 0
            numy = 0
            middle_x = 0
            if not left_dot_dic and not right_dot_dic:
                dx = -10  # 一个点都看到
            elif not left_dot_dic:  # 没有左边的边界
                num_list = right_dot_dic.values()
                num_list = sorted(num_list, reverse=True)  # 降序
                if len(num_list) == 1:
                    dx = -1  # 只有右边边界的一个点
                else:
                    dx = -6  # 右边边界有两个及以上个点
                    print(num_list)
                    middle_x = num_list[0] - num_list[-1]
            elif not right_dot_dic:  # 没有右边的点
                num_list = left_dot_dic.values()
                num_list = sorted(num_list, reverse=True)  # 降序
                if len(num_list) == 1:
                    dx = -2  # 只有左边的一个点
                else:
                    dx = -7  # 左边边界有两个及以上个点
                    middle_x = num_list[0] - num_list[-1]
            elif len(right_dot_dic) == 2 and len(left_dot_dic) == 1:
                dx = -3  # 右边两个点，左边一个点
            elif len(right_dot_dic) == 1 and len(left_dot_dic) == 2:
                print("right_dot_dic:", right_dot_dic)
                print("left_dot_dic:", left_dot_dic)
                dx = -4  # 左边两个点，右边一个点
            else:
                print("right_dot_dic:", right_dot_dic)
                print("left_dot_dic:", left_dot_dic)
                dx = -5

        return dx, middle_x, numy

    def get_distance(self):
        blobs = self.img.find_blobs([self.red_threshold], invert=True)
        max_pix = 0
        count = -1
        if blobs:
            local_i = -1
            for ele in blobs:
                count += 1
                if ele[4] > max_pix:
                    max_pix = ele[4]
                    local_i = count
            if local_i >= 0:
                b = blobs[local_i]
                self.img.draw_rectangle(b[0:4])  # 画正方形
                self.img.draw_cross(b[5], b[6])  # 画十字
                cx = b.cx()
                cy = b.cy()  # 获取像素大小
                pixels = b.pixels()
                rotation = math.degrees(b.rotation()) - 90
                return cx, cy, pixels, rotation
        return 0, 0, 0, 0

    def send_next_move_order(self):
        # define GO_STRAIGHT                 2       // 直行
        # define GO_LITTLE_STRAIGHT          3        // 小直行
        # define GO_MICRO_STRAIGHT           4        // 小小直行
        # define GO_RIGHT                    5        // 右转
        # define GO_LITTLE_RIGHT             6        // 小右转
        # define GO_LEFT                     7        // 左转
        # define GO_LITTLE_LEFT              8        // 小左转
        # define GO_LEFT_TRANSVERSE          9        // 原地左胯一步
        # define GO_RIGHT_TRANSVERSE         10       // 原地右胯一步
        # define GO_OBSTACLE                 11       // 越障
        self.img = sensor.snapshot()
        dx, middle_x, numy = self.get_dx()  # dx 中点偏差  middle_x 中点的中间值（算出来的）  numy 中间点的个数
        print("dx, middle_x, numy:", dx, middle_x, numy)
        # 若有三个中点， 则只用dx斜率判断
        if numy == 3:
            # 直行
            if -1 <= dx <= 4:
                self.send_to_arduino(2)       # 直行
            elif dx <= -2:
                # 在赛道偏外侧，还是能直行
                if middle_x <= 80:
                    self.send_to_arduino(2)  # 直行
                else:
                    self.send_to_arduino(5)  # 右转
            elif dx >= 10:
                if middle_x <= 70:
                    self.send_to_arduino(8)  # 小左转
                else:
                    self.send_to_arduino(2)  # 直走
            elif 5 <= dx <= 10:
                if middle_x >= 90:
                    self.send_to_arduino(5)  # 右转
                elif 82 <= middle_x <= 89:
                    self.send_to_arduino(2)  # 直行
                else:
                    self.send_to_arduino(7)  # 左转
        elif numy == 2:
            if middle_x >= 85:
                self.send_to_arduino(2)     # 直行
            else:
                self.send_to_arduino(8)     # 小左转
        elif numy == 1:
            if dx >= 10:
                if 86<=middle_x<=100:
                    self.send_to_arduino(5)   # 右转
                else:
                    self.send_to_arduino(7)   # 左转
            elif dx >= 5:
                if middle_x>=90:
                    self.send_to_arduino(2)   # 直行
                else:
                    self.send_to_arduino(8)   # 小左转
            else:
                self.send_to_arduino(2)       # 直行
            #if middle_x <= 85:
                #self.send_to_arduino(8)
            #elif middle_x >= 105:
                #self.send_to_arduino(5)
            #else:
                #self.send_to_arduino(2)
        else:  # 没有扫到正常的中点 dx有 -1 -2 -3 -4 -5 -6 -7 -10
            if dx == -1:  # 只有右边一个点
                self.send_to_arduino(9)  # 左胯一步
            elif dx == -2:  # 只有左边的一个点
                self.send_to_arduino(10)  # 右胯一步
            elif dx == -3:  # 右边两个点，左边一个点
                self.send_to_arduino(8)  # 小左
            elif dx == -4:  # 左边两个点，右边一个点
                self.send_to_arduino(5)  # 右转
            elif dx == -6:  # 右边边界有两个及以上个点
                if middle_x >= 20:
                    self.send_to_arduino(7)  # 左转
                elif middle_x >= 15:
                    self.send_to_arduino(8)  # 小左转
                else:
                    self.send_to_arduino(9)  # 原地左胯一步
                    #self.send_to_arduino(10)  # 原地右胯一步
                    #self.send_to_arduino(5)  # 原地右转
            elif dx == -7:
                if 5<=middle_x <= 20:
                    self.send_to_arduino(5)  # 右转
                elif middle_x >= 21:
                    self.send_to_arduino(7)  # 左转
                else:
                    self.send_to_arduino(2)  # 直行
            elif dx == -5:
                self.send_to_arduino(7)   # 左转
            elif dx == -10:
                self.send_to_arduino(8)

    def find_line_and_red(self):
        # define GO_STRAIGHT                 2       // 直行
        # define GO_LITTLE_STRAIGHT          3        // 小直行
        # define GO_MICRO_STRAIGHT           4        // 小小直行
        # define GO_RIGHT                    5        // 右转
        # define GO_LITTLE_RIGHT             6        // 小右转
        # define GO_LEFT                     7        // 左转
        # define GO_LITTLE_LEFT              8        // 小左转
        # define GO_LEFT_TRANSVERSE          9        // 原地左胯一步
        # define GO_RIGHT_TRANSVERSE         10       // 原地右胯一步
        # define GO_OBSTACLE                 11       // 越障
        #                                    20       // 小小小直行
        #                                    18       // 原地右转
        #                                    19       // 原地左转
        # 99 小小直行+越障
        # 98 左转+右胯
        self.img = sensor.snapshot()
        cx, cy, pixels, rotation = self.get_distance()
        if pixels <= 450 and self.is_cross_barr:
            if self.is_cross_barr:
                if cx>130:
                    self.is_cross_barr = 0
                    self.send_to_arduino(97)
                    return
            self.send_next_move_order()
        else:
            self.is_cross_barr = 1
            # 先判断距离，再判断朝向，最后判断左右距离
            if cy <= 75:
                self.send_to_arduino(2)
            elif cy <= 95:
                self.send_to_arduino(20)
            else:
                if rotation >= 0 or rotation <= -85:
                    self.count_turn_left += 1
                    if self.count_turn_left == 2:
                        self.send_to_arduino(98)
                    else:
                        self.send_to_arduino(19)

                elif -79 <= rotation <= 0:
                    self.send_to_arduino(18)
                else:
                    if 15 <= cx <= 75:
                        self.send_to_arduino(99)
                    elif cx > 76:
                        self.send_to_arduino(10)
                    else:
                        self.send_to_arduino(9)
    # 运行入口
    def run(self):
        self.while_rgb()
        while 1:
            order = self.recv_from_arduino()
            # self.img = sensor.snapshot()
            # print(self.get_distance())
            sensor.snapshot()
            #self.send_next_move_order()
            if order:
                order = order.decode("utf-8")
                print("接受消息：", order)
                if order == "g":
                    self.send_next_move_order()
                if order == "r":
                    self.find_line_and_red()

if __name__ == '__main__':
    robot = RobotControl()
    robot.run()