Joonejärgimine 3pi robotiga

QTR andurite lugemisel on vaja eelnevalt andurid initsialiseerida. Vastav funktsioon on “pololu_3pi_init(2000)”, mis seadistab andurite lugemise viite Timer2 klikkide arvu järgi ( soovituslik vahemik 1000-4000). ATmega328p mikrokontrolleri töötab taktsagedusel 20Mhz, kuid Timer2 taktsagedus on jagatud 8-ga, mis teeb 2,5Mhz. Arvutuslikult üks Timer2 klikk kestab T = 1/2500000 = 0,4us. Järelikult 2000 klikki annab 2000*0,4us = 800us = 0,8ms. Selle aja jooksul mõõdab mikrokontroller iga anduri madalasse (LOW ehk 0) olekusse jõudmise aega. Mida rohkem valgust peegeldub pinnalt seda kiiremini saabub anduri madal olek. Kui 0,8ms jooksul madalat olekut ei saabunud, siis tuvastab mikrokontroller selle siiski kõige tumedamaks pinnaks. Andurite lugemise funktsioon on “read_line(sensors,IR_EMITTERS_ON)”, mis ootab parameetriteks andurite lugemite salvestamiseks viie elemendiga massiivi (nt. unsigned int sensors[5]) ja lugemise režiimi (nt. IR_EMITTERS_ON). Enne andurite lugemise implementeerimist on soovituslik teostada ka kalibreerimine funktsiooniga “calibrate_line_sensors(IR_EMITTERS_ON)”. Funktsioon leiab iga anduri jaoks kõige heledama ja kõige tumedama pinna väärtuse. Funktsiooni tuleb kasutada mitu korda, et kõik andurid oleks olnud nii valgel taustal kui ka mustal joonel. Soovituslik on kalibreerimine automatiseerida, kuna parima tulemuse saamiseks tuleb seda teostada enne igat sõitu.

Andurite lugemise funktsioon (read_line(sensors,IR_EMITTERS_ON)) tagastab kaalutud keskmise andurite väärtuse vastavalt valemile:

  0*value0 + 1000*value1 + 2000*value2 + ...
 --------------------------------------------
       value0  +  value1  +  value2 + ...

Funktsioon loeb iga anduri väärtuse vahemikus 0 kuni 1000, mis on vastavalt kalibratsioonile kõige heledama ja kõige tumedama pinna väärtusele vastavusse seatud. Edasi korrutatakse iga anduri väärtus läbi vastava kaaluteguriga. Alustades kõige vasakpoolseimast andurist on kaalutegurid vastavalt 0, 1000, 2000 jne. Lõpuks jagatakse kõigi kaaluteguritega korrutatud andurite summa veel kõigi andurite summaga. Valemi tulemuseks on väärtus vahemikus 0 kuni 4000. Seega kui ainult kõige vasakpoolsem andur näeb musta joont, siis on tagastatav väärtus 0. Vasakult kolmanda anduri puhul on see 2000 ja kõige parempoolsema korral 4000. Valemi hea omadus on see, et joone positsioon on paremini määratav, sest teada on ka olekud kui must joon on kahe anduri vahepealses osas. Lisaks kui tekib olukord et näeb rohkem kui üks andur siis valem leiab kõigi andurite summa järgi umbkaudse joone asukoha.

Tegemist on Pololu poolt koostatud näiteprogrammiga, mis kasutab lihtsat algoritmi joone järgimiseks. Algoritm vajab töötamiseks musta joont valgel taustal. Programm tutvustab automaatset andurite kalibreerimist, infrapunaandurite kasutamist, mootorite juhtimist, LCD-le kirjutamist ja helide mängimist piezo heligeneraatorist. Tegemist on hea alusega, millest saab edasi arendada juba keerukama algoritmi, mis kasutab PID kontrollerit. Alloleval pildil on näidatud ära joonejärgimise algoritmi tööpõhimõte. 3pi robotil on 5 andurit, aga lihtsuse mõttes vajab algoritm neist ainult 3-e keskmist. Algoritmil on kolm olekut olenevalt roboti andurite asetsemisest joone suhtes.

