2020-08-22

广西●河池学院
广西高校重点实验室培训基地
系统控制与信息处理重点实验室
本篇博客来自河池学院: 无人机智控小组
写作时间: 2020年8月22日

基于openmv小车循迹的调试

import sensor, image, time

from pyb import LED

import car

from pid import PID

import sensor, image, time, math

from pyb import UART

import json

import ustruct

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.
i=0

#初始化串口
uart = UART(3, 115200)
uart.init(115200, bits=8, parity=None, stop=1)

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
        print(theta_err)


    else:


        theta_err = line.theta()
        print(theta_err)




    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(52+output, 42-output)
        uart.write(52+output,42-output\r\n)


    else:


        car.run(-45,-45)
        # car.run(0,0)


else:


    car.run(50,-50)


    pass


#print(clock.fps())

在这里首先要调试PID和PWM，先确定好PWM的值，再调节P，先把P从小到大调试，找到一个适合的值，然后在调节I，D，慢慢去寻找其中规律，一边调试一边运行小车。