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:

#define LED_DIR                REGISTER_BIT(DDRB,0) 
#define LED                    REGISTER_BIT(PORTB,0)

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:

LED_DIR = 1;            // wpisuje jedynkę do bitu 0. rejestru DDRB

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:

LED = 1;

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

LED ^= 1;

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:

#define SW1                    REGISTER_BIT(PORTD,3) 
#define SW1_DIR                REGISTER_BIT(DDRD,3) 
#define SW1_PIN                REGISTER_BIT(PIND,3)

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

foo = SW1_PIN;

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):

#define INPUT 0 
#define OUTPUT 1 

EN_11_DIR = OUTPUT; 
EN_11 = 0; 
EN_12_DIR = OUTPUT; 
EN_12 = 0; 
EN_21_DIR = OUTPUT; 
EN_21 = 0; 
EN_22_DIR = OUTPUT; 
EN_22 = 0; 
PWM1_DIR = OUTPUT; 
PWM1 = 0; 
PWM2_DIR = OUTPUT; 
PWM2 = 0; 
LED_DIR = OUTPUT; 
LED = 0; 
SW1_DIR = INPUT; 
SW1 = 1;                     // podciągnięcie wejścia do VCC (SW1 odpowiada bitowi 3. w PORTD)

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

DDRB |= _BV(0);

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

PORTB |= _BV(0); 

PORTB &= ~_BV(0); 

PORTB ^= _BV(0); 
lub 
PINB |= _BV(0);             // punkt 14.2.2 DS

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

DDRD |= _BV(5); 
PORTD &= ~_BV(5); 
DDRB |= _BV(7); 
PORTB &= ~_BV(7); 
DDRD |= _BV(6); 
PORTD &= ~_BV(6); 
DDRD |= _BV(7); 
PORTD &= ~_BV(7); 
DDRD |= _BV(5); 
PORTD &= ~_BV(5); 
DDRD |= _BV(5); 
PORTD &= ~_BV(5); 
DDRB |= _BV(0); 
PORTB &= ~_BV(0); 
DDRD |= _BV(3); 
PORTD |= _BV(3);

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:

PCICR |= _BV(PCIE2); 
PCMSK2 |= _BV(PCINT19);

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

sei();

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:

volatile int jazda = 0;            // globalna zmienna/flaga zero-jedynkowa 
ISR(PCINT2_vect) 
{ 
    PCMSK2 &= ~_BV(PCINT19);       // wyłączamy obsługę przerwania na PD3 - drgania styków spowodowałyby wielokrotne jego wywołanie 
    _delay_ms(50);                // czekamy na ustanie drgań styków 
    
    jazda ^= 1;                    // zmieniamy stan flagi 
    
    PCMSK2 |= _BV(PCINT19);        // włączamy z powrotem obsługę przerwania 
}

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:

TCCR1A |= _BV(COM1A1) | _BV(COM1B1)

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):

TCCR1A |= _BV(WGM10); 
TCCR1B |= _BV(WGM12);

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:

TCCR1A |= _BV(WGM10) | _BV(WGM11); 
TCCR1B |= _BV(WGM12);

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:

TCCR1B = _BV(CS10);

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

TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM10); 
TCCR1B = _BV(CS10) | _BV(WGM12);

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ę:

OCR1A = 127;

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:

TCCR0A |= _BV(WGM01);

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).

TCCR0B |= _BV(CS02) | _BV(CS00);

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):

OCR0A = 78;

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:

TIMSK0 |= _BV(OCIE0A);

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

TCCR0A |= _BV(WGM01); 
TCCR0B |= _BV(CS02) | _BV(CS00); 
OCR0A = 78; 
TIMSK0 |= _BV(OCIE0A);

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:

ISR(TIMER0_COMPA_vect) 
{ 
    LED ^= 1; 
}

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:

volatile int petla = 0;    // globalna flaga informująca o konieczności wywołania pętli algorytmu 
int main() 
{ 
    while(1) 
    { 
        if(petla) 
        { 
            rob_cos(); 
            petla = 0; 
        } 
    } 
} 

