文章目录
- 一、代码
- 使用方法
- 1.1 Python3 版本
- 1.2 MicroPython - OpenMV版本
- 1.3 MicroPython - K210版本
- 二、说明
- 三、参考资料
一、代码
使用方法
from pid import PID
# p:比例系数,i:微分系数,d:积分系数,imax:积分限幅
pid1 = PID(p=0.07, i=0.01, d=0.01, imax=90)
while(True):
# error:误差值, 误差值 = 目标值 - 测量值
error = 50
# optput:PID的输出
output = pid1.get_pid(error)
1.1 Python3 版本
from math import pi, isnan
import time
def millis():
return int(time.time() * 1000)
class PID:
_kp = _ki = _kd = _integrator = _imax = 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')
def get_pid(self, error, scaler=1):
tnow = millis()
dt = tnow - self._last_t
output = 0
if self._last_t == 0 or dt > 1000:
dt = 0
self.reset_I()
self._last_t = tnow
delta_time = float(dt) / float(1000)
output += error * self._kp
if abs(self._kd) > 0 and dt > 0:
if isnan(self._last_derivative):
derivative = 0
self._last_derivative = 0
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:
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):
self._integrator = 0
self._last_derivative = float('nan')
1.2 MicroPython - OpenMV版本
from pyb import millis
from math import pi, isnan
class PID:
_kp = _ki = _kd = _integrator = _imax = 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')
def get_pid(self, error, scaler=1):
tnow = millis()
dt = tnow - self._last_t
output = 0
if self._last_t == 0 or dt > 1000:
dt = 0
self.reset_I()
self._last_t = tnow
delta_time = float(dt) / float(1000)
output += error * self._kp
if abs(self._kd) > 0 and dt > 0:
if isnan(self._last_derivative):
derivative = 0
self._last_derivative = 0
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:
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):
self._integrator = 0
self._last_derivative = float('nan')
1.3 MicroPython - K210版本
from math import pi, isnan
import utime
def millis():
return utime.ticks_ms()
class PID:
_kp = _ki = _kd = _integrator = _imax = 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')
def get_pid(self, error, scaler=1):
tnow = millis()
dt = tnow - self._last_t
output = 0
if self._last_t == 0 or dt > 1000:
dt = 0
self.reset_I()
self._last_t = tnow
delta_time = float(dt) / float(1000)
output += error * self._kp
if abs(self._kd) > 0 and dt > 0:
if isnan(self._last_derivative):
derivative = 0
self._last_derivative = 0
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:
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):
self._integrator = 0
self._last_derivative = float('nan')
二、说明
原代码来自 OpenMV 官方的 PID 代码。因为不同平台的 MicroPython 的 API 有差异,所以移植需要重写 millis
函数。
millis 函数用来获取系统开机后运行的时间长度,单位是毫秒。
三、参考资料
- openmv/pid.py · GitHub:https://github.com/openmv/openmv/blob/master/scripts/libraries/pid.py
致正在学习嵌入式的小伙伴们:
嵌入式的学习是要基础知识 + 动手实践同步进行的,在这里给大家推荐一个可以系统学习嵌入式和刷题的网站:牛客网(传送门:牛客网)。
牛客网原本是一个刷 IT 题库的网站,经过多年的发展,已经成为了一个集 课程+刷题+面经+求职+讨论区分享 的一站式求职学习网站,最最最重要的里面的资源全部免费, 而最近他们正打算将这套模式推广到嵌入式行业,专门为嵌入式工程师开设了相应的板块,正在学习嵌入式、寻找嵌入式专项题库、想要找嵌入式相关工作但又缺乏经验的小伙伴们,千万不要错过