KursyPoradnikiInspirujące DIYForum

Algorytm linefollowera w C – dla początkujących i nie tylko

Algorytm linefollowera w C – dla początkujących i nie tylko

Artykuł dotyczy podstaw programowania robota typu linefollower w języku C. Omówiony został kompletny program pozwalający na przyzwoitą kontrolę nad LFem.

Dodatkowo wskazane zostały również miejsca, w których inwencja programisty może w wyraźny sposób poprawić pracę robota.

Tekst można podzielić na dwie części - konfigurację wszystkich wymaganych do pracy peryferiów (porty I/O wraz z przerwaniem zewnętrznym, timer generujący sygnał PWM oraz przerwanie co określony czas, ADC) oraz przedstawiającą właściwy algorytm jazdy po linii.

Artykuł starałem się pisać w możliwie przystępny sposób, a adresatami są osoby, które nie programowały wcześniej swoich robotów opartych o mikrokontroler AVR w języku C, a także szukające wskazówek czy inspiracji odnośnie algorytmu działania linefollowera. Podstawy programowania w C oraz wiedza z ogólnie pojętej robotyki oraz działania mikrokontrolera mogą się jednak okazać konieczne do lekkiego przyjęcia tekstu (nie będę omawiał kwestii sprzętowych oraz tłumaczył np. co to jest PWM).

Schemat robota, dla którego zostanie przedstawiony kod programu, znajduje się poniżej (jest to robot rzazim kolegi ojebejbe, którego opis znajduje się na forum):

Przykładowy schemat LineFollowera

Przykładowy schemat LineFollowera

W rzeczywistości w układzie znajduje się mikrokontroler ATmega328 i na jego przykładzie będę opisywał obsługę układów peryferyjnych (konfiguracja dla całej rodziny ATmegaX8 wygląda identycznie). Dodatkowo (w celu zaprezentowania jego obsługi) zakładamy, że w miejscu odbiornika TSOP znajduje się switch zwierajacy pin PD3 do masy po wciśnięciu.

Serdecznie zapraszam do lektury!

Zanim rozpoczniesz

Przed przystąpieniem do pracy koniecznie zaopatrz się w bardzo ważny dokument - datasheet mikrokontrolera. Jeśli nie korzystałeś wcześniej z DSów, teraz nadszedł na to czas. Nie da się efektywnie korzystać z jakiegokolwiek układu elektoronicznego bez umiejętności posługiwania się jego dokumentacją techniczną. A DSy Atmela są wg. mnie bardzo przystępnie napisane.

W ramach przygotowań należy także ustawić odpowiednie fusebity w procesorze. W przypadku ATmegi328 jedyną zamianą, którą musimy wprowadzić względem ustawień fabrycznych, to wyłączenie dzielenia zegara systemowego przez 8, uzyskując częstotliwość taktowania procesora równą 8MHz.

Konfiguracja peryferiów

Programowanie zaczniemy od odpowiedniej konfiguracji układów peryferyjnych mikrokontrolera.

GPIO

Na pierwszy ogień pójdzie rzecz najprostsza - konfiguracja portów I/O, której opis znajdziemy w DS począwszy od strony 77. Do tego celu wykorzystamy znaleziony przeze mnie kiedyś w internecie plik nagłówkowy ze sprytnym makrem pozwalającym na przypisanie danemu bitowi w rejestrze etykiety, z której możemy potem korzystać jak ze zwykłej zmiennej. W działanie makra nie trzeba się zagłębiać - jest to jedyny fragment kodu w tym artykule, który 'bierzemy jak jest' i po prostu z niego korzystamy (jeśli jednak io_cfg.h Ci się nie podoba - niżej przedstawiam także tradycyjny sposób).

Jak wiadomo, każdy port w AVRze ma trzy odpowiadające mu rejestry - DDRx, PORTx i PINx. Do pełnej kontroli nad danym pinem trzeba więc stworzyć odpowiednie 3 makra. Pamiętamy jednak, że jeśli pin działać będzie jako wyjście - potrzebujemy dostepu tylko do rejestrów DDRx oraz PORTx.

Dla przykładu, jeśli pin PB0 wykorzystujemy jako wyjście sterujące diodą LED, wymagane makra wyglądają tak:

