openmv+PID算法详解

openmv官网上讲得太简略了，我是比较好奇算法的，看了一篇讲PID的，讲得很好。

一文读懂PID控制算法（抛弃公式，从原理上真正理解PID控制）

请先理解这篇非常棒的文章。

现在结合openmv提供的代码看看

注释一律在代码下面

先复习一下Δt和dt的关系，下面用得着，别笑，我忘了

from pyb import millis
#返回代码执行到当前的时间

from math import pi, isnan
#pi-->Π，isnan-->用于检查给定数字是否为“ NaN” (不是数字)，它接受一个数字，如果给定数字为“ NaN” ，则返回True ，否则返回False 。



class PID:
#PID(proportion integration differentiation)
#     比例		   积分		微分
    _kp = _ki = _kd = _integrator = _imax = 0
    #初始化三个系数，积分，？？？为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')
		#设置微分为nan


    def get_pid(self, error, scaler):
#根据差值，K获取pid
        tnow = millis()
		#现在的时间
        dt = tnow - self._last_t
		#和上次的时间差
        output = 0
		#总输出（你调节的量）
        if self._last_t == 0 or dt > 1000:
		#如果是第一个轮回（初始值为0）或时间差>1s（大于可微积分的阈值）
            dt = 0
		#时间差归零
            self.reset_I()
			#重置I
        self._last_t = tnow
		#记录结束时间
        delta_time = float(dt) / float(1000)
		#获取Δt
        output += error * self._kp
		#加入比例控制积分
        if abs(self._kd) > 0 and dt > 0:
		#若微分参数和时间差>0
            if isnan(self._last_derivative):
		    #若微分是NaN
                derivative = 0
			#微分归零
                self._last_derivative = 0
			#PS:前面已经声明为nan，所以openmv没有用微分控制
            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:
		#若I参数>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):
	//重置I
        self._integrator = 0

        self._last_derivative = float('nan')

好像还是有点看不懂，看看openmv是怎么用的。

x.y轴分布

# Blob Detection Example

#

# This example shows off how to use the find_blobs function to find color

# blobs in the image. This example in particular looks for dark green objects.



import sensor, image, time

import car

from pid import PID



# You may need to tweak the above settings for tracking green things...

# Select an area in the Framebuffer to copy the color settings.



sensor.reset() # Initialize the camera sensor.

sensor.set_pixformat(sensor.RGB565) # use RGB565.

sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed.

sensor.skip_frames(10) # Let new settings take affect.

sensor.set_auto_whitebal(False) # turn this off.

clock = time.clock() # Tracks FPS.



# For color tracking to work really well you should ideally be in a very, very,

# very, controlled enviroment where the lighting is constant...

green_threshold   = (76, 96, -110, -30, 8, 66)

size_threshold = 2000

x_pid = PID(p=0.5, i=1, imax=100)
#1.位置的pid调节

h_pid = PID(p=0.05, i=0.1, imax=50)
#2.大小的pid


def find_max(blobs):

    max_size=0

    for blob in blobs:

        if blob[2]*blob[3] > max_size:

            max_blob=blob

            max_size = blob[2]*blob[3]

    return max_blob



while(True):

    clock.tick() # Track elapsed milliseconds between snapshots().

    img = sensor.snapshot() # Take a picture and return the image.



    blobs = img.find_blobs([green_threshold])

    if blobs:

        max_blob = find_max(blobs)

        x_error = max_blob[5]-img.width()/2
		#球心x减去一半宽度-->距离中心点的差值（以中间为0，左-右+）
        h_error = max_blob[2]*max_blob[3]-size_threshold
		#距离设定大小的差值(远-近+)
        print("x error: ", x_error)

        '''

        for b in blobs:

            # Draw a rect around the blob.

            img.draw_rectangle(b[0:4]) # rect

            img.draw_cross(b[5], b[6]) # cx, cy

        '''

        img.draw_rectangle(max_blob[0:4]) # rect

        img.draw_cross(max_blob[5], max_blob[6]) # cx, cy

        x_output=x_pid.get_pid(x_error,1)
		#调整位置
        h_output=h_pid.get_pid(h_error,1)
		#调整距离
        print("h_output",h_output)

        car.run(-h_output-x_output,-h_output+x_output)
        #run 我不展示了，第一个是左侧速度，第二个是右侧速度
     #与小车远离成正比，左侧与偏左成正比，右侧与偏右成正比
    #这里我感觉左右是反的，想了好久好久，直到我看到视屏里的openmv是倒置的23333
    #也就是摄像头左右对调，那就对了。。。
    else:

        car.run(18,-18)

还有不明白，希望有人告诉我，其他的不懂可以留言，说的不好还请指正。