人形机器人全能赛openmv巡线代码

人形机器人全能赛openmv巡线代码

import sensor, image, time
from pyb import LED, millis, UART
from math import pi, isnan
import lcd

# PID类
class PID:
    _kp = _ki = _kd = _integrator = _imax = 0
    _last_error = _last_derivative = _last_t = 0
    _RC = 1 / (2 * pi * 20)

    def __init__(self, p=0, i=0, d=0, imax=0):
        self._kp = float(p)
        self._ki = float(i)
        self._kd = float(d)
        self._imax = abs(imax)
        self._last_derivative = float('nan')

    def get_pid(self, error, scaler):
        tnow = millis()
        dt = tnow - self._last_t
        output = 0
        if self._last_t == 0 or dt > 1000:
            dt = 0
            self.reset_I()
        self._last_t = tnow
        delta_time = float(dt) / float(1000)
        output += error * self._kp
        if abs(self._kd) > 0 and dt > 0:
            if isnan(self._last_derivative):
                derivative = 0
                self._last_derivative = 0
            else:
                derivative = (error - self._last_error) / delta_time
            derivative = self._last_derivative + \
                         ((delta_time / (self._RC + delta_time)) * \
                          (derivative - self._last_derivative))
            self._last_error = error
            self._last_derivative = derivative
            output += self._kd * derivative
        output *= scaler
        if abs(self._ki) > 0 and dt > 0:
            self._integrator += (error * self._ki) * scaler * delta_time
            if self._integrator < -self._imax:
                self._integrator = -self._imax
            elif self._integrator > self._imax:
                self._integrator = self._imax
            output += self._integrator
        return output

    def reset_I(self):
        self._integrator = 0
        self._last_derivative = float('nan')