Pierwsza linijka tworzy etykietę odpowiadającą bitowi 0 w rejestrze DDRB. Wykorzystując ją, możemy w prosty sposób ustawić pin PB0 jako wyjście:

Druga linijka pozwala z kolei na dostęp do bitu 0. w rejestrze PORTB. Szczęśliwie dla nas jest ona tzw. lValue - czyli możemy z niej zarówno czytać, jak i do niej zapisywać. Wpisanie jedynki do powyższego rejestru będzie teraz wyglądać następująco:

Zmina stanu pinu także jest banalna (wykorzystujemy operację XOR - 0^1=1 , 1^1=0):

W przypadku pinu wejściowego, musimy jeszcze mieć dostęp do rejestru odpowiedzialnego za informowanie nas o stanie pinu. Przykładowo makra do obsługi pinu z podłączonym switchem przedstawiają się następująco:

Odczyt stanu na pinie PB1 będzie od teraz możliwy przy pomocy etykiety SW1_PIN. Wpisanie stanu pinu do zmiennej foo:

Podsumowując, dla każdego pinu tworzymy 2 (pin wyjściowy) lub 3 (pin wejściowy) makra, dzięki którym późniejsza ich inicjalizacja i obsługa będzie łatwa i przede wszystkim bardzo przejrzysta.

Gdy stworzymy już wszystkie wymagane makra, sama konfiguracja portów I/O przebiega szybko i bezboleśnie (konfiguracja wyjść sterujących mostek H i diodę LED oraz wejście switcha):

W przypadku opisywanej wcześniej diody podłączonej do nóżki PB0 ustawienie tego pinu jako wyjście będzie wyglądało tak:

Wystawienie na wyjście odpowiednio logicznej jedynki i zera oraz zmiana stanu odbywa się teraz poprzez operacje:

Konfiguracja odpowiadająca poprzedniemu fragmentowi kodu przedstawia się następująco:

Przerwanie zewnętrzne

Dokonaliśmy już konfiguracji pinu PD3 jako wejścia - jest do niego podłączony switch zwierający do masy i jest on podciągnięty wewnętrznie do linii zasilania. Nic więc nie stoi na przeszkodzie, aby ustawić wyzwalanie przerwania po wciśnięciu przycisku, tj. zmianie stanu pinu.

Aby dowiedzieć się, na jak działa obsługa przerwań zewnętrznych i jak je skonfigurować, zaglądamy do dokumentacji - strona 72. Pin PD3 odpowiada linii PCINT19 kontrolera zewnętrznych przerwań. Aby więc uruchomić jej obsługę, należy ustawić bit PCIE2 w rejstrze PCICR (czyli de facto włączyć przerwania zewnętrzne na porcie D), oraz uaktywnić ten konkretny pin w rejestrze PCMSK2:

Finalnie należy jeszcze uruchomić globalną obsługę przerwań za pomocą krótkiego:

I to wszystko! Od teraz za każdym razem, gdy zmieni się stan pinu PD3, zostanie uruchomione przerwanie obsługiwane przez ISR (Interrupt Service Routine - procedura obsługi przerwania) PCINT1_vect. Nasz przycisk zostanie wykorzystany do zmieniania flagi jazda, dzięki czemu będzie działał jako przycisk on/off. ISR wykonujący to zadanie będzie wyglądał następująco:

Należy jednak zwrócić uwagę, że powyższa obsługa przerwania nie jest idealna - przede wszystkim ze względu na znajdującą się tam funkcję _delay_ms(), której raczej nie powinno stosować się w ISRze, ponieważ blokuje ona działanie całego programu. Ponieważ jednak powyższy kod implementuje przycisk on/off, który niejako ma władzę absolutną nad resztą programu - możemy sobie pozwolić na takie ułatwienie pracy.

Timer - generowanie sygnału PWM

Następnym punktem będzie konfiguracja Timera 1 do generowania sygnału PWM potrzebnego do sterowania silnikami. Wszystkie potrzebne informacje znajdziemy w DS począwszy od strony 115. Po zapoznaniu się z opisem działania generatora PWM możemy przejść na stronę 136., gdzie opisane są poszczególne rejestry konfiguracyjne Timera 1.