ISR(TIMER0_COMPA_vect) 
{ 
    petla = 1; 
}

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:

ADMUX |=  _BV(REFS0);

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.

ADMUX |= _BV(ADLAR);

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):

ADCSRA |= _BV(ADPS2) | _BV(ADPS1);

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

ADCSRA |= _BV(ADEN);

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

ADCSRA |= _BV(ADPS2) | _BV(ADPS1) | _BV(ADEN); 
ADMUX |= _BV(ADLAR) | _BV(REFS0);

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:

volatile int jazda = 0, petla = 0, licznik = 0; 
int main(void) 
{ 
    init();                        // inicjalizacja napisana przed chwilą 
    LED = 1;                        // włączamy LEDa 
    EN_12 = EN_22 = 1;            // ustawiamy piny sterujące mostkiem H 
    EN_11 = EN_21 = 0; 

    while(1)                        // nieskończona pętla 
    { 
        if(jazda) 
            OCR1A = OCR1B = 50;    // flaga jazda równa 1 - ustawiamy niskie wypełnienie PWM 
        else 
            OCR1A = OCR1B = 0;    // flaga jazda równa 0 - wyłączamy silniki 
            
        if(petla)                  // flaga petla rowna 1 - przerwanie od Timera 0 wywołało się 100 razy 
        { 
            LED ^= 1; 
            petla = 0; 
        } 
    } 
} 

ISR(PCINT2_vect) 
{ 
    PCMSK2 &= ~_BV(PCINT19);    // wyłączamy obsługę przerwania na PD3 - drgania styków spowodowałyby wielokrotne jego wywołanie 
    _delay_ms(50);                // czekamy na ustanie drgań styków 
    
    jazda ^= 1;                    // zmieniamy stan flagi 
    
    PCMSK2 |= _BV(PCINT19);        // włączamy z powrotem obsługę przerwania 
} 

ISR(TIMER0_COMPA_vect) 
{ 
    licznik++; 
    if(licznik == 100) 
    { 
        licznik = 0; 
        petla = 1; 
    } 
}

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:

void PWM(int lewy, int prawy) 
{ 
    if(lewy >= 0) 
    { 
        if(lewy>255) 
            lewy = 255; 
        EN_12 = 1; 
        EN_11 = 0; 
    } 
    else 
    { 
        if(lewy<-255) 
            lewy = -255; 
        EN_12 = 0; 
        EN_11 = 1;    
    } 
    
    if(prawy >= 0) 
    { 
        if(prawy>255) 
            prawy = 255; 
        EN_22 = 1; 
        EN_21 = 0; 
    } 
    else 
    { 
        if(prawy<-255) 
            prawy = -255; 
        EN_22 = 0; 
        EN_21 = 1;    
    } 
    
    PWML = abs(lewy); 
    PWMP = abs(prawy); 
}

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:

#define PWML OCR1A 
#define PWMP OCR1B

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

EN_12 = EN_22 = 1; 
EN_11 = EN_21 = 0; 
OCR1A = OCR1B = 50;

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

PWM(50,50);

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):

void czytaj_adc() 
{ 
    for(int i=1; i<6; i++) 
    { 
        ADMUX &= 0b11100000;            // zerujemy bity MUX odpowiedzialne za wybór kanału (s. 255 w DS) 
        ADMUX |= i;                      // wybieramy kanał przetwornika 
        ADCSRA |= _BV(ADSC);            // uruchamiamy pomiar 
        while(ADCSRA & _BV(ADSC)) {};    // czekamy aż skończy się pomiar 
        
        czujniki[i-1] = ADCH;            // odczyt 8 starszych bitów wyniku; indeks [i-1] ponieważ zaczynamy z i == 1 
    } 
}

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.

