如何让黑线始终在小车中央

线性回归巡线例程用不了

# Untitled - By: wfeng - 周六 3月 20 2021

import sensor, image, time, math
from pyb import LED
import car
from pid import PID		#调用模块
theta_pid =PID(p=0.4,i=0
GRAYSCALE_THRESHOLD =[(0,64)]	#跟踪一条黑线。使用[(0128,255)]跟踪白线。

LED(1).on()
LED(2).on()
LED(3).on()			#开启led补光

ROIS =[#[ROI,weight]
        (0, 100, 160, 20, 0.7), # 你需要调整应用程序的权重
        (0,  50, 160, 20, 0.3), # 取决于你的机器人是如何设置的。
        (0,   0, 160, 20, 0.1)
    ]			#设置感兴趣区域

weight_sum=0
for r in ROIS:weight_sum += r[4]	#r[4]是roi权重

#相机设置
sensor.reset()			#初始化摄像头
sensor.set_pixformat(sensor.GRAYSCALE)#使用灰度图
sensor.set_framesize(sensor.QQVGA)	#使用QVGA加速
sensor.skip_frames(time =2000)	#让新设置生效
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)	#必须关闭颜色追踪
clock =time.clock()			#跟踪FPS

while(True):
    clock.tick()			#跟踪快照间的毫秒数（）
    img =sensor.snapshot()		#拍照并返回图像
    centroid_sum =0

    for r in ROIS:
        blobs =img.find_blobs(GRAYSCALE_THRESHOLD, roi=r[0:4],merge=True)

        if blobs:
            largest_blob=max(blobs,key=lambda b: b.pixels())	#找到像素最多的点

            img.draw_rectangle(largest_blob.rect())
            img.draw_cross(largest_blob.cx(),
                            largest_blob.cy())			#在色块周围画一个矩形

            centroid_sum+= largest_blob.cx()*r[4]

    center_pos =(centroid_sum / weight_sum)		#确定线的中心

    theta =0

    theta= -math.atan((center_pos-80)/60)

    theta= math.degrees(theta)		#将角度转化

    print("Turn Angle: %f" % theta)

    output = theta_pid.get_pid(theta,1)
    car.run(50+int(output), 50-int(output))

    print(clock.fps)

THRESHOLD = (5, 70, -23, 15, -57, 0) # Grayscale threshold for dark things...
import sensor, image, time
from pyb import LED
import car
from pid import PID
rho_pid = PID(p=0.4, i=0)
theta_pid = PID(p=0.001, i=0)

LED(1).on()
LED(2).on()
LED(3).on()

sensor.reset()
sensor.set_vflip(True)
sensor.set_hmirror(True)
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQQVGA) # 80x60 (4,800 pixels) - O(N^2) max = 2,3040,000.
#sensor.set_windowing([0,20,80,40])
sensor.skip_frames(time = 2000)     # WARNING: If you use QQVGA it may take seconds
clock = time.clock()                # to process a frame sometimes.

while(True):
    clock.tick()
    img = sensor.snapshot().binary([THRESHOLD])
    line = img.get_regression([(100,100,0,0,0,0)], 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()
        img.draw_line(line.line(), color = 127)
        print(rho_err,line.magnitude(),rho_err)
        if line.magnitude()>8:
            #if -40<b_err<40 and -30<t_err<30:
            rho_output = rho_pid.get_pid(rho_err,1)
            theta_output = theta_pid.get_pid(theta_err,1)
            output = rho_output+theta_output
            car.run(50+output, 50-output)
        else:
            car.run(0,0)
    else:
        car.run(50,-50)
        pass
    #print(clock.fps())