Programmi initsialisseerimise funktsioon on hästi pikk ja lohisev, kuid ei tasu selle osas ära ehmatada kui kõike kohe ei mõista. Targem on initsialiseerimise funktsiooni mitte muuta, kui ei tea täpselt mida teed. Põhiline joonejärgimise algoritm jookseb aga main() funktsiooni while tsüklis, mis asub programmi alumises osas.

<private> Joonejärgimise algoritmi kohta saab rohkem lugeda Pololu näidisprojektist: 3Pi line following.

#include <pololu/3pi.h>
 
// pgmspace.h teek lubab andmeid salvestada mikrokontrolleri programmimällu (Flash)
#include <avr/pgmspace.h>
 
// Tutvustavad tekstid (massiivid)
// "PROGMEM" kasutamine salvestab tekstid Flash mällu
const char welcome_line1[] PROGMEM = " Pololu";
const char welcome_line2[] PROGMEM = "3\xf7 Robot";
const char demo_name_line1[] PROGMEM = "Line";
const char demo_name_line2[] PROGMEM = "follower";
 
// Mõned lihtsate meloodiate massiivid
const char welcome[] PROGMEM = ">g32>>c32";
const char go[] PROGMEM = "L16 cdegreg4";
 
// Erisümbolite tekitamiseks vajalikke andmeid sisaldav massiiv
const char levels[] PROGMEM = {
    0b00000,
    0b00000,
    0b00000,
    0b00000,
    0b00000,
    0b00000,
    0b00000,
    0b11111,
    0b11111,
    0b11111,
    0b11111,
    0b11111,
    0b11111,
    0b11111
};
 
// Funktsioon laeb tekitatud erisümboli LCD-le
// Kasutab 7 sümbolit, mis on erineva kõrgusega tulbad
void load_custom_characters()
{
    lcd_load_custom_character(levels+0,0); // tase 1
    lcd_load_custom_character(levels+1,1); // tase 2
    lcd_load_custom_character(levels+2,2); // jne...
    lcd_load_custom_character(levels+3,3);
    lcd_load_custom_character(levels+4,4);
    lcd_load_custom_character(levels+5,5);
    lcd_load_custom_character(levels+6,6);
    clear(); // LCD tuleb tühjendada, et sümbolid toimima hakkaks
}
 
// Funktsioon kuvab loodud erilise sümboli ekraanile vastavalt anduri väärtusele
void display_readings(const unsigned int *calibrated_values)
{
    unsigned char i;
 
    for(i=0;i<5;i++) {
        // Sümbolite massiivi algväärtustamine
        const char display_characters[10] = {' ',0,0,1,2,3,4,5,6,255};
 
        // Muutujale c omistatakse väärtusi vahemikus 0 kuni 9
        // Kalibreeritud andurite väärtused on vahemikus 0 kuni 1000
        // 1000/101 = 9 kuna kasutatakse täisarvulist muutujat
        char c = display_characters[calibrated_values[i]/101];
 
        // Kuvab loodud tulba sümboli LCD-le
        print_character(c);
    }
}
 
// Algväärtustab 3pi roboti funktsioonid ja mängib lühikese meloodia
void initialize()
{
    unsigned int counter; // Kasutatakse lihtsa taimerina
    unsigned int sensors[5]; // Hoiab andurite väärtusi meeles
 
    // 3pi roboti andurite lugemise seadistamine
    // Kasutab väärtust 2000, mis määrab aja, mille jooksul võetakse üks lugem
    // Vastavalt 2000*0.4 us = 0.8 ms, 20 MHz juures töötaval mikrokontrolleril
    pololu_3pi_init(2000);
    load_custom_characters(); // laeb loodud erisümbolid LCD-le
 
    // Mängib muusikat ja kuvab tervitussõnumid LCD-l
    print_from_program_space(welcome_line1);
    lcd_goto_xy(0,1);
    print_from_program_space(welcome_line2);
    play_from_program_space(welcome);
    delay_ms(1000);
 
    clear();
    print_from_program_space(demo_name_line1);
    lcd_goto_xy(0,1);
    print_from_program_space(demo_name_line2);
    delay_ms(1000);
 
    // Kuvab akude pinge millivoltides ja ootab nupu B vajutamist
    while(!button_is_pressed(BUTTON_B))
    {
        int bat = read_battery_millivolts();
        clear();
        print_long(bat);
        print("mV");
        lcd_goto_xy(0,1);
        print("Press B");
        delay_ms(100);
    }
 
    // Ootab nupu vabastamist, et robot nupu vajutamisel kohe liikuma ei hakkaks
    wait_for_button_release(BUTTON_B);
    delay_ms(1000);
 
    // Automaatne andurite kalibreerimine paremale ja vasakule pööramise teel
    for(counter=0;counter<80;counter++)
    {
        if(counter < 20 || counter >= 60)
            set_motors(40,-40);
        else
            set_motors(-40,40);
 
        // Salvestab iga anduri jaoks minimaalse ja maksimaalse väärtuse,
        // et kõigi andurite puhul oleks joone tuvastamine võrdväärne.
        // IR_EMITTERS_ON argument määrab, et infrapuna LED-id anduril
        // põlevad kalibreerimise ajal.
        calibrate_line_sensors(IR_EMITTERS_ON);
        // Mõõtmise kestus on 80*20 = 1600 ms
        delay_ms(20);
    }
    set_motors(0,0);
 
    // Kuvab kalibreeritud andurite väärtused tulpadena
    while(!button_is_pressed(BUTTON_B))
    {
        // Loeb andurite väärtused massiivi ja arvutab välja positsiooni
        unsigned int position = read_line(sensors,IR_EMITTERS_ON);
 
        // Kuvab arvutatud positsiooni väärtuse, mis jääb vahemikku 0 - 4000
        // Väärtus on 0 kui ainult kõige vasakpoolsem andur näeb joont 
        // Väärtus on 4000 kui ainult kõige parempoolsem andur näeb joont
        clear();
        print_long(position);
        lcd_goto_xy(0,1);
        display_readings(sensors);
        delay_ms(100);
    }
    wait_for_button_release(BUTTON_B);
    clear();
    print("Go!");       
 
    // Mängib meloodiat ja ootab kuni meloodia lõppeb
    play_from_program_space(go);
    while(is_playing());
}
 
// Peaprogrammi algus
int main()
{
    unsigned int sensors[5]; // Massiiv andurite väärtuste mälus hoidmiseks
 
    initialize(); // Algväärtustamise funktsioon
 
    // Lõputu tsükkel
    while(1)
    {
        // Tagastab andurite väärtustest arvutatud positsiooni 
        unsigned int position = read_line(sensors,IR_EMITTERS_ON);
 
        if(position < 1000)
        {
            // Robot on joonelt liiga paremale kaldunud
 
            // Parempoolne mootor saab kiiruse 100 ja vasak pannakse seisma,
            // et robot pööraks vasakule (mootori väärtused -255 kuni 255)
            set_motors(0,100);
 
            // Näitab põhja all oleva kahe LED tulega olekut (0-väljas, 1-sees)
            left_led(1);  // Punane LED
            right_led(0);  // Roheline LED
        }
        else if(position < 3000)
        {
            // Robot on üsna keskel ja mõlemad mootorid saavad kiiruse 100
            // ning robot sõidab otse
            set_motors(100,100);
            left_led(1);  // Punane LED
            right_led(1); // Roheline LED
        }
        else
        {
            // Robot on joonelt liiga vasakule kaldunud
            // Vasakpoolne mootor saab kiiruse 100 ja parem pannakse seisma,
            // et robot pööraks paremale
            set_motors(100,0);
            left_led(0);  // Punane LED
            right_led(1);  // Roheline LED
        }
    }
}

</private>