Będziemy korzystać z trybu Fast PWM - jestem zwolennikiem używania PWMa o wysokiej częstotliwości gdy przychodzi do sterowania silnikami (a w przypadku silników 'coreless' jak np. Faulhabery jest to wręcz konieczne). Co więcej, jeśli chcemy jeździć szybko, pętla sterowania (o której będzie mowa w sekcji poświęconej tworzeniu algorytmu) musi być wykonywana możliwie często (raz na 10ms to minimum, a lepiej szybciej) i najlepiej, żeby silnikowy PWM miał częstotliwość kilkukrotnie większą, niż częstotliwość wywoływania pętli sterowania.

Tabela 16-2 w dokumentacji układu mówi nam, jak powinniśmy ustawić bity COM1A1/COM1B1 oraz COM1A0/COM1B0 w rejestrze TCCR1A, aby przystosować wyjscia OC mikrokontrolera do pracy z Fast PWMem w trybie nieodwracającym:

Gdy ustawiliśmy już wyjścia OC, należy uruchomić sam generator PWM. Zgodnie z tabelą 16-4 , robimy to w następujący sposób (dla Fast PWM 8-bitowego):

Jeśli potrzebujemy większej dokładności w sterowaniu obrotami silnika, możemy wybrać tryb 10-bitowy (w ten sposób obniżamy jednak częstotliwość generowanego sygnału PWM). Jedyna różnica, to wpisanie jedynki także do bitu WGM11:

Pozostało nam jeszcze tylko ustawienie prescalera, a tym samym uruchomienie timera. Ustawiając prescaler na 1 (nie dzielimy sygnału zegarowego), w 8-bitowym Fast PWMie otrzymamy częstotliwość sygnału równą około 31kHz (8000000/256/1 = 31250). Z pomocą znów rusza DS i tabela 16-5:

Tym samym kończymy konfigurację timera do generowania sygnału PWM. Cała konfiguracja przedstawia się następująco:

Wypełnienie sygnału można zmieniać wpisując interesującą nas wartość (0-255) do rejestrów OCR1A/OCR2B. Aby otrzymać sygnał o wypełnieniu 50% na pinie PB1(OC1A), wykonujemy instrukcję:

Timer - przerwanie czasowe

Wykonywanie jej w pętli głównej programu i opóźnianie za pomocą funkcji _delay_ms() nie jest dobrym rozwiązaniem - wraz z rozwojem algorytmu, będzie on potrzebował coraz więcej czasu na wykonanie, przez co w rzeczywistości częstotliwość wywoływania funkcji będzie spadać. Aby temu zapobiec i mieć pełną kontrolę nad okresem czasu między kolejnymi przejsciami pętli algorytmu, wykorzystamy Timer 0 do generowania przerwań z określoną częstotliwością.

Zapoznając się z zasadą oraz trybami działania Timera 0 opisanymi w DSie od strony 96. znajdujemy interesujące nas rozwiązanie - CTC (punkt 15.7.2 na s. 102). W trybie tym timer zlicza w górę aż do osiągnięcia zadanej wartości, po czym wywołuje przerwanie i automatycznie się zeruje. Efektem jest cyklicznie powtarzające się przerwanie w równych odstępach czasu. Opis użycia timera jak zwykle podsumowują tabelki informujące o działaniu poszczególnych rejestrów konfiguracyjnych. Z tabeli 15-8 uzyskujemy informację, jak ustawić tryb CTC:

Do poprawnego działania timera jest jeszcze wymagane ustawienie preskalera. Zakładając, że chcemy uzyskać częstotliwość wywoływania przerwania równą 100Hz, musimy wybrać jego maksymalną dostępną wartość - 1024. Pozwoli to na ustawienie częstotliwości przepełnienia licznika w zakresie 30,5 - 7812,5 Hz (taktowanie timera - 8Mhz/1024 = 7812,5Hz; może zliczać do 256 - 7812,5/256 = 30,5Hz).

Wartość maksymalną licznika, po osiągnięciu której ten będzie się zerował i generował przerwanie, wpisujemy do rejestru OCR0A. Wymaganą do uzyskania częstotliwości bliskiej 100Hz wartością jest 78 (8Mhz/1024/78 = 100,16Hz):