int tab_czujnikow[7] = {5,4,3,2,1,0,7}; 
void czytaj_adc() 
{ 
    for(int i=0; i<7; i++) 
    { 
        ADMUX &= 0b11100000; 
        ADMUX |= tab_czujnikow[i]; 
        ADCSRA |= _BV(ADSC); 
        while(ADCSRA & _BV(ADSC)) {}; 
        
        if(ADCH > 150)                    // odczyt 8 starszych bitów i progowanie; próg = 150 
            czujniki[i] = 1; 
        else 
            czujniki[i] = 0; 
    } 
}

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.):

int prog = 0;                        // globalna zmienna przechowująca wartość progu 
void kalibracja() 
{ 
    int bialy, czarny; 
    
    ADMUX &= 0b11100000; 
    ADMUX |= 1; 
    ADCSRA |= _BV(ADSC); 
    while(ADCSRA & _BV(ADSC)) {}; 
    bialy = ADCH;                        // odczyt z czujnika nad białym polem 
    
    ADMUX &= 0b11100000; 
    ADMUX |= 2; 
    ADCSRA |= _BV(ADSC); 
    while(ADCSRA & _BV(ADSC)) {}; 
    czarny = ADCH;                        // odczyt z czujnika nad linią 
    
    prog = bialy + (czarny-bialy)/2;    // ustalenie progu pośrodku zmierzonych wartości 
}

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.

int tab_czujnikow[7] = {5,4,3,2,1,0,7}; 
void czytaj_adc() 
{ 
    IR_CZUJ = 1;                          // włączenie diod IR 
    _delay_ns(10);                        // może okazać się niezbędne - tranzystor potrzebuje chwili na otwarcie, a diody na zaświecenie 
    for(int i=0; i<7; i++) 
    { 
        ADMUX &= 0b11100000; 
        ADMUX |= tab_czujnikow[i]; 
        ADCSRA |= _BV(ADSC); 
        while(ADCSRA & _BV(ADSC)) {}; 
        
        czujniki[i] = ADCH;                // odczyt wyniku przy zapalonej diodzie 
    } 
    
    IR_CZUJ = 0;                          // wyłączenie diod IR 
    for(int i=0; i<7; i++) 
    { 
        ADMUX &= 0b11100000; 
        ADMUX |= tab_czujnikow[i]; 
        ADCSRA |= _BV(ADSC); 
        while(ADCSRA & _BV(ADSC)) {}; 
                
        if(czujniki[i] - ADCH > prog)    // odjęcie wyniku przy zgaszonych diodach i progowanie 
            czujniki[i] = 1; 
        else 
            czujniki[i] = 0; 
    } 
}

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):

int licz_blad() 
{ 
    int err = 0;                          // wartość błędu 
    int ilosc = 0;                        // ilość czujników z wykrytą linią 
    
    for(int i=0; i<7; i++) 
    { 
        err += czujniki[i]*(i-3)*10;    // wagi kolejnych czujników (dla i z zakresu [0;7]): -30, -20, -10, 0, 10, 20, 30 
        ilosc += czujniki[i];            // czujniki[i] ma wartość 1/0 
    } 
    
    err /= ilosc;                        // liczymy średnią wagę; 
    
    return err; 
}

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:

int wagi[7] = {-50, -20, -10, 0, 10, 20, 50}; 
int licz_blad() 
{ 
    int err = 0;                          // wartość błędu 
    int ilosc = 0;                        // ilość czujników z wykrytą linią 
    
    for(int i=0; i<7; i++) 
    { 
        err += czujniki[i]*wagi[i];        // wagi kolejnych czujników (dla i z zakresu [0;7]): -30, -20, -10, 0, 10, 20, 30 
        ilosc += czujniki[i];            // czujniki[i] ma wartość 1/0 
    } 
    
    err /= ilosc;                        // liczymy średnią wagę; 
    
    return err; 
}

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:

int prev_err = 0;                // globalna zmienna z zapisanym ostatnim błędem 
int licz_blad() 
{ 
    int err = 0; 
    int ilosc = 0; 
    
    for(int i=0; i<7; i++) 
    { 
        err += czujniki[i]*(i-3)*10; 
        ilosc += czujniki[i]; 
    } 
    
    if(ilosc != 0)                    // czujniki wykryły linię 
    { 
        err /= ilosc;                  // liczymy i zapisujemy aktualny błąd 
        prev_err = err; 
    } 
    else                               // czujniki nie wykryły linii 
        err = prev_err;                // używamy zapisanego poprzedniego błędu 
    
    return err; 
}

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.

int prev_err = 0; 
int licz_blad() 
{ 
    int err = 0; 
    int ilosc = 0; 
    
    for(int i=0; i<7; i++) 
    { 
        err += czujniki[i]*(i-3)*10; 
        ilosc += czujniki[i]; 
    } 
    
    if(ilosc != 0) 
    { 
        err /= ilosc; 
        prev_err = err; 
    } 
    else 
    { 
        if(prev_err < -20)                // linia ostatanio widziana po lewej stronie - ustalamy ujemny błąd większy od błędu skrajnego lewego czujnika 
            err = -40; 
        else if(prev_err > 20)            // linia ostatanio widziana po prawej stronie - analogicznie do powyższego 
            err = 40; 
        else                            // przerwa w linii - trzeba jechać prosto 
            err = 0; 
    } 
    
    return err; 
}

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.

int prev_err = 0; 
int przestrzelony = 0; 
int licz_blad() 
{ 
    int err = 0; 
    int ilosc = 0; 
    
    int waga = 10;                        // współczynnik wagi czujników ustawiany w zależności od przestrzelenia; poprzednio stały i równy 10 
    
    if(przestrzelony)                    // zmniejszenie wag czujników w przypadku przestrzelenia zakrętu 
        waga = 5; 
    
    for(int i=0; i<7; i++) 
    { 
        err += czujniki[i]*(i-3)*waga; 
        ilosc += czujniki[i]; 
    } 
    
    if(ilosc != 0) 
    { 
        err /= ilosc; 
        prev_err = err; 
    } 
    else 
    { 
        if(prev_err < -20) 
        { 
            err = -40; 
            przestrzelony = 1;                // ustawienie flagi - przestrzelony, linia po lewej 
        } 
        else if(prev_err > 20) 
        { 
            err = 40; 
            przestrzelony = 2;                // ustawienie flagi - przestrzelony, linia po prawej 
        } 
        else 
            err = 0; 
    } 
    
    if(przestrzelony == 1 && err >= 0)        // zerowanie flagi przestrzelenia zakrętu po powrocie na środek linii 
        przestrzelony = 0; 
    else if(przestrzelony == 2 && err <= 0) 
        przestrzelony = 0; 
    
    return err; 
}

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:

start: 
    wait(dt) 
    error = setpoint - process_feedback 
    derivative = (error - previous_error)/dt 
    output = (Kp*error) + (Kd*derivative) 
    previous_error = error 
    goto start

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:

int blad, pop_blad, Kp, Kd;    // wymagane zmienne globalne 
int PD() 
{ 
    //zmienna blad zawiera aktualny wynik fukcji licz_blad() 
    int rozniczka = blad - pop_blad; 
    pop_blad = blad; 
    return Kp*blad + Kd*rozniczka; 
}

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:

int V_zad = 100; 
PWM(V_zad + regulacja, V_zad - regulacja);

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:

int tab_czujnikow[7] = {5,4,3,2,1,0,7}, czujniki[7] = {0}; 
int prev_err = 0, przestrzelony = 0; 
int blad, pop_blad = 0, Kp = 1, Kd = 0; 
int V_zad = 100; 

void petla_LF() 
{ 
    czytaj_adc(); 
    blad = licz_blad(); 
    int regulacja = PD(); 
    PWM(V_zad + regulacja, V_zad - regulacja); 
}

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

linefollower, PD, PID, regulator

Trwa ładowanie komentarzy...