class RobotControl:
    flag = 0

    def __init__(self):
        # 设置pid的初始值
        self.rho_pid = PID(p=0.4, i=0)
        self.theta_pid = PID(p=0.001, i=0)
        lcd.init()
        # 获取三种颜色的灯
        self.red_led = LED(1)
        self.green_led = LED(2)
        self.blue_led = LED(3)
        # 图像翻转

        #sensor.set_pixformat(sensor.GRAYSCALE)  # 读入灰色图片
        sensor.reset()
        sensor.set_vflip(True)
        sensor.set_hmirror(True)
        sensor.set_pixformat(sensor.RGB565)     # 读入彩色图片
        sensor.set_framesize(sensor.QQVGA)  # 160x120 设置图像大小
        #sensor.set_vflip(True)  #纵向旋转
        #sensor.set_hmirror(True)  # 水平旋转
        #sensor.set_framesize(sensor.CIF)  # 320×240
        # sensor.set_framesize(sensor.HQQQVGA)  # 80x40 设置图像大小
        sensor.skip_frames(time=2000)  # 跳过2000张图片
        # 线的阈值
        #self.THRESHOLD = (35, 100, -47, 75, -28, 87)  # 黑色
        self.THRESHOLD = (29, 0, 61, -86, -69, 55)    # 黑色0
        #self.THRESHOLD = (2, 34, 61, -83, 80, -71)    # 灰度的
        #黑色识别
        #self.THRESHOLD = (43, 100, -48, 127, -54, 127) #黑色
        #self.THRESHOLD = (88,255)
        # 通信初始化
        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.line_to_middle = 10  # 正常情况下边界到中线的距离  要调的
        self.img = 0  # 每次的图片存在这，方便操作
        self.sleeptime = 0
        self.clocktime = 0
        self.blinblin = 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_message(self, mes):
        """
        :param mes: 0 小左  1 左  2 右  3 直走
        :param sleeptime:   执行该动作需要睡眠的时间
        :return:
        """
        # 维护一个睡眠池
        sleep_dic = {
            0: 3950,
            1: 3950,
            2: 3950,
            3: 3950,

        }

        self.uart.write(str(mes) + "\n")
        self.blinblin += 1
        #print(self.blinblin)
        if self.blinblin%2 == 1:
            self.green_rgb()   # 闪了绿灯代表发送消息了
        else:
            self.blue_rgb()
        print(mes)
        self.sleeptime = sleep_dic.get(mes, 1000)
        #print(self.sleeptime)
        #time.sleep_ms(sleep_dic.get(mes, 1000))

    def recv_message(self):
        pass

    # 获取偏差（可能用不上）
    def get_output(self, line, img):
        output = 0
        if (line):
            # print(line)
            # 中心位置的差值
            rho_err = abs(line.rho()) - img.width() / 2
            # 计算直线的角度
            if line.theta() > 90:
                theta_err = line.theta() - 180
            else:
                theta_err = line.theta()
            # 把线画出来
            img.draw_line(line.line(), color=127)
            print(rho_err, line.magnitude(), theta_err)
            if line.magnitude() > 8:  # 表示线性回归的效果怎么样
                rho_output = self.rho_pid.get_pid(rho_err, 1)
                theta_output = self.theta_pid.get_pid(theta_err, 1)
                output = rho_output + theta_output
                # 缺运动控制
        return output

    # 获取边界点（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
        # 获取边界坐标
        for i in range(6):
            roii = roi[i]
            dot = self.get_dot(roii)
            #print(dot)
            if dot:
                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_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   # 如果没找到中线的话返回中线值即可
        else:
            RobotControl.flag = 0
            numy = 0
            middle_x = 0
            if not left_dot_dic:
                dx = -1
            else:
                dx = -2

        return dx, middle_x, numy

    def run(self):
        while 1:
            img = sensor.snapshot().binary([self.THRESHOLD], invert=True)
            line1 = img.get_regression([(100, 100)], roi=(0, 0, 80, 120), robust=True)
            line2 = img.get_regression([(100, 100)], roi=(80, 0, 80, 120), robust=True)
            output1 = self.get_output(img, line1)
            output2 = self.get_output(img, line2)
            if line1 and line2:
                img.draw_line(((line1.x1() + line2.x1()) // 2, (line1.y1() + line2.y1()) // 2,
                               (line1.x2() + line2.x2()) // 2, (line1.y2() + line2.y2()) // 2),
                              color=(255, 0, 0))
            elif line1:
                img.draw_line((line1.x1() + self.middle, line1.y1(), line1.x2() + self.middle, line1.y2()),
                              color=(255, 0, 0))
            elif line2:
                img.draw_line((line2.x1() - self.middle, line2.y1(), line2.x2() - self.middle, line2.y2()),
                              color=(255, 0, 0))
            else:
                pass

    def run2(self):
        self.clocktime = time.time_ns()
        #print(self.clocktime)
        while 1:
            self.img = sensor.snapshot()
            #self.img.rotation_corr(x_rotation=45)
            dx, middle_x, numy = self.get_dx()
            #print(dx, middle_x, numy)
            #print("dx:", dx, "middle_x:", middle_x)
            #print(time.time_ns())
            #print(self.clocktime)

            if (time.time_ns() - self.clocktime)*0.000001 >= self.sleeptime - 100:
                print((time.time_ns() - self.clocktime)*0.000001)
                self.clocktime = time.time_ns()
                if numy == 2:
                    dx *= 2
                elif numy == 1 and middle_x <= 80:
                    self.send_message(1)

                elif numy == 1 and middle_x >= 80:
                    self.send_message(3)

                elif numy == 0:
                    if dx == -1:
                        self.send_message(1)
                    else:
                        self.send_message(2)
                else:
                    if dx >= 5: # 小左
                        self.send_message(0)
                    elif dx <= -10:  # 右
                        self.send_message(2)
                    else:   # 直行
                        self.send_message(3)
            lcd.display(self.img)

if __name__ == '__main__':
    robot1 = RobotControl()
    robot1.run2()