当直线是横向的时候线性回归就会乱 求解

[stzo]

固件是最新的

import sensor, image, time
from pyb import UART

# 鲁棒线性回归例程
# 可以跟踪所有指向相同的总方向但实际上没有连接的线。 在线路上使用
# find_blobs（），以便更好地过滤选项和控制。


from pid import PID
rho_pid = PID(p=6.0, i=0.1, d=0.3)   #控制rol
theta_pid = PID(p=2.5, i=0)#控制yaw

finds_blob=0
flymode=1


THRESHOLD = (0, 100) # Grayscale threshold for dark things...

BINARY_VISIBLE = True # 首先二值化

up_roi   = (0,0, 80, 15)
down_roi = (0, 55, 80, 15)
left_roi = (0, 0, 25, 60)
righ_roi = (55, 0,25, 40)

y=0

up_roif   = (0,0, 80, 30)
down_roif = (0, 30, 80, 30)
left_roif = (0, 0, 40, 60)
righ_roif = (40, 0,40, 60)


sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)
#阈值内数据（白色像素）较多时，QQVGA帧率降到5以下。线较细时可以使用（7·19测试）
#QQQVGA较稳定，帧率保持高速（50以上）
sensor.set_framesize(sensor.QQQVGA) # 80x60 (4,800 pixels) - O(N^2) max = 2,3040,000.
sensor.skip_frames(time = 2000)     # WARNING: If you use QQVGA it may take seconds
clock = time.clock()                # to process a frame sometimes.
sensor.set_vflip(True)
sensor.set_hmirror(True)


def find_border(img,area_roi):
    area=0
    singleline_check = img.get_regression([(255,255)],roi=area_roi, robust = True)
    if (singleline_check):
        area=1
    return area



uart = UART(3,500000)#初始化串口 波特率 500000

class Receive(object):
    uart_buf = []
    _data_len = 0
    _data_cnt = 0
    state = 0

R=Receive()

def UartSendData(Data):
    uart.write(Data)


def DotDataPack(color,flag,x,y):
    print("found: x=",x,"  y=",y,          flymode)
    pack_data=bytearray([0xAA,0x29,0x05,0x41,0x00,color,flag,x>>8,x,(y)>>8,(y),0x00])
    lens = len(pack_data)#数据包大小
    pack_data[4] = 6;#有效数据个数
    i = 0
    sum = 0
    #和校验
    while i<(lens-1):
        sum = sum + pack_data[i]
        i = i+1
    pack_data[lens-1] = sum;
    return pack_data


times=0
def find_123(img):
    global times
    global flymode
    upflag=0
    leftflag=0
    rightflag=0
    downflag=0
    finds_blob=0
    upflag=find_border(img,up_roi)
    downflag=find_border(img,down_roi)
    leftflag=find_border(img,left_roi)
    rightflag=find_border(img,righ_roi)

    if upflag:
        finds_blob=finds_blob|0x01

    if downflag:
        finds_blob=finds_blob|0x02

    if leftflag:
        finds_blob=finds_blob|0x04

    if rightflag:
        finds_blob=finds_blob|0x08

    if flymode==1 :
        if finds_blob&0x01==0x00:
            times+=1
        else :
            times=0
        if times>10:
            if finds_blob&0x04!=0x00:
                flymode=3
            elif finds_blob&0x08!=0x00:
                flymode=4
    elif flymode==2 :
        if finds_blob&0x02==0x00:
            times+=1
            if times>15:
                if finds_blob&0x04!=0x00:
                    flymode=3
                elif finds_blob&0x08!=0x00:
                    flymode=4
        else :
             times=0
    elif flymode==3 :
        if finds_blob&0x04==0x00:
            times+=1
            if times>15:
                if finds_blob&0x01!=0x00:
                    flymode=1
                elif finds_blob&0x02!=0x00:
                    flymode=2
        else :
             times=0
    elif flymode==4 :
        if finds_blob&0x08==0x00:
            times+=1
            if times>15:
                if finds_blob&0x01!=0x00:
                    flymode=1
                elif finds_blob&0x02!=0x00:
                    flymode=2
        else :
             times=0

while(True):
    clock.tick()
    img = sensor.snapshot().binary([(0,100)]).lens_corr(strength = 1.8, zoom = 1.0)# if BINARY_VISIBLE else sensor.snapshot()
    #for i in [0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30]:
        #img[i*160:i*160+80]=255

    #print(img[0])
    # 返回类似于由find_lines（）和find_line_segments（）返回的线对象。
    # 你有x1（），y1（），x2（），y2（），length（），
    # theta（）（以度为单位的旋转），rho（）和magnitude（）。
    #
    # magnitude() 表示线性回归的工作情况。这对于鲁棒的线性回归意味着不同
    # 的东西。一般来说，值越大越好...

  #  for l in img.find_lines(roi=(30,20,20,20),threshold=1000)
    find_123(img)
    #print(flymode)
    if flymode==1 :
        roi=up_roif
    elif flymode==2 :
        roi=down_roif
    elif flymode==3 :
        roi=left_roif
    elif flymdoe==4 :
        roi=righ_roif
    line = img.get_regression([(255,255) if BINARY_VISIBLE else THRESHOLD],roi=roi,robust = True)
    #line = img.get_regression([(255,255) if BINARY_VISIBLE else THRESHOLD],robust = True)

    if (line):
        rho_err = abs(line.rho())-img.width()/2
        if line.theta()>90:
            theta_err = line.theta()-180
        else:
            theta_err = line.theta()

        if line.magnitude()>8:
            #if -40<b_err<40 and -30<t_err<30:
            rho_output = rho_pid.get_pid(rho_err,1)         #正值->右移  rol
            theta_output = theta_pid.get_pid(theta_err,1)   #正值->右转  yaw
            #if abs(theta_output)>80:
                #y=0
            #else :
            #   y=200
            if(flymode==1):
                x=rho_output
                y=100

            elif(flymode==2):
                x=rho_output
                y=-100

            elif(flymode==3):
                x=-100
                y=rho_output

            elif(flymode==4):
                x=100
                y=rho_output
            UartSendData(DotDataPack(0,1,int(x),int(y)))
            #print("rho_output="+str(rho_output)+"    theta_output="+str(theta_output))

        img.draw_line(line.line(), color = 127,thickness=2)
    print("FPS %f, mag = %s" % (clock.fps(), str(line.magnitude()) if (line) else "N/A"))