Ostatnią konieczną czynnością jest włączenie przerwania wywoływanego po osiągnięciu przez timer zadanej przez nas wartości. Zgodnie z punktem 15.9.6 DSa, wymaga to ustawienia bitu:

Zbierając wszystko w całość, konfiguracja Timera 0 przedstawia się następująco:

W tym momencie w odstępach 10ms uruchamiany będzie ISR TIMER0_COMPA_vect. Aby w prosty sposób sprawdzić, czy wszystko działa zgodnie z założeniami, możemy napisać testową obsługę przerwania o treści:

W efekcie dioda powinna migać z częstotliwością ~50Hz (zmiana stanu 100 razy na sekundę). Jeśli mamy oscyloskop lub multimetr pozwalający na pomiar częstotliwości - jest możliwość skonfrontowania teorii z praktyką. Aby tym razem, zgodnie z dobrą sztuką programowania mikrokontrolerów, nie wykonywać w ISRze czasochłonnych funkcji, będzie on tylko ustawiał flagę petla, a ta z kolei będzie sprawdzana w głównej pętli programu. Jeśli flaga będzie ustawiona - wywołana zostanie fukcja pętli algorytmu LFa (opisana w drugiej części artykułu), a flaga wyzerowana. Szkielet takiego programu będzie wyglądał następująco:

W naszym przypadku wywołanie czasochłonnej funkcji rob_cos() w samym ISRze nie spowodowałoby żadnych problemów. Gdyby jednak chcieć dodać np. obsługę komunikacji IR przy pomocy odbiornika typu TSOP, długa obsługa przerwania od timera mogłaby już powodować konflikty.

Przetwornik analogowy-cyfrowy

Ostatnim niezbędnym dla nas urządzeniem peryferyjnym mikrokontrolera jest ADC. Zaczynamy oczywiście od strony 252 DSa. Gdy zaznajomimy się już z zasadą jego działania, zabawę z czytaniem tabelek kontynuujemy na s. 264. W zależości od tego, jak podłączyliśmy pin VREF, ustawiamy odpowiednio bity REFS. W moim przypadku VREF został podłączony przez kondensator do masy, dlatego konfiguracja wygląda następująco:

Jeśli wystarczy nam 8 bitowa rozdzielczość wyników pozyskiwanych z ADC (a w tym przypadku będzie ona z pewnością zadowalająca), warto ustawić bit ADLAR powodujący wyrównanie 10-bitowego wyniku konwersji do lewej, dzięki czemu 8 starszych bitów wyniku uzyskamy zespośrednio z 8-bitowego rejestru ADCH.

Ponieważ, zgodnie z opisem, ADC powinno być taktowane zegarem z przedziału 50-200kHz, pomocna okazuje się tabela 24-4, która informuje nas, jak ustawić odpowiedni preskaler. Aby uzyskać wartość z wymaganego przedziału, wybieramy prescaler równy 64 (8MHz/64=125kHz):

Pozostaje nam tylko uruchomić przetwornik ustawiając bit ADEN w rejestrze ACDSRA:

Podsumowując, konfigurację ADC do naszych potrzeb można zawrzeć w dwóch linijkach:

Podsumowanie i sprawdzenie działania

Niniejszym, dzięki wsparciu dokumentacji układu oraz odrobinie intuicji, konfiguracja układów peryferyjnych dobiegła końca. Pełna konfiguracja znajduje się w funkcji init() w pliku init.c.

W tym momencie możemy już sprawdzić działanie mikrokontrolera wraz ze wszystkimi uruchomionymi peryferiami, pisząc prostą funkcję main oraz procedury obsługi przerwań o treści:

Aby sprawdzić działanie przerwania od Timera 0, ustawia ono flagę petla co 100 wywołań - w efekcie dioda będzie zapalać się i gasnąć co 1 sekundę. Po wciśnięciu przycisku, silniki zaczną powoli się obracać i zatrzymają się po ponownym jego wciśnięciu. Jeśli na tym etapie coś nie działa tak jak powinno - należy wrócić do części opisującej konfigurację danego układu i sprawdzić jej poprawność.

Aby ułatwić sobie kontrolę nad mostkiem napiszmy jeszcze funkcję, do której przekazać będzie można wartość wypełnienia w zakresie [-255;255] dla każdego z kół, a ta ustawi odpowiednio linie sterujące mostkiem:

