基于open mv 搭配stm32循迹

基于open mv 搭配stm32循迹

一.代码

THRESHOLD = (0, 43, 20, -8, -37, 14) # Grayscale threshold for dark things...
import sensor, image, time
from pyb import LED
import car
from pid import PID
from pyb import UART
from pyb import Pin
rho_pid = PID(p=0.4, i=0)
theta_pid = PID(p=0.001, i=0)

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.
uart = UART(3, 115200)

while(True):

    clock.tick()
    img = sensor.snapshot().binary([THRESHOLD])
    line = img.get_regression([(100,100)], 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
            rho_output = rho_pid.get_pid(rho_err,1)
            theta_output = theta_pid.get_pid(theta_err,1)
            output = rho_output+theta_output
            if rho_err > -3 and rho_err < 3:
                uart.write("1")
                p_out = Pin('P0', Pin.OUT_PP)
                p_out.high()
                p_out = Pin('P1', Pin.OUT_PP)
                p_out.low()#设置p_out引脚为低
                p_out = Pin('P2', Pin.OUT_PP)
                p_out.low()#设置p_out引脚为低
                print('0')
            elif rho_err <= -3:
                p_out = Pin('P1', Pin.OUT_PP)#设置p_out为输出引脚
                p_out.high()#设置p_out引脚为高
                p_out = Pin('P0', Pin.OUT_PP)
                p_out.low()#设置p_out引脚为低
                p_out = Pin('P2', Pin.OUT_PP)
                p_out.low()#设置p_out引脚为低
                uart.write("2")
                print('1')
            elif rho_err >= 3:
                uart.write("3")
                p_out = Pin('P2', Pin.OUT_PP)#设置p_out为输出引脚
                p_out.high()#设置p_out引脚为高
                p_out = Pin('P1', Pin.OUT_PP)
                p_out.low()#设置p_out引脚为低
                p_out = Pin('P0', Pin.OUT_PP)
                p_out.low()#设置p_out引脚为低
                print('2')

    #print(clock.fps())

这里使用星瞳的例程，加入自己的思想，由于时刚学OPEN MV所以使用这种简单的思想。这里OPEN MV相当于循迹模块。

二.思想
这里我是主要使用rho_err这个函数，在星瞳例程中会在所循迹物体中间画出一条线，而rho_err便是那个线对于所循的误差，偏左了小于0，偏大了大于0，rho_err >= 某个数，某个数就是偏移量，可以对某个数进行调整使路线更加精准。然后对IO口设置高低电平。32读取io电平对路线进行调整。

如果还有问题可以在下方留言。