openmv上的抗积分饱和、梯形积分、变积分与不完全微分PID

最新推荐文章于 2023-01-28 08:00:00 发布

ZC_lianshuang

最新推荐文章于 2023-01-28 08:00:00 发布

阅读量491

点赞数

分类专栏： openmv 文章标签： openmv PID 改进算法

本文链接：https://blog.csdn.net/ZC_lianshuang/article/details/97135600

版权

openmv 专栏收录该内容

1 篇文章 0 订阅

订阅专栏

import math,pyb
class PID:
    _kp = _ki = _kd = 0
    _lasterror = 0
    _maximum = _minimum = _errorabsmax = _errorabsmax = 0
    _alpha = 0
    _output = 0
    _derivative = _integralValue = 0
    def __init__(self, p=0, i=0, d=0,alpha=0,maximum=0,minimum=0,errorabsmax=0,errorabsmin=0,integralValue=0,lasterror=0):
        self._kp = float(p)
        self._ki = float(i)
        self._kd = float(d)
        self._alpha = float(alpha)
        self._maximum = float(maximum)
        self._minimum = float(minimum)
        self._errorabsmax = float(errorabsmax)
        self._errorabsmin = float(errorabsmin)
        self._integralValue = float(integralValue)
        self._lasterror = float(lasterror)
        self._derivative = 0.0
        self._output = 0.0
    def VariableIntegralCoefficient(self, error,absmax,absmin):
        factor=0.0
        if abs(error) <= absmin:
           factor=1.0;
        elif abs(error) > absmax:
           factor=0.0;
        else:
           factor=(absmax-abs(error))/(absmax-absmin);
        return factor
    def get_pid(self, error):
        thisError = error
        output = self._output
        if output > self._maximum:
            if thisError <= 0:
                self._integralValue = self._integralValue + thisError
        elif output < self._minimum:
            if thisError >= 0:
                self._integralValue = self._integralValue + thisError
        else:
            self._integralValue = (self._integralValue + thisError)/2
        factor = self.VariableIntegralCoefficient(thisError,self._errorabsmax,self._errorabsmin)
        self._derivative = self._kd*(1 - self._alpha)*(thisError - self._lasterror) + self._alpha * self._derivative
        output = self._kp*thisError + self._ki*self._integralValue + self._derivative
        self._output = output
        self.lasterror = thisError
        return output