Na moich studiach z automatyki i robotyki dużo się mówiło o regulatorach wszelkiego typu, było dużo wykresów, dużo wzorów i trochę trudnych mądrych słów. Natomiast nikt nie zademonstrował jak zrobić taki regulator dlatego kilka lat od zakończenia studiów z automatyki postanowiłem w końcu zaspokoić moją ciekawość i sam przygotowałem regulator PID silnika elektrycznego. Za regulator posłużył mi popularny komputerek Raspberry Pi 4 ze skryptem napisanym w pythonie.
Co będzie potrzebne?
Potrzebne jest 5 rzeczy:
- Raspberry Pi 4
- Silnik elektryczny, ja wykorzystałem silnik firmy DFRobot o numerze katalogowym FIT0450
- Mostek H, ja użyłem mostka L293D
- Koszyczek na baterie AA aby móc zasilić silnik
- Kabelki goldpin męsko-żeńskie
Wszystkie wymienione elementy można kupić w sklepie botland.
Teoria
Nie będę się rozwodzić na teorią, jest tego wystarczająco dużo w internecie, nie ma sensu mnożyć bytów ponad potrzebę. Dobre omówienie za co jest odpowiedzialny dany typ regulatora znajduje się w tym nagraniu:
Podłączenie
Podłączenie mostka H L293D wygląda następująco:
Schemat jest dosyć prosty. Zwrócę jedynie uwagę, że wszystkie piny GND powinny być ze sobą połączone pomimo faktu, że piny z lewej strony są podłączone do zasilania bateryjnego, a piny GND z prawej strony do RPi.
Podłączenie silnika z enkoderem wygląda tak:
Piny 1 i 2 to wyprowadzenia silnika. Piny 3 i 4 to sygnały enkodera. W tym przykładzie interesuje nas tylko określenie obrotów silnika dlatego jeden z pinów zostaje niepodłączony. Użycie dwóch sygnałów enkodera było by konieczne gdybyśmy chcieli określać kierunek obrotu silnika. Piny 5 i 6 to zasilanie enkodera.
Przygotowanie Raspberry Pi
Na Raspberry Pi mam zainstalowany system Raspberry Pi OS. Aby móc sterować prędkością silnika musimy włączyć obsługę PWM w systemie. Aby to zrobić w pliku config.txt znajdującym się na partycji boot musimy dodać następującą linijkę:
dtoverlay=pwm,pin=12Powyższa linijka oznacza, że chcemy uruchomić wsparcie dla PWM na pinie GPIO12
Pomiar prędkości obrotowej
Użyty przeze mnie silnik posiada przekładnie o przełożeniu 120:1. Enkoder silnika generuje 16 impulsów na jeden pełny obrót. Mnożąc 120 przez 16 otrzymamy ilość impulsów, które generuje enkoder na jeden pełny obrót wału. Zatem otrzymujemy 1920 impulsów na jeden obrót wału.
Będziemy chcieli zmierzyć ilość obrotów wału na minutę czyli mierzymy tzw. RPM. Pomiar prędkości będzie dokonywany co sekundę. Czyli będziemy przeliczać ilość impulsów na sekundę na ilość obrotów na minutę.
Musimy rozwiązać następujący układ równań:
# Ilość impulsów na minutę to 1920 razy ilość obrotów na minutę
1920 * RPM = IPM
# Ilość impulsów na minutę to 60 razy ilość impulsów na sekundę
IPM = 60 * IPSMamy wyznaczone IPM czyli ilość impulsów na minutę w obu równaniach więc możemy je do siebie przyrównać:
1920 * RPM = 60 * IPS
RPM = (60 / 1920) * IPS
RPM = IPS / 32Aby więc obliczyć aktualną prędkość musimy podzielić ilość impulsów silnika na sekundę przez 32.
Kod regulatora
Regulator napisałem w pythonie, cały skrypt ma dokładnie 64 linie licząc puste linijki. Omówmy po kolei każdy kawałek kodu:
import signal
import sys
import time
import threading
import RPi.GPIO as GPIOOczywiście skrypt zaczynamy od zaimportowania niezbędnych pakietów. Pakiet RPi umożliwia pracę z peryferiami podłączonymi do RPi, nie jest on chyba domyślnie zainstalowany więc trzeba to zrobić samemu.
INPUT1_GPIO = 23
INPUT2_GPIO = 24
ENABLE_GPIO = 12
ENCODER_GPIO = 4Powyższe zmienne wskazują piny do których są podłączone endkoder i mostek H.
impulses = 0
mutex = threading.Lock()
set_value = int(sys.argv[1])W zmiennej impulses będziemy przechowywać ilość wykrytych impulsów wygenerowanych przez enkoder, mutex będzie nam umożliwiaj sterowanie dostępem do zmiennej impulses. Zmienna set_value to wartość zadana czyli w naszym przypadku ilość obrotów wału silnika na minutę, wartość tę będziemy przekazywać jako parametr skryptu.
def signal_handler(sig, frame):
GPIO.cleanup()
sys.exit(0)Duża część pracy skryptu będzie wykonywana w nieskończonej pętli, jedyną możliwością zakończenia jego pracy będzie wciśnięcie kombinacji Ctrl+C. Aby nie pozostawiać po sobie bałaganu zostanie wywołana ta funkcja gdy zostanie wciśnięta ta kombinacja.
def encoder_callback(channel):
global impulses
mutex.acquire()
impulses += 1
mutex.release()Jest to funkcja, która będzie zliczać impulsy wygenerowane przez enkoder. Jak widać dostęp do zmiennej impulses regulujemy za pomocą wcześniej zdefiniowanego muteksa.
GPIO.setmode(GPIO.BCM)
GPIO.setup(INPUT1_GPIO, GPIO.OUT)
GPIO.setup(INPUT2_GPIO, GPIO.OUT)
GPIO.setup(ENABLE_GPIO, GPIO.OUT)
GPIO.setup(ENCODER_GPIO, GPIO.IN)
GPIO.add_event_detect(ENCODER_GPIO, GPIO.BOTH, callback=encoder_callback)
en_pwm = GPIO.PWM(ENABLE_GPIO, 1000)
en_pwm.start(0)
GPIO.output(INPUT1_GPIO, 1)
GPIO.output(INPUT2_GPIO, 0)Powyższe linie to ustawienia pinów. Pierwsza linia GPIO.setmode określa sposób w jaki będziemy się odwoływać do poszczególnych pinów na płytce. GPIO.BCM oznacza, że będziemy się do nich odwoływać tak jak to jest zdefiniowane w dokumentacji Broadcoma.
Funkcja GPIO.add_event_detect umożliwia przypisanie funkcji do wywołania gdy nastąpi dane zdarzenie. W naszym przypdaku zdarzeniem jest zmiana stanu pinu.
signal.signal(signal.SIGINT, signal_handler)Ustawiamy wywołanie funkcji signal_handler jako reakcję na naciśnięcie kombinacji klawiszy Ctrl+C.
kp = 0.3
ki = 0.2
kd = 0.1
prev_error = 0
err_sum = 0Zmienne przechowujące nastawy regulatora, poprzedni błąd oraz sumę błędów. Poprzedni błąd będzie potrzebny aby obliczyć sygnał części różniczkującej, a suma błędów aby obliczyć sygnał części całkującej.
while True:
mutex.acquire()
error = set_value - (impulses / 32)
err_sum += error
P = kp * error
I = ki * err_sum
D = kd * (error - prev_error)
duty = P + I + D
if duty > 100.0:
duty = 100.0
elif duty < 0.0:
duty = 0.0
en_pwm.ChangeDutyCycle(duty)
print(impulses / 32)
impulses = 0
prev_error = error
mutex.release()
time.sleep(1)Pętla nieskończona w której są obliczane sygnały naszego regulatora. Wszystkie obliczenia są dokonywane po zamknięciu muteksa.
Najpierw obliczamy aktualny błąd. Zwróć uwagę, że wartość zadana jest wyrażona w RPM czyli nie możemy tak po prostu odjąć od niej zmierzonego sygnału, sygnał musi być przekształcony na tę samą jednosktę dlatego dzielimy impulsów przez 32.
Następnie zwiększamy sumę błędów, która to będzie użyta przez część całkującą.
Gdy już mamy obliczony błąd możemy przystąpić do obliczenia sygnałów poszczególnych części naszego regulatora. Kolejn obliczamy wartość sygnału części proporcjonalnej poprzez zwykłe pomnożenie błędu przez wartość kp. Wartość sygnału części całkującej obliczamy poprzez pomnożenie sumy błędów przez wartość ki. Ostatnią wartość czyli wyjście części różniczkującej obliczamy poprzez pomnożenie różnicy pomiędzy aktualnym błędem a poprzednim błędem przez wartość kd.
Gdy już mamy obliczone wartości wyjściowe wszystkich części to sumujemy je. Suma ta będzie wypełnieniem sygnału PWM. Wartość wypełnienia zawiera się w widełkach od 0.0 do 100.0 dlatego też jest w pętli warunek sprawdzający czy obliczona suma mieści się w tych widełkach. Następnie oczywiście ustawiamy tę wartość na pinie za pomocą ChangeDutyCycle.
Następnie wyświetlamy aktualną prędkość na ekranie. Po tych wszystkich operacjach możemy wyzerować ilość zliczonych impulsów. Zapisujemy wartość błędu do zmiennej prev_errror aby użyć jej w kolejnym przebiegu pętli. Zwalniamy muteksa i czekamy sekundę aby powtórzyć cały proces.
Regulator działa w miarę dobrze, regulacja do obrotów na poziomie ok.70-80 RPM w moim przypadku zachodziła dobrze. Regulacja dla 90 RPM działała delikatnie mówiąc tak sobie. A jak wygląda przy wyższych prędkościach to nie wiem ponieważ szybciej nie udało mi się rozpędzić silnika. Może podałem zbyt niskie zasilanie.
Jeśli chodzi o potencjalne rzeczy do poprawy to napewno trzeba by się przyjrzeć nastawom regulatora ponieważ te tak sobie z kapelusza wziąłem i nie szukałem za długo lepszych nastaw.