Etykiety PWML oraz PWMP zostały zdefiniowane w pliku io_cfg.h i odpowiadają rejestrom Timera 1 służącym ustawieniu wypełnienia generowanego sygnału PWM:

Dzięki powyższej funkcji, we wcześniej napisanym programie testującym, linijki odpowiadające za wysterowanie mostka:

Możemy zastąpić łatwym w odbiorze:

Jeśli okaże się, że dla dodatnich argumentów przekazywanych do funkcji PWM() któreś z kół robota kręci się do tyłu - należy odwrócić ustawienie linii sterujących (EN_xx) w tej funkcji.

Algorytm jazdy robota

Gdy mamy już skonfigurowane wszystkie potrzebne peryferia, możemy przejść do sedna sprawy, czyli pisania właściwego algorytmu linefollowera. Zostanie od podzielony na trzy części: określenie swojej pozycji względem trasy, obliczenie błędu pozycji oraz wyliczenie wyjścia regulatora PD sterującego silnikami.

Określenie pozycji względem linii - czujniki

Podstawowym zadaniem algorytmu linefollowera jest określenie swojej pozycji, tj. odchylenia od środka linii. Pierwszym zadaniem do wykonania jest oczywiście odczyt danych pochodzących z czujników linii, którego możemy dokonać poprzez wywołanie pomiaru ADC na wejściach przetwornika odpowiadających kolejnym czujnikom.

Najprostsza funkcja zapisująca wyniki pomiaru ADC do globalnej tabeli czujniki wyglądać będzie następująco (przykład dla pięciu czujników podłączonych kolejno do kanałów 1-5 przetwornika):

Jeśli nasz robot nie ma jednak czujników podłączonych do kolejnych kanałów ADC, możemy wpisać ich numery w odpowiedniej kolejności do tablicy i wykorzystać ją rozszerzając powyższą funkcję. Tym razem dokonamy także progowania odczytanej wartości i wynik już w postaci zero-jedynkowej wpiszemy do tablicy czujniki.

W większości przypadków powyższy kod wystarczy do poprawnej obsługi czujników linii. Może się jednak okazać, że ustalony z góry próg nie zawsze będzie gwarantował poprawną pracę w przypadku zmiany toru lub jego oświetlenia. Rozwiązaniem jest kalibracja czujników przed przejazdem. Idea jest prosta - przed kalibracją ustawiamy robota tak, aby pierwszym czujnikiem stał nad białym polem, a drugim nad linią. Kalibracja polega na wykonaniu pomiaru tymi dwoma czujnikami i ustaleniu progu pośrodku otrzymanych wartości. W kodzie wyglądać to będzie następująco (zakładając, że czujnik nad białym polem jest na 1. kanale ADC, zaś czujnik widzący linię na kanale 2.):

Oczywiście jeśli wykorzystujemy taką kalibrację, musimy w poprzednio napisanej funkcji w miejscu progowania zmienić wartość 150 na zmienną prog.

Jeszcze jednym usprawnieniem wykrywania linii może być pomiar różnicowy, tj. odczyt czujników przy zgaszonej i zapalonej diodzie IR czujnika. Robot musi być jednak do tego fizycznie przystosowany - do jednego z pinów musi być dołączony tranzystor zapalający te diody. W poniższym przykładzie zakładamy, że IR_CZUJ to makro sterujące pinem odpowiedzialnym za ich włączenie.

Może się jednak okazać, że po pomiarze z zapalonymi diodami, robot przemieści się na tyle, że stan czujników zmieni się, zanim zostanie odczytany stan przy zgaszonych IRach. Myślę, że jest to kwestia badań i wypracowania pewnego kompromisu między prędkością, a jakością pomiarów. Co więcej, przy dobrych czujnikach (jak popularne KTIRy) podłączonych w optymalny sposób, pomiar różnicowy może okazać się zupełnie zbędny.

Określenie pozycji względem linii - analiza wyników

Gdy mamy już wyniki odczytu z czujników linii w postaci 1/0, możemy przystąpić do obliczenia odchylenia lub jak kto woli błędu położenia, który wykorzystamy później w regulatorze PD. Dopiero tutaj zaczynają się schody, gdyż sposobów realizacji tego zadania jest zapewne tyle, ile twórców LFów. Poniżej przedstawię kilka swoich pomysłów.

Dość prostą i jednocześnie skuteczną metodą jest przypisanie czujnikom liniowej skali wag, a następnie obliczenie średniej z wag czujników, które wykryły linię. Przykładowo dla siedmiu czujników skala wag może wyglądać tak: -30, -20, -10, 0, 10, 20, 30.

W kodzie taki sposób liczenia błędu (dla siedmiu czujników) przedstawić można następująco (zakładamy, że tabela czujniki zawiera już dane z sensorów linii):

Alternatywnym rozwiązaniem może być przypisanie na sztywno wag do poszczególnych czujników. Może się to przydać, jeśli są one rozmieszczone nieliniowo lub korzystamy z wysuniętych sensorów do wykrywania kątów prostych. Do tego celu potrzebna będzie globalna tablica zawierająca odpowiednie wagi, a poprzednia funkcja liczenia błędu przekształci się w:

W dalszej części artykułu pozostanę jednak przy poprzednim rozwiązaniu - wydaje mi się łatwiejsze do ewentualnej modyfikacji.

W trakcie walki o coraz większą prędkość naszego robota przyjdzie moment, w którym nie będzie on w stanie wykonać skrętu na czas i wypadnie czujnikami poza trasę (taka sytuacja zajdzie także w przypadku kąta prostego na trasie). Najłatwiejszym sposobem na poradzenie sobie z tym problemem jest użycie ostatnio zapamiętanej wartości błędu lub przyznanie takiej sytuacji jeszcze większej wagi, niż skrajny czujnik.

Pierwsze z powyższych rozwiązań jest proste w implementacji - zapamiętujemy aktualnie wyliczony błąd i w razie potrzeby wykorzystujemy go w następnym kroku:

Prawdopodobnie jeszcze lepszym rozwiązaniem jest przypisanie do stanu "bezliniowego" odpowiedniej wagi. Dzięki temu możemy jeszcze sprawniej zareagować na wypadnięcie z linii, a także bezproblemowo pokonać odcinek bez linii występujący na trasach LF Enhanced.

W tym przypadku także przyda nam się zapamiętany poprzedni błąd - na jego podstawie możliwe będzie oszacowanie, z której strony robota znajduje się linia.

Ostatnim pomysłem, który chciałbym przedstawić, jest dodanie flagi przestrzelenia zakrętu. Ponieważ w momencie wypadnięcia poza trasę ustalony przez nas błąd ma dużą wartość, a co za tym idzie korekcja ruchu wykonana przez robota będzie bardzo gwałtowna, po powrocie na linię robot może z rozpędu wypaść na drugą stronę trasy. Aby uniknąć tego zjawiska, warto odpowiednio zmniejszyć wagi czujników na czas powrotu robota na linię.

Przykładowo z poprzednich wartości błędu od -30 do 30 można zejść do przedziału [-15;15]. Flaga przestrzelenia zakrętu będzie zerowana po osiągnięciu pozycji zero, tj. po ustawieniu się na środku linii.

Podane przeze mnie wartości wag są oczywiście przykładowe i z pewnością będą się różnić dla różnych robotów. Dobrym pomysłem może też być ich uzależnienie od maksymalnej dozwolonej prędkości robota.

Może się także okazać, że "obsługa przestrzelenia" nie będzie zawsze dobrze działać dla danego robota, lub da się ją zrealizować w lepszy sposób. To jest właśnie miejsce, gdzie dłuższa chwila zastanowienia i duże ilości prób mogą dać wymierne rezultaty - lepsze czasy przejazdów.

Regulator PD

Ostatnią ważną częścią programu jest regulator, który w zależności od obliczonego błędu położenia wyznaczy odpowiednie wypełnienie sygnału PWM podawanego na silniki. W naszym przypadku wybór pada na regulator PD. Jest to podyktowane prostotą jego implementacji, niezbyt wysoką trudnością regulacji oraz zadowalającymi wynikami w tym zastosowaniu.

Ponieważ jest to poradnik programowania, nie będę rozwodził się na temat teorii i zasad działania regulatora i przejdę od razu do jego tworzenia. Pseudokod regulatora PD w formie, w której będziemy z niego korzystać, przedstawia się następująco:

Przez błąd (error) rozumiemy różnicę między stanem zadanym (setpoint), a rzeczywistem stanem obiektu (process_feedback).

W przypadku LFa badanym stanem obiektu jest odchylenie względem linii. Ponieważ wartość zadana w tym przypadku zawsze równa będzie 0 (chcemy w końcu jechać równo po linii), błąd w rozumieniu regulatora będziemy pozyskiwali bezpośrednio z napisanej poprzednio funkcji.

Ponieważ funkcja regulatora będzie cyklicznie uruchamiana przez przerwanie w równych odstępach czasu, przyjmujemy także wartość dt, czyli upływ czasu, za stałą i równą 1 - pozwoli to uniknąć dzielenia w członie różniczkującym (derivative).

Napisany w C dyskretny regulator PD dla potrzeb linefollowera będzie więc wyglądał tak:

Zmienna regulacja stanowi wyjście regulatora i zawiera informację o tym, jak należy zareagować, aby błąd zmierzał do zera, tj. aby robot utrzymywał się na linii. Nie pozostało nam więc nic innego, jak nakazać silnikom jazdę z uwzględnieniem wymaganej korekty:

V_zad jest prędkością podstawową, z jaką kręcić się mają silniki. Jeśli do tej wartości odpowienio dla lewego i prawego koła dodamy/odejmiemy wartość regulacji wyliczoną przez PD, robot będzie trzymał się linii. Aby to się stało, potrzebne jest jeszcze tylko ustawienie regulatora, o czym będzie mowa za chwilę.

Podsumowując, napisana przez nas pętla regulacji składa się z:

  • odczytu z czujników
  • obliczenia błędu
  • obliczenia regulacji
  • wysterowania silników

Składając w całość napisane przez nas funkcje, otrzymujemy kompletny algorytm jazdy LFa:

Ustawienie regulatora PD

Ostatnią rzeczą, jaką musimy wykonać, aby nasz robot zgrabnie poruszał się po trasie, jest ustawienie regulatora. Wbrew pozorom, jest to dość proste zadanie. Zaczynamy od ustawienia współczynnika Kd na zero i stopniowego zwiększania parametru Kp. Aby ułatwić sobie to zadanie, możemy oprogramować przycisk robota, aby zwiększał wartość Kp np. o 1 za każdym naciśnięciem.

Gdy robot zacznie oscylować, czyli zauważalnie "bujać się" na boki w trakcie jazdy po linii, zmniejszamy wartość Kp o połowę. Następnie, już przy stałym Kp, zwiększamy stopniowo Kd do momentu osiągnięcia zadowalających efektów (oscylacje po wejściu w silny zakręt oraz czas potrzebny do równego ustawienia się na linii będą maleć wraz ze wzrostem Kd).

Podsumowanie

W załączniku znajdują się pliki źródłowe z kompletnym kodem, wraz z większością przedstawionych rozwiązań. Oprócz tego, umieściłem tam wersję minimalistyczną - najprostszy kod, który pozwoli na jazdę po linii - oraz "tester konfiguracji" przedstawiony na końcu części traktującej o inicjalizacji układów peryferyjnych.

Implementacja regulatora PD przedstawiona przeze mnie jest najprostszą z możliwych. Z pewnością sprawdzi się w średniozaawansowanych konstrukcjach, lecz prawdopodobnie będzie wymagała rozwinięcia w przypadku robotów walczących o podium na zawodach. Zagadnienie to jest bardzo dobrze opisane choćby na polskiej Wikipedii.

Jako że artykuł znacznie rozrósł się w trakcie pisania, mogły się do niego wkraść pewne błędy, za które z góry przepraszam i proszę o ich wytknięcie. Niemniej mam nadzieję, że pomoże on szerokiemu gronu użytkowników w łagodnym wejściu do świata AVRów programowanych w C, a także pokaże, jak ważnym i łatwym w użyciu narzędziem jest dokumentacja używanego układu.

Załączniki

KodyZrodlowe (zip, 9 KB)

linefollower, PD, PID, regulator

Trwa ładowanie komentarzy...