RaspberryPi w projektach

Regulator PID

Tym razem odpalamy regulator PID, czyli matematyczny kod proporcjnalno, róźniczkująco, całkujący.

Jego zadanie polega na regulowaniu wartości porządanej w zależności od zmienności jej chwilowej wartości. W prostszy sposób można powiedzieć, że zadaniem regulatora PID jest korygowanie nastaw, w taki sposób aby ewentualne odchylenia wartości wyjściowej korygować do żądanej wielkści.

W tym celu stosowany jest kod matematyczny w którym regulowane są zmienne przypisane do elementów proporcjonalnego, różniczkującego i całkującego. Poniższy kod prezentuje sposób działania regulatora PID. klawiszami od 1 do 8 możemy zmieniać nastawy poszczególnych zmiennych.

import time
import random
import pygame
from pygame.locals import *
from sys import exit

pygame.init()

class PID:


    def __init__(self,P=2.0, I=0.0, D=1.0, Derivator=0, Integrator=0, Integrator_max=500, Integrator_min=-500):

        self.Kp=P
        self.Ki=I
        self.Kd=D
        self.Derivator=Derivator
        self.Integrator=Integrator
        self.Integrator_max=Integrator_max
        self.Integrator_min=Integrator_min
        
        self.set_point=0.0
        self.error=0.0

    def update(self,current_value):
        """
        Calculate PID output value for given reference input and feedback
        """

        self.error = self.set_point - current_value

        self.P_value = self.Kp * self.error
        self.D_value = self.Kd * ( self.error - self.Derivator)
        self.Derivator = self.error

        self.Integrator = self.Integrator + self.error

        if self.Integrator > self.Integrator_max:
            self.Integrator = self.Integrator_max
        elif self.Integrator < self.Integrator_min:
            self.Integrator = self.Integrator_min

        self.I_value = self.Integrator * self.Ki

        PID = self.P_value + self.I_value + self.D_value

        return PID

    def setPoint(self,set_point):

        self.set_point = set_point
        self.Integrator=0
        self.Derivator=0

    def setIntegrator(self, Integrator):
        self.Integrator = Integrator

    def setDerivator(self, Derivator):
        self.Derivator = Derivator

    def setKp(self,P):
        self.Kp=P

    def setKi(self,I):
        self.Ki=I

    def setKd(self,D):
        self.Kd=D

    def getPoint(self):
        return self.set_point

    def getError(self):
        return self.error

    def getIntegrator(self):
        return self.Integrator

    def getDerivator(self):
        return self.Derivator

BLACK = (0,0,0)
WHITE = (255,255,255)
BLUE = (0,0,255)
GREEN = (0,255,0)
RED = (255,0,0)

ekran=pygame.display.set_mode((1000,800))
pygame.display.set_caption("filtr dolnoprzepustowy")
czcionka = pygame.font.SysFont("monospace", 30)
ekran.fill(WHITE)
pygame.draw.line(ekran,BLACK,(10,10),(10,610),1)
pygame.draw.line(ekran,BLACK,(10,305),(950,305),1)

i=10
erro=0
erro2=0
t1=0
t2=0
wy2=0
wy=0
out2=0
PO=0
PO2=0
P=0.1
I=0
D=0
p=PID(P,I,D)
wzmocnienie=20
p.setPoint(PO)

while True:
    pid = p.update(erro)
    i=i+1
    if i>950:
        ekran.fill(WHITE)
        pygame.draw.line(ekran,BLACK,(10,10),(10,610),1)
        pygame.draw.line(ekran,BLACK,(10,305),(950,305),1)

        i=10
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            pygame.quit()
            exit()
        if event.type == pygame.KEYDOWN:
            if event.key == pygame.K_1:
                wzmocnienie=wzmocnienie*2
            if event.key == pygame.K_2:
                wzmocnienie=wzmocnienie/2
            if event.key == pygame.K_3:
                P=P+0.1
            if event.key == pygame.K_4:
                P=P-0.1
            if event.key == pygame.K_5:
                I=I+0.1
            if event.key == pygame.K_6:
                I=I-0.1
            if event.key == pygame.K_7:
                D=D+0.1
            if event.key == pygame.K_8:
                D=D-0.1
        p=PID(P,I,D)
    if i==100:
        PO=5
    if i==200:
        PO=1
    if i==300:
        PO=5
    if i==400:
        PO=1
    if i==500:
        PO=5
    if i==600:
        PO=1
    if i==700:
        PO=5
    if i==800:
        PO=1
    if i==900:
        PO=5
    pygame.draw.line(ekran,RED,(i,(305-wzmocnienie*PO)),(i-1,(305-wzmocnienie*PO2)),3)

    erro=PO-wy
    if erro>0:
        wy=wy2-pid
    else:
        wy=wy2-pid
    PO2=PO
    wy2=wy
    if wy<-300:
        out=-290
    else:
        out=wy
    pygame.draw.line(ekran,BLUE,(i,(305-wzmocnienie*out)),(i-1,(305-wzmocnienie*out2)),1)
    out2=out

    label = czcionka.render("PO= %.2f" %(PO), 1, BLUE,WHITE)
    ekran.blit(label, (650, 10))
    label = czcionka.render("error = %.2f" %(erro), 1, BLUE,WHITE)
    ekran.blit(label, (650, 120))
    label = czcionka.render("PID = %.2f" %(pid), 1, BLUE,WHITE)
    ekran.blit(label, (650, 350))
    label = czcionka.render("Wy = %.2f" %(wy), 1, BLUE,WHITE)
    ekran.blit(label, (650, 450))
    label = czcionka.render("I = %.2f" %(i), 1, BLUE,WHITE)
    ekran.blit(label, (650, 550))
    label = czcionka.render("Kl.3 i 4 P= %.2f" %(P), 1, BLUE,WHITE)
    ekran.blit(label, (50, 650))
    label = czcionka.render("Kl.5 i 6 I= %.2f" %(I), 1, BLUE,WHITE)
    ekran.blit(label, (50, 700))
    label = czcionka.render("Kl.7 i 8 D= %.2f" %(D), 1, BLUE,WHITE)
    ekran.blit(label, (50, 750))
    time.sleep(0.02)
    pygame.display.update()

Na poniszym zdjęciu można zuważyć sposób działania regulatora. Kolorem czerwonym prezentowana jest wartość pożądana, a kolorem niebieskim wartość jaka wychodzi z regulatora PID, czyli wartość adaptowana do pożądanej.

RASPBERRY PI w projektach

Regulator PID