ESP32（MicroPython） 编码器电机PID调参

ESP32（MicroPython）编码器电机PID调参

之前的闭环控制程序只调节了I参数，这次的程序写了完整的增量式PID算法，电机无负载，实测P=10，I=2时响应快并且无明显抖动。

代码如下

from machine import *
import time
from moto import *
import random
'''
本程序使用增量式PID，公式为
Pwm+=Kp[e(k)-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
e(k)：本次偏差
e(k-1)：上一次的偏差
e(k-2)：上上次的偏差
Kp：比例项参数
Ki：积分项参数
Kd：微分项参数
Pwm：代表增量输出
'''
# 编码器初始化
pin17 = Pin(17, Pin.IN)
pin5 = Pin(5, Pin.IN)   
encoder = encoder(pin17, pin5, 0)   # 参数(编码器A相引脚，编码器B相引脚，定时器序号)

# 电机初始化
motor=PWM(Pin(15),freq=1000,duty=0)
duty=0
target=50
count=0
offset=0 #e(k)
offset1=0 #e(k-1)
offset2=0 #e(k-2)
p=10 #PID参数
i=2
d=0
  
while True:
    count+=1
    speed = encoder.read()
    offset2=offset1 #记录上一次偏差
    offset1=offset
    offset=target-speed
    adjustmentP=offset-offset1
    adjustmentP*=p
    adjustmentI=offset
    adjustmentI*=i
    adjustmentD=offset-offset1-offset1+offset2
    adjustmentD*=d
    adjustment=adjustmentP+adjustmentI+adjustmentD
    duty+=adjustment
    if duty<0:
        duty=0
    elif duty>1023:
        duty=1023
    motor.duty(duty)
    print(target,speed,offset,duty)
    time.sleep(0.05)
    if count==40:
        count=0
        target=random.randint(0,70)

外设驱动

moto.py

from machine import *
import time


class moto:
    def __init__(self, pwm0, pwm1):
        self.pwm0 = pwm0
        self.pwm1 = pwm1
        
    def setPwm(self, pwm):
        pwm = int(pwm)
        if pwm < 0:
            self.pwm0.duty(-pwm)
            self.pwm1.duty(0)
        else:
            self.pwm0.duty(0)
            self.pwm1.duty(pwm)
            
            
class encoder:
    def __init__(self, pin0, pin1, i):
        self.pin0 = pin0
        self.pin0.irq(trigger=Pin.IRQ_RISING, handler=self.handler0)
        self.pin1 = pin1
        self.pin0.irq(trigger=Pin.IRQ_RISING, handler=self.handler1)
        self.counter = 0
        self.speed = 0
        
        self.tim = Timer(i)
        self.tim.init(period=50, callback=self.timHandler)
    
    def handler0(self, a):
        if self.pin0.value():
            self.counter += 1
        else:
            self.counter -= 1
    
    def handler1(self, a):
        if not self.pin1.value():
            self.counter += 1
        else:
            self.counter -= 1
                
    def timHandler(self, t):
        self.speed = self.counter
        self.counter = 0
                
    def read(self):
        return self.speed