RCK Ruđera Boškovića - mobilna / uslužna robotika

Priručnik - Proračun robota

Odabir motora mobilnog robota

Osnovne pretpostavke

Odabir motora mobilnog robota može se provesti tako da se odabere motor bez prethodnog proračuna te izradi se robot za kojeg ne znamo koje su njegove mobilne karakteristike.

Nadalje pokusima možemo utvrditi koje su karakteristike robota i da li su ispunjena naša očekivanja. Takvim pristupom ne ostvarujemo svoje zahtjeve nego se pomirimo s karakteristikama robota koje smo dobili.

Drugi način je provesti jednostavan proračun motora koji nam omogućuje precizniji odabir motora prema našim zahtjevima.

Prilikom izbora istosmjernog motora za pogon mobilnog robota potrebno je definirati:

Maksimalnu brzinu kretanja robota
Maksimalnu akceleraciju robota
Uzbrdicu koju taj robot mora savladati
Pretpostaviti masu robota
Odrediti promjer pogonskih kotača

Pretpostavimo da robot radi u režimu predstavljenom na Slici 2. Sa Slike 2.(Profil gibanja) vidi se da robot mora ubrzavati do maksimalne brzine od 0,8m/s za jednu sekundu, zatim se kreće postignutom brzinom od 0,8ms (jednoliko gibanje) 12 sekundi te na kraju uspori do stanja mirovanja za 1,5 sekundi.

Na Slici 2. (Profil podloge) daje nam uvid u nagib podloge kojom se giba izražen u [%].

Na primjer nagib 20% znači da će nakon prevaljenih 100m horizontalnog puta savladati uzbrdicu od 20m Slika 1.

Slika 1.

Karakteristike robota:

m=3Kg

r=7cm (promjer kotača)

Profil gibanja

Profil podloge

Slika 2.

Izračun brzine vrtnje kotača robota

Tehnički podatak brzine vrtnje motora iskazuje se mjernom jedinicom [o/min] okretaji u minuti. Da bi robot postigao zadanu brzinu kotači robota moraju se vrtjeti određenom brzinom vrtnje. Brzina kretanja robota i brzina vrtnje kotača ovisi o promjeru kotača , odnosno o opsegu kotača.

O= π ∙ d = 3,14 ∙ 0,07 = 0,2198 ≈ 0,22 [m]

Za jedan okret kotača robot se pomakne za 0,22m.

Nadalje izračunavamo broj okretaja kotača koji moramo postići da bi se robot kretao zahtijevanom brzinom od 0,8m/s. Kada podijelite maksimalnu brzinu sa opsegom kotača dobijemo broj okretaja u sekundi da bi robot ostvario zadanu brzinu. U tehničkim specifikacijama motora i reduktora broj okretaja je izražen u okretajima u minuti. Pošto jedna minuta ima 60 sekundi tada kutnu brzinu pomnožimo sa brojem sekundi u minuti i dobivamo broj okretaja u minuti.

Izračun momenta

Zadane profile gibanja i podloge možemo podijeliti na segmente u kojima se mijenja ili profil gibanja ili profil podloge Slika 3. Vidljivo je da se mijenja opterećenje, odnosno moment ukoliko se promijeni jedan od profila.

Slika 3.

Za vrijeme kretanja konstantnom brzinom motor razvija moment potreban za savladavanje trenja, a to je na Slici 3. moment M4.

Moment trenja

M₄ = M_tr = μ∙m∙g∙r = 0,05∙3∙10∙0,035 = 0,053 [Nm]

gdje je μ – koeficijent trenja kotrljanja između podloge i kotača robota procijenimo na 0,05

g – gravitacija (zaokruženo na 10m/s²)

Moment potreban za ubrzavanje ili kočenje je:

Moment ubrzanja:

Moment kočenja (različit je od momenta ubrzanja zbog razlike u vremenu promjene i smjeru):

Moment uzbrdice

Slika 4.

G = m· g

M_UZ = F_UZ ∙ r = G sin α ∙ r = m ∙ g ∙ r ∙ sin ¿¿¿

M_UZ = 3 ∙ 10 ∙ 0,035 ∙ sin 11,31 = 0,21 Nm

Moment uzbrdice nagiba 14%:

M_UZ = 3 ∙ 10 ∙ 0,035 ∙ sin ¿¿

Momenti za segmente profila:

Moment M1 sastoji se od momenata istog smjera djelovanja, te se mogu zbrojiti:

M1=Moment uzbrdice 20% + moment ubrzanja + moment trenja = M_UZ20 + Ma + M4

M1=0,21 + 0,084 + 0,053 = 0,35Nm

M2=M_UZ14 + Ma + M4 = 0,15 + 0,084 + 0,053 = 0,29Nm

M3=M_UZ14 + M4 = 0,15 + 0,053 = 0,20Nm

Momentu kočenja suprotstavlja se trenje i uzbrdica negativan s negativni predznakom.

M5 = - M_UZ14 – M4 + Ma = -0,15 - 0,053 + 0,056 = -0,15Nm

Ukoliko bi robot kočio na nizbrdici tada bi momenti nizbrdice i kočenja bilo pozitivni dok bi momenti trenja bio negativan.

Robot se kreče prema pretpostavljenom profilu na kojem se mijenjaju momenti, a time i opterećenje motora. S opterećenjem motora mijenja se struja koju motor uzima iz baterije. Da bi smo izračunali prosječnu potrošnju struje koju motor konzumira za savladavanje profila potrebno je izračunati efektivni moment. Efektivni moment nam daje kvadratnu srednju vrijednost momenta za zadani profil. Izračun efektivnog momenta uzima u obzir vrijeme trajanja pojedinog momenta u odnosu na cjelokupno vrijeme savladavanja profila. Kvadratna srednja vrijednost izračunava se da bi negativni momenti (moment kočenja) ušli u proračun kao pozitivni. Kvadrat negativnog broja daje pozitivan broj.

Efektivni moment za zadani profil gibanja

Nakon izračunavanja dobivamo:

Prije odabira motora nakon što su nam poznati momenti i brzina vrtnje potrebno je definirati koliko pogonskih motora će posjedovati robot, te da li će istovremeno raditi svi motori te sudjelovati u stvaranju momenta.

Ovisno o konstrukciji robot može posjedovati jedan pogonski motor, a skretanje robota izvodi se zakretanjem kotača (kao kod automobila).

Najčešća konstrukcija robota je s dva pogonska motora koji tvore diferencijalni pogon, a skretanje robota vrši se promjenom brzina vrtnje motora (kao kod gusjeničara).

Konstrukcije robota s višesmjernim kotačima koriste četiri pogonska motora gdje se robot može kretati u svim smjerovima bez zakretanja robotske platforme.

Ukoliko odaberemo diferencijalni pogon s dva pogonska motora tada najčešće za savladavanje momenta sudjeluju oba motora jer istovremeno rade. Kod diferencijalnog pogona jedan motor radi samo kad je potrebno zakrenuti robot oko drugog kotača. Ukoliko zanemarimo takvo gibanje prilikom odabira motora uzimamo polovinu izračunatog maksimalnog momenta. Ukoliko ne zanemarujemo gibanje oko jednog kotača tada za odabir uzimamo maksimalni moment.

Odabir DC motora s ugrađenim reduktorom

Maksimalni moment u našem primjeru je M1=0,35 Nm, a brzina vrtnje kod maksimalnog momenta iznosi n=218 o/min.

Slika 5.

Odabir motora prema maksimalnom momentu

Odabrani motor

Odabir motora prema polovini maksimalnog momenta

Odabrani motor

Tehničke karakteristike drugog proizvođača motora:

Mehanička snaga

Mehanička snaga je produkt momenta i kutne brzine. Zbog promjene profila gibanja mijenja moment, a time i mehanička snaga.

Maksimalna mehanička snaga potrebna za gibanje robota

Prema zadanom profilu gibanja robot pri maksimalnoj brzini ima najveći moment M2=0,29Nm.

Minimalna mehanička snaga

Efektivna mehanička snaga

Efektivna mehanička snaga je "prosječna" snaga za zadani profil gibanja.

Promjenom profila gibanja robota mijenja se mehanička snaga potrebna za realizaciju gibanja.

Izračun stvarnih parametara

Slika 1.

Kako se robot giba kroz profil gibanja tako se mijenjaju momenti opterećenja, a time i brzine vrtnje motora. Ukoliko promatramo područje jednolikog gibanja robot u tom području ne smije promijeniti svoju brzinu gibanja. Zbog promjene opterećenja (momenta) robot će promijeniti brzinu gibanja ukoliko napon napajanja motora ostane nepromijenjen. Regulacija brzine vrtnje motora, kod DC motora s četkicama, vrši se primjenom napona na motoru.

U daljnjim razmatranjima potrebno je odrediti napon motora koji će zadovoljiti kriterij brzine vrtnje. Možemo si postaviti pitanje: Koliki napon moramo dovesti na DC motor da se robot pri promjeni momenta giba zadanom brzinom?

Da bi smo došli do tog podatka prvo moramo za svaki moment izračunati stvarnu brzinu vrtnje. U tehničkim podacima DC motora istaknut je podatak za brzinu vrtnje bez opterećenja, kojeg nazivamo prazni hod, te brzinu vrtnje za maksimalno opterećenje (At Max. Torque). Ovisnost brzine o momentu je linearna karakteristika pa ju jednostavno iz tehničkih podataka možemo primijeniti, a naziva se i vanjska karakteristika motora.

Izračun brzine vrtnje motora za stvarni moment

Odabrani motor

Tablica 1.

Tip E192.12.13 koji smo izabrali ukoliko ne zanemarujemo gibanje oko jednog kotača, odnosno odabiremo motor prema maksimalnom momentu.

Slika 2.

Pravac kroz dvije točke:

Točka (M₁,n₁) predstavlja brzinu vrtnje za maksimalni moment (Tablica 1. At Max. Torque), dok točka (M₂, n₂) predstavlja brzinu vrtnje za prazni hod (Tablica 1. No load) M₂ =0.

Nakon izračuna dobiva se:

Brzina vrtnje kod maksimalnog momenta je zanemarivo veća od zahtijevane brzine vrtnje (n=218 o/min) , ali naš robot neće biti opterećen maksimalnim momentom i da pri tome mora postići brzinu n=218 o/min. Pri maksimalnom momentu mora postići brzinu n=109 o/min.

Za ostale momente koji nastaju prilikom gibanja zadanim profilom brzine su date u Tablici 2.

Tablica 2.
Moment [Nm]	Brzina [o/min]
M2=0.29	n2=236
M3=0,2	n3=256
M4=0,053	n4=288
M5=-0,15	n5=267

Daljnja razmatranja ovisnosti brzine vrtnje o momentu provest ćemo za jednoliko gibanje gdje se razvija moment M3 i M4. Kod istosmjernih motora gdje se regulacija brzine vrtnje vrši promjenom napona napajanja motora u karakteristici se dobivaju paralelni pravci Slika 3. (uz uvjet nepromijenjene uzbude). Kod malih DC motora uzbuda je izvedena permanentnim magnetima tako da je uzbuda konstantna uz zanemarivanje reakcije armature.

Slika 3.

Ovisnost brzine vrtnje o naponu na DC motoru prikazana je na Slici 4.

Slika 4.

Nadalje je potrebno izračunati napon motora za koji pri stvarnom momentu motor postigne zahtijevanu brzinu vrtnje. Izračun kreče od vanjske karakteristike DC motora gdje povlačimo pravac paralelan vanjskoj karakteristici uz nominalni napon napajanja, te izračunavamo maksimalnu brzinu za M=0 uz smanjeni napon U < Un.

Poznat nam je koeficijent smjera pravca -222.22, moment 0,2Nm te brzina vrtnje za taj moment 218 o/min.

Slika 5.

218=-222,2 ∙ 0,2 + y

y=218 + 222,22 ∙ 0,2 ≈ 262 o/min

Dobiveni rezultata y je u stvari maksimalna brzina (n?) koju motor u praznom hodu (M=0) može postići kad je na motoru manji napon od nominalnog napona motora. Da bi smo izračunali taj napon potrebno je rezultate uvrstiti u karakteristiku ovisnosti brzine o naponu Slika 4.

Slika 6.

Ukoliko na motor dovedemo napon od 10,5V maksimalna brzina u praznom hodu iznosi 262 o/min, a pri opterećenju s momentom od 0,2Nm brzina će pasti na 218 o/min.

Kad isti postupak provedemo za M4 dobiveni napon iznosi 9,24V.

Nadalje je potrebno osigurati izračunati napon za određeni dio profila gibanja. To se ostvaruje pulsno-širinskom modulacijom (PWM) koju dovodimo kao upravljački signal na elektronski H-most.

Koliki faktor vođenja moramo podesiti da bi na motor doveli napon od 10,5V?

D – faktor vođenja (duty cycle) odnosno 87,5% signala u stanju vođenja, a 12,5% u stanju mirovanja.

Slika 7.

Pri tome se zanemaruje pad napona na H-mostu u stanju zasićenja koji ovisi o struji koja teče kroz H-most.

Za dovođenje napona na motor u iznosu od 9,24V faktor vođenja iznosi D=0,77.

Iznosi faktora vođenja su bitni prilikom izrade dijela programa koji će se generirati PWM za pogonske motore.

Napon ubrzavanja

Slika 8.

Izračun promjene napona da bi se ostvario profil gibanja za područje ubrzavanja bazira se na prethodno opisanom postupku s tom razlikom što se faktor vođenja za vrijeme ubrzanja mijenja. Potrebno je izračunati granice promjene faktora vođenja.

Za ubrzanje od stanja mirovanja do brzine n=109 o/min uz moment M1=0,35Nm

y= 109 + 222,22 ∙ 0, 35 ≈ 187 o/min

Napon

Napon se mijenja od 0V do 7,5V u vremenu od 0,5s. Takvu promjenu napona potrebno je programski opisati.

Za ubrzanje od 109 o/min do 218 o/min potrebno uz moment M2=0,29Nm.

y= 218 + 222,22 ∙ 0, 29 ≈ 282 o/min

Napon

Napon se mijenja od 7,5V do 11,3V u vremenu od 0,5s.

Kad robot ubrza do zadane brzine napon se smanjuje na 10,5V da bi se nastavio gibati jednoliko uzbrdicom od 14%. Zadržavanje jednolikog gibanja po ravnom terenu ostvaruje se tako da se napon smanji na izračunatih 9,24V.

Usporavanje

Prilikom usporavanja razvija se moment M5=-0,15Nm, jer robot usporava na uzbrdici.

y=218 + 222,22 ∙ (- 0, 15) ≈ 185 o/min

Napon se smanjuje od 7,4V do nule u 1,5s.

Pulsno-širinska modulacija - PWM

Pod pojmom modulacije podrazumijevamo promjenu signala tako da mu mijenjamo frekvenciju ili oblik pomoću drugog signala.

Na primjer u radio transmisiji najpoznatije su AM-amplitudna modulacija i FM-frekvencijska modulacija. Da bi se audio signal iz radio stanice prenio do radio prijemnika, istovremeno više stanica bez miješanja signala, potrebno je osnovni audio signal (na primjer govor voditelja emisije) modulirati tako zvanim valom nosiocem koji je određene mnogostruko više frekvencije od audio signala. Tada audio signal mijenja frekvenciju vala nosioca i odašilje se. Mi svoj radio prijemnik podešavamo na frekvenciju vala nosioca u koji je „utisnut“ moduliran audio signal. U radio prijemniku se vrši demoduliranje i dobije se osnovni audio signal (govor voditelje emisije).

U energetske svrhe koristi se PWM („pulse-width mudulation“ ) pulsno-širinska modulacija. U prezentaciji „Izračun napona motora“ spomenuta je pulsno-širinska modulacija i izračunat faktor vođenja (duty cycle).

Način generiranja PWM signala:

Slika 1.

PWM signal dobiva se tako da se val nosilac i signal za moduliranje dovode na komparator. Komparator je tako podešen ukoliko je napon vala nosilaca viši od napona signala za moduliranje izlaz komparatora je nizak nivo ili 0V. Ukoliko je napon signala za moduliranje viši od napona vala nosilaca tada je izlaz komparatora u visokom nivou odnosno 12V. Odnos komparacije se može promijeniti pa će izlaz biti invertiran.

Da bi smo lakše komunicirali uvodimo pojam faktora vođenja „D“ (duty cycle or power cycle) kojim definiramo vrijeme vođenja odnosno vrijeme u kojem je PWM signal u višem nivou (logičkoj „1“) u odnosu na frekvenciju PWM signala.

Faktor vođenja može poprimiti vrijednost:

0 ≥ D ≤ 1

Općenito:

U_PWM – Napon PWM-a (srednja vrijednost napona napajanja PWM-a)

Val nosilac na Slici 1. ima pilasti oblik, dok na Slici 2. ima trokutasti oblik.

Slika 2.

Ekvivalentna shema DC motora.

Slika 3.

V_EMF – napon induciran u zavojnici prilikom vrtnje – generatorski način rada (napon koji se suprotstavlja dovedenom naponu)

L – induktivitet armaturnog namota stroja

R – radni otpor armaturnog namota + otpor na četkicama motora

Prilikom priključenja motora na DC napon u stanju mirovanja rotora napon V_EMF = 0V kroz motor u prvom trenutku potekne struja kratkog spoja koju kod motora nazivamo poteznom strujom motora. To dovodi do zaključka da je električna vremenska konstanta τe kruga vrlo mala. Ekvivalentna shema u stanju mirovanja rotora.

Slika 4.

Električna vremenska konstanta kruga računa se:

L – induktivitet armaturnog namota [H]

R – otpor armaturnog namota [Ω]

Promjena struje na induktivnom trošilu uzrokovana električnom vremenskom konstantom τ_e.

Slika 5.

Maksimalna vrijednost struje određena je naponom napajanja i otporom armature i iznosi:

Moment DC motora proporcionalan je struji motora.

k_M – konstanta momenta motora

U trenutku mirovanja rotora V_EMF = 0 zbog male električne vremenske konstante τe potekne vrlo velika struja (struja kratkog spoja) koja ima za posljedicu veliki potezni moment. Rotor motora počinje se okretati što dovodi do induciranja napona V_EMF > 0 (armaturni namot motora okreče se u magnetskom polju i u namotu se inducira napon V_EMFSlika 3.). Povećanje brzine vrtnje motora uzrokuje povećanje napona V_EMF. Napon V_EMF suprotstavlja se dovedenom naponu na priključku motora. Kad promatramo napon na armaturnom namotu tada se od napona dovedenog na priključak motora oduzima inducirani napon V_EMF pa napon na armaturnom namotu opada.

Opadanjem napona na armaturnom namotu opada i struja kroz armaturni namot sve dok se ne uspostavi ravnoteža ovisna o:

momentu kojim je opterećen motor
dovedenom naponu na priključke motora
brzini vrtnje motora

Promjena brzine vrtnje motora odvija se po istoj funkciji kao i promjena struje na induktivnom opterećenju (Slika 5.) s tom razlikom što je vremenska konstanta motora τ_m mnogostruko veća od električne vremenske konstante τ_e.

Vremenska konstanta motora objedinjuje:

električnu vremensku konstantu
moment inercije rotora
trenje u ležajevima
ventilator hlađenja motora

Prethodno razmatranje bazira se na istosmjernom naponu dovedenom na priključke motora. Ukoliko na priključke motora dovodimo PWM signal tada dolazi do promjene oblika napona dovedenog na priključke motora pa je potrebno odrediti najpogodniju frekvenciju PWM-a. Zbog odnosa τ_e > τ_m frekvencija PWM-a neće imati utjecaj na τ_m jer je frekvencija PWM-a veća od τ_m.

S druge strane frekvencija PWM-a može utjecati na promjenu struje motora koja je povezana s τ_e vremenskom konstantom. Usko povezano s frekvencijom PWM- a su granice u kojima se mijenja faktor vođenja.

Za odabrani motor proizvođač nije dao podatak o vremenskoj konstanti pa ćemo je procijeniti na τ_e = 10μs.

Poveznica na tehničke podatke motora:

https://www.micromotors.eu/en/gear-motors/series-e192/

Frekvencija PWM-a

Što je faktor vođenja manji to je vrijeme za promjenu struje krače, te pri previsokim frekvencijama PWM nema dovoljno vremena za promjenu struje što za posljedicu ima smanjeni moment.

Na Slici 5. može se vidjeti da je potrebno 5 vremenskih konstanti τe da struja dosegne približno maksimalnu vrijednost pa možemo zaključiti ako je τ_e = 10μs potrebno je 50μs da struja dosegne približno maksimalnu vrijednost.

Maksimalna frekvencija PWM-a:

Slika 6.

Na prvom dijagramu struja dosegne 86,5% maksimalne vrijednosti za 2τ_e dok u drugom dijagramu gdje je frekvencija dvostruko veća dosegne 63,2% maksimalne vrijednosti za 1τ_e. Može se zaključiti da ukoliko PWM ima previsoku frekvenciju struja nema dovoljno vremena za postizanje svoje maksimalne vrijednosti.

Poveznica za detalje:

https://www.precisionmicrodrives.com/content/ab-022-pwm-frequency-for-linear-motion-control/

Programiranje PWM-a

PWM u mikroupravljaču generira periferni alternativni uređaj Timer/Counter kojeg je potrebno podesiti tako da koristi svoj generator oblika koji je zadužen za generiranje PWM-a. PWM možemo generirati samo na predviđenim priključcima mikroupravljača koji se moraju definirati kao izlazni. Izlazi PWM-a kod AVR mikroupravljača označavaju se OCnx.

Za generiranje PWM signala koristit ćemo Timer/Couner1 s fiksnom rezolucijom PWM-a. Postoje tri vrste PWM-a koje možemo ostvariti AVR mikroupravljačima, a mi ćemo koristiti fazni PWM. Rezoluciju PWM postavit ćemo 9-bitnu (Mode 2.)

Slika 1.

Frekvencija faznog PWM-a izračunava se prema formuli:

f_CPU – frekvencija centralne procesorske jedinice

N – faktor dijeljenja frekvencije (1, 8, 64, 256, 1024)

TOP – gornja vrijednost za 9-bitni 511

Frekvencija PWM-a može se podesiti pravilnim izborom faktore dijeljenja frekvencije N i odabirom rezolucije PWM-a uz poznavanje frekvencije rada CPU-a.

Frekvencija PWM za upravljanje DC motorima ne bi trebala biti viša od 2KHz.

Faktor dijeljenja iznosi:

Odabiremo N=8

Frekvencija PWM:

Konfiguracija priključaka

Slika 2.

Odredili smo da će PWM generirati Timer1 koji ima dva neovisna izlazna kanala za PWM (za svaki motor jedan kanal). Stoga je potrebno locirati priključke na kojima ćemo generirati PWM radi daljnjeg planiranja upravljačke elektronike. Izlazi periferne funkcije Timer/Counter1 na kojima generiramo PWM nazivaju se OC1A za kanal A i OC1B za kanal B. Izlazi periferne funkcije nalaze se na PB1 za OC1A priključak broj 15 i PB2 za OC1B priključak broj 16. Na ArduinoUno pločici su priključci OC1A broj 9 i OC1B broj 10.

Slika 3.

Bitovi COMnx kontrolnog registra timera TCCRnx djeluju na ILI sklop koji kad je u stanju logičke "1" ukoliko postavimo bilo koji COMnx bit. Na izlaz OCnx pin prosljeđuje se stanje iz perifernog uređaja (Timer/Counter n) odnosno iz OCnx.

Kad je izlaz ILI sklopa u logičkoj "0" tada na izlaz se prosljeđuje stanje iz registra PORT.

Nadalje iz sheme se vidi da DDR registar mora biti postavljan kao izlazni odnosno u stanju logičke "1" da bi se stanje ili iz registra PORT ili iz OCnx proslijedilo na izlaz.

Slika 4.

Kontrolni registar TCCR1A s pripadajućim bitovima za postavljanje načina rada.

Slika 5.

Povećanje vrijednosti komparacijskog registra OCR1A i OCR1B povećava faktor vođenja što za posljedicu ima povećanje izlaznog napona na priključcima OC1A i OC1B ukoliko odaberemo crtkano označene postavke.

Ukoliko drugačije konfiguriramo komparacijsku jedinicu tada će povećanje vrijednosti komparacijskih registara OCR1A i OCR1B smanjivati faktor vođenja i na izlazima će se napon smanjivati.

Na vremenskom dijagramu je naznačeno da TOVn zastavica (zastavica preljeva registra) postavlja se kad vrijednost TCNTn registra dosegne nulu kad timer broji prema dolje. Ukoliko omogućimo prekid preljeva registra tada će se ostvariti prekid u kojem možemo mijenjati stanje komparacijskog registra OCR1A/B.

Napomena: Bitno je shvatiti da generator oblika koristi prekidne zastavice signala komparacije za generiranje oblika a pri tome programer može i ne mora koristiti te iste zastavice za pokretanje prekida.

Slika 6.

Slika 7.

Kontrolni registra TCCR1B s pripadajučim bitovima za postavljenje načina rada i pokretanje timera.

Postavljanjem bitova CS10-CS12 pokreče se timer s određenim faktorom dijeljenja N.

Prethodno smo izračunali N=8, moramo postaviti bit CS11.

Program pisan u MichrochipStudio (bivši AtmelStudio):

Mikroupravljač ATMega328P – koristi ga ArduinoUno

Nakon postavki mikroupravljača otvara se radna površina.

#include <avr/io.h> //unesen dokument zaglavlja mikroupravljača ATMega328P

int main (void) //otvaranje glavne funkcije

{ //područje pretprocesorskih postavki

/* Replace with your application code */

while (1)

{ //program koji se odvija u beskonačnoj petlji

}

Inicijalni program gdje se postavlja PWM i pokreče se Timer/Counter:

#include <avr/io.h> //unesen dokument zaglavlja postavljenog mikroupravljača ATMega328P

int main (void) //otvaranje glavne funkcije

{ /*PODRUČJE PRETPROCESORSKIH POSTAVKI*/

/* pravilno postavljanje priključaka da bi se izbjeglo stanje visoke impedancije*/

DDRB=255; //priključci PORTB postavljeni kao izlazni

PORTD=255; //priključeni pull-up otpornici na priključcima PORTD

PORTC=255; //priključeni pull-up otpornici na priključcima PORTC

/*postavljanje rada timera 9-bitni PWM s djeliteljem frekvencije N=8*/

TCCR1A=(1<<WGM11) | (1<<COM1A1) | (1<<COM1B1); //9-bitni fazni PWM, Povečanje vrijednosti komparacijskog registra povečava napon na izlazu

TCCR1B=(1<<CS11); //pokretanje timera s djeliteljem N=8

/*izlazni napon je 0 jer je po uključenju upisna vrijednost komparacijskih registara OCR1A i OCR1B nula*/

/* Replace with your application code */

while (1)

{ //program koji se odvija u beskonačnoj petlji

}

Promjena napona na izlazu od 0 do 7,5V u 0,5s da bi se ostvario moment M1 uz frekvenciju PWM-a 1957Hz ostvaruje broj prekida preljeva:

Dijelimo s dva jer se promjena napona odvija u 0,5s a frekvencija daje broj promjena u 1s.

Faktor vođenja za napon od 7,5V:

U_Hmost – pad napona na H-mostu

Vrijednost komparacijskog registra za faktor vođenja 0,72 iznosi:

OCR1_A/B = rezolucija * D = 511 * 0,72 = 377

Nadalje u 978 prekida potrebno je izvršiti promjenu OCR1A/B registra od nule do 377.

Ako za svaki peti prekid 2,59*2≈5 povećamo OCR1A/B za 2 i postavimo granice promjene dobivamo približnu promjenu napona od 0 do 7,5v u 0,5s.

Slika 8.

Registar najave (omogućavanja) prekida. U našem slučaju potrebno je omogućiti prekid preljeva registra TOIE.

Program promjene napona od 0-7,5V (8,7V s uračunatim padom napona na H-mostu) na oba kanala PWM-a Timera1.

#include <avr/io.h> //unesen dokument zaglavlja mikroupravljača ATMega328P

/* https://www.nongnu.org/avr-libc/user-manual/modules.html poveznica s dokumentima zaglavlja*/

#include <avr/interrupt.h> //unesen dokument zaglavlja za pozivanje funkcija prekida

unsigned char a; //globalna pozitivna varijabla tipa char 0-255 za broj prekida OVF

ISR(TIMER1_OVF_vect){ //prekidna funkcija

a++; //varijabla koja broji svaki 5 prekid

if (a==5 && OCR1A < 377){ //za svaki 5 prekid i ako je OCR1A<<377

OCR1A=OCR1A+2; //povećaj OCR1A za 2

OCR1B=OCR1B+2; //povećaj OCR1B za 2

a=0; //varijabla a=0 Broji sljedećih 5 prekida

}

int main (void) //otvaranje glavne funkcije

{ /*PODRUČJE PRETPROCESORSKIH POSTAVKI*/

/* pravilno postavljanje priključaka da bi se izbjeglo stanje visoke impedancije*/

DDRB=255; //priključci PORTB postavljeni kao izlazni

PORTD=255; //priključeni pull-up otpornici na priključcima PORTD

PORTC=255; //priključeni pull-up otpornici na priključcima PORTC

/*postavljanje rada timera 9-bitni PWM s djeliteljem frekvencije N=8*/

TCCR1A=(1<<WGM11) | (1<<COM1A1) | (1<<COM1B1); //9-bitni fazni PWM, Povećanje vrijednosti komparacijskog registra povećava napon na izlazu

TCCR1B=(1<<CS11); //pokretanje timera s djeliteljem N=8

/*izlazni napon je 0 jer je po uključenju upisna vrijednost komparacijskih registara OCR1A i OCR1B nula*/

/*najava TOV prekida za promjenu OCR1A/B*/

TIMSK1=(1<<TOIE1); //omogućavanje prekida preljeva registra

sei(); //omogućavanje globalnog prekida u statusnom registru

/* Replace with your application code */

while (1)

{ //program koji se odvija u beskonačnoj petlji

}

Nastavak programa za promjenu napona od 7,5 do 11,3 odnosno do maksimuma napona baterije za uračunati pad napona na H-mostu.

511-377=134 promjene OCR1A/B registra u 0,5s i 978 promjena u 0,5s.

Potrebno je za svaki 7 prekid povećati OCR1A/B za 1 uz postavljanje granice od 511.

#include <avr/io.h> //unesen dokument zaglavlja mikroupravljača ATMega328P

/* https://www.nongnu.org/avr-libc/user-manual/modules.html poveznica s dokumentima zaglavlja*/

#include <avr/interrupt.h> //unesen dokument zaglavlja za pozivanje funkcija prekida

unsigned char a; //globalna pozitivna varijabla tipa char 0-255 za broj prekida OVF

ISR(TIMER1_OVF_vect){ //prekidna funkcija

a++; //varijabla koja broji svaki 5 prekid

if (a==5 && OCR1A<377){ //za svaki 5 prekid i ako je OCR1A<<377

OCR1A=OCR1A+2; //povećaj OCR1A za 2

OCR1B=OCR1B+2; //povećaj OCR1B za 2

a=0; //varijabla a=0 Broji sljedećih 5 prekida

}

else if (a==7 && OCR1A>=377 && OCR1A<511){ //za svaki 7 prekid u granicama OCR1A povećaj OCR1A i OCR1B

OCR1A++; //povećaj OCR1A za 1

OCR1B++; //povećaj OCR1B za 1

a=0; //varijabla a=0 Broji sljedećih 7 prekida

}

int main (void) //otvaranje glavne funkcije

{ /*PODRUČJE PRETPROCESORSKIH POSTAVKI*/

/* pravilno postavljanje priključaka da bi se izbjeglo stanje visoke impedancije*/

DDRB=255; //priključci PORTB postavljeni kao izlazni

PORTD=255; //priključeni pull-up otpornici na priključcima PORTD

PORTC=255; //priključeni pull-up otpornici na priključcima PORTC

/*postavljanje rada timera 9-bitni PWM s djeliteljem frekvencije N=8*/

TCCR1A=(1<<WGM11) | (1<<COM1A1) | (1<<COM1B1); //9-bitni fazni PWM, Povećanje vrijednosti komparacijskog registra povećava napon na izlazu

TCCR1B=(1<<CS11); //pokretanje timera s djeliteljem N=8

/*izlazni napon je 0 jer je po uključenju upisna vrijednost komparacijskih registara OCR1A i OCR1B nula*/

/*najava TOV prekida za promjenu OCR1A/B*/

TIMSK1=(1<<TOIE1); //omogućavanje prekida preljeva registra

sei(); //omogućavanje globalnog prekida u statusnom registru

while (1)

{ //program koji se odvija u beskonačnoj petlji

}

Nakon ubrzavanja slijedi jednoliko gibanje po uzbrdici gdje se razvija moment M3 i potreban je napon na motoru od 10,5V, odnosno ako uzmemo u obzir pad napona na H-mostu tada generirani napon PWM-a iznosi 10,5+1,2=11,7V.

Faktor voženja iznosi:

Vrijednost OCR1 registara iznosi:

OCR1_A/B = 511 * 0,97 = 496

Prema profilu gibanja jednoliko gibanje uzbrdo traje 6s, te se mijenja u jednoliko gibanje po ravnom terenu koje traje 6s.

Za moment M4 faktor vođenja s uračunatim padom napona na H-mostu iznosi:

Vrijednost OCR1 registara iznosi:

OCR1_A/B = 511 * 0,87 = 445

#include <avr/io.h> //unesen dokument zaglavlja mikroupravljača ATMega328P

/* https://www.nongnu.org/avr-libc/user-manual/modules.html poveznica s dokumentima zaglavlja*/

#include <avr/interrupt.h> //unesen dokument zaglavlja za pozivanje funkcija prekida

#define F_CPU 16000000UL //16 MHz definiranje frekvencije oscilatora

unsigned char a; //globalna pozitivna varijabla tipa char 0-255 za broj prekida OVF

ISR(TIMER1_OVF_vect){ //prekidna funkcija

a++; //varijabla koja broji svaki 5 prekid

if (a==5 && OCR1A<377){ //za svaki 5 prekid i ako je OCR1A<<377

OCR1A=OCR1A+2; //povećaj OCR1A za 2

OCR1B=OCR1B+2; //povećaj OCR1B za 2

a=0; //varijabla a=0 Broji sljedećih 5 prekida

}

else if (a==7 && OCR1A>=377 && OCR1A<511){ //za svaki 7 prekid u granicama OCR1A povećaj OCR1A i OCR1B

OCR1A++; //povećaj OCR1A za 1

OCR1B++; //povećaj OCR1B za 1

a=0; //varijabla a=0 Broji sljedećih 7 prekida

}

if (OCR1A==511)

TIMSK1^=(1<<TOIE1); //onemogućavanje prekida preljeva registra

}

int main (void) //otvaranje glavne funkcije

{ /*PODRUČJE PRETPROCESORSKIH POSTAVKI*/

/* pravilno postavljanje priključaka da bi se izbjeglo stanje visoke impedancije*/

DDRB=255; //priključci PORTB postavljeni kao izlazni

PORTD=255; //priključeni pull-up otpornici na priključcima PORTD

PORTC=255; //priključeni pull-up otpornici na priključcima PORTC

/*postavljanje rada timera 9-bitni PWM s djeliteljem frekvencije N=8*/

TCCR1A=(1<<WGM11) | (1<<COM1A1) | (1<<COM1B1); //9-bitni fazni PWM, Povečanje vrijednosti komparacijskog registra povečava napon na izlazu

TCCR1B=(1<<CS11); //pokretanje timera s djeliteljem N=8

/*izlazni napon je 0 jer je po uključenju upisna vrijednost komparacijskih registara OCR1A i OCR1B nula*/

/*najava TOV prekida za promjenu OCR1A/B*/

TIMSK1=(1<<TOIE1); //omogućavanje prekida preljeva registra

sei(); //omogućavanje globalnog prekida u statusnom registru

/* Replace with your application code */

while (1)

{ //program koji se odvija u beskonačnoj petlji

if (OCR1A==511){ //kad je završeno ubrzavanje

OCR1A=496; //jednoliko gibanje uzbrdo

OCR1B=496;

_delay_ms(6000); //trajanje 6s (može se pokrenuti timer za mjerenje vremena)

}

else if (OCR1A==496){ //kad je završeno jednoliko uzbrdo

OCR1A=445; //jednoliko ravno

OCR1B=445;

_delay_ms(6000); //trajanje 6s (može se pokrenuti timer za mjerenje vremena)

}

Faza usporavanja:

Prilikom usporavanja smanjujemo napon od vrijednosti 8,6V s uračunatim padom napona na H-mostu do nule u vremenu od 1,5s.

OCR1A = 511 * 0,72 = 366

U 1,5s dogodit će se 2936 prekida preljeva registra uz frekvenciju 1957Hz, te u tom broju prekida potrebno je OCR1A/B registra iz vrijednosti 366 svesti na nulu. To će se ostvariti ako za svaki 8 prekid umanjimo OCR1A/B za 1.

#include <avr/io.h> //unesen dokument zaglavlja mikroupravljača ATMega328P

/* https://www.nongnu.org/avr-libc/user-manual/modules.html poveznica s dokumentima zaglavlja*/

#include <avr/interrupt.h> //unesen dokument zaglavlja za pozivanje funkcija prekida

#define F_CPU 16000000UL //16 MHz definiranje frekvencije oscilatora

#include <until/delay.h> //unesen dokument zaglavlja za funkciju kašnjenja

unsigned char a, b, c, d; //globalna pozitivna varijabla tipa char 0-255 za broj prekida OVF

ISR(TIMER1_OVF_vect){ //prekidna funkcija

if (b==0){

a++; //varijabla koja broji svaki 5 prekid

if (a==5 && OCR1A<377){ //za svaki 5 prekid i ako je OCR1A<<377

OCR1A=OCR1A+2; //povećaj OCR1A za 2

OCR1B=OCR1B+2; //povećaj OCR1B za 2

a=0; //varijabla a=0 Broji sljedećih 5 prekida

}

else if (a==7 && OCR1A>=377 && OCR1A<511) { //za svaki 7 prekid u granicama OCR1A povečaj OCR1a i B

OCR1A++; //povećaj OCR1A za 1

OCR1B++; //povećaj OCR1B za 1

a=0; //varijabla a=0 Broji sljedećih 7 prekida

}

else if (OCR1A==511){

TIMSK1^=(1<<TOIE1); //onemogućavanje prekida preljeva registra

d=1; //varijabla uvjeta

}

if (b==1){

a++;

if (a==8 && OCR1A>0){

OCR1A--;

OCR1B--;

a=0;}

if (OCR1A==0)

TCCR1B^=(1<<CS11); //zaustavljen timer

}}

int main (void) { //otvaranje glavne funkcije

/*PODRUČJE PRETPROCESORSKIH POSTAVKI*/

/* pravilno postavljanje priključaka da bi se izbjeglo stanje visoke impedancije*/

DDRB=255; //priključci PORTB postavljeni kao izlazni

PORTD=255; //priključeni pull-up otpornici na priključcima PORTD

PORTC=255; //priključeni pull-up otpornici na priključcima PORTC

/*postavljanje rada timera 9-bitni PWM s djeliteljem frekvencije N=8*/

TCCR1A=(1<<WGM11) | (1<<COM1A1) | (1<<COM1B1); //9-bitni fazni PWM, Povećanje vrijednosti komparacijskog registra povećava napon na izlazu

TCCR1B=(1<<CS11); //pokretanje timera s djeliteljem N=8

/*izlazni napon je 0 jer je po uključenju upisna vrijednost komparacijskih registara OCR1A i OCR1B nula*/

//OCR1A=480;

//OCR1B=480;

/*najava TOV prekida za promjenu OCR1A/B*/

TIMSK1=(1<<TOIE1); //omogućavanje prekida preljeva registra

sei(); //omogućavanje globalnog prekida u statusnom registru

/* Replace with your application code */

while (1) { //program koji se odvija u beskonačnoj petlji

if (OCR1A==511 && c==0 && d==1){ //kad je završeno ubrzavanje i zadovoljene varijable uvjeta

OCR1A=496; //jednoliko gibanje uzbrdo

OCR1B=496;

_delay_ms(6000); //trajanje 6s

}

if (OCR1A==496 && c==0 && d==1){ //kad je završeno jednoliko uzbrdo i zadovojene varijable uvjeta

OCR1A=445; //jednoliko ravno

OCR1B=445;

_delay_ms(6000); //trajanje 6s

c=1;

}

if (c==1){ //faza usporavanja

TIMSK1=(1<<TOIE1); //faza usporavanja

b=1; //varijabla koja osigurava u ISR(prekid preljeva) da odlazi u dio usporavanja

}

if (OCR1A==0 && b==1){

TCCR1B=(1<<CS11); /pokrenut timer

b=0; //varijabla koja osigurava u ISR(prekid preljeva) da odlazi u dio ubrzavanja

c=0;

d=0;

}

Konačni program u kojem su dodane varijable uvjeta.

Prezentiran je jednostavan program koji omogućuje približno točno kretanje robota prema zadanom profilu gibanja i podloge.

Kad bi smo na robotu probali program robot bi se gibao prema zadanom profilu, ali ne znamo u kojem bi se smjeru gibao, odnosno ne znamo da li bi robot skretao ili bi se gibao ravno.

Bitni razlozi su:

Nesavršenost motora i reduktora (tolerancije u proizvodnji)
Pad napona na H-mostu na oba kanala nije isti

U daljnjim razmatranjima pozabavit ćemo se regulacijom brzine vrtnje radi uklanjanja nesavršenosti i tvorničkih tolerancija.

Napomena!

Kod izmjene vrijednosti OCRnx registara potrebno je obratiti pažnju na da vrijednost OCR1nx registra ne prekorači vrijednost rezolucije PWM-a.

Na primjer kod 9-bitne rezolucije ne smijemo dozvoliti da vrijednost OCR1x registra prekorači 511 jer su neiskorišteni bitovi maskirani na logičku "0".

Ukoliko se prekorači vrijednost OCR1x registra ne dolazi do komparacijskog signala i vrijednost OCR1x registra se poništava.

Kod programiranja potrebno je obratiti pažnju kad se vrijednost OCR1x registra povećava za broj veći od 1, a približava se vrijednosti rezolucije PWM-a.

Primjer:

if (OCR1A<=511){ //kad je vrijednost OCR1A manja ili jednaka 511

OCR1A=OCR1A+7; //povećaj OCR1A za 7

if (OCR1A>=511) //kad je vrijednost OCR1A veća od 511 Ne može utvrditi jer su neiskorišteni bitovi registra OCR1A maskirani na nulu

OCR1A=511; //OCR1A jednak je 511

Ukoliko se OCR1x mijenja za 1 (OCR1x++) tada ne dolazi do prekoračenja OCR1x registra, naravno ukoliko se pravilno postave granice promjene.

Regulacija brzine vrtnje motora

U primjeru „Programiranje PWM-a“ izrađen je program koji opisuje profil gibanja i zahtijeva mnogo izmjena PWM-a prema izračunatim naponima i faktorima vođenja.

Napredniji program robota automatski održava jednoliku brzinu pri promjeni profila podloge odnosno pri promijeni nagiba uzbrdice. Za izradu takvog programa potrebno je izmjeriti brzinu gibanja i izvršiti korekciju ukoliko brzina odstupa od zadane brzine.

Mjerenje brzine robota možemo izvršiti indirektno mjerenjem brzine vrtnje kotača. Za mjerenje brzine vrtnje kotača koristimo senzor inkrementalni enkoder.

Inkrementalni enkoder može se nalaziti:

Kao sastavni dio motora
Kao sastavni dio kotača robota

Inkrementalni enkoder u motoru

Poveznica na tehničke podatke:

https://www.micromotors.eu/en/gear-motors/series-e192/

Potražiti „Technical data encoder“

Iz tehničkih podataka vidljivo je da enkoder daje tri impulsa po okretaju (nimp/okr) koji su elektronički pripremljeni tako ih nije potrebno naknadno filtrirati. Osim toga daje dva izlaza A i B koji su pomaknuti za 90° što omogućuje osim očitanja brzine i određivanje smjera vrtnje motora. U daljnjim razmatranjima koristit ću jedan izlaz enkodera.

Frekvencija impulsa enkodera ovisi o brzini vrtnje motora koju možemo izračunati iz prijenosnog omjera reduktora.

Slika 1.

Za odabrani motor prijenosni omjer iznosi i=13,44.

Frekvencija enkodera za maksimalno opterećenje:

Frekvencija enkodera za prazni hod:

U konačnici mjerenje brzine vrtnje svodi se na brojanje impulsa enkodera u nekom vremenu, dok se regulacija brzine vrtnje svodi na usporedbu izmjerene brzine s zadanom brzinom i ukoliko postoji regulacijsko odstupanje tada se vrši korekcija brzine odnosno PWM-a.

Vrijeme mjerenja brzine vrtnje treba biti što krače, a s druge strane broj impulsa u tom vremenu mora biti dovoljan da postignemo dovoljnu razlučivost.

Na primjer ukoliko bi vrijeme brojanja impulsa ograničili na 5ms tada bi minimalni broj impulsa enkodera kod maksimalnog opterećenja bio 6 do 7 impulsa dok za prazni hod maksimalni broj iznosi 10 impulsa. Razlika između minimalnog i maksimalnog broja impulsa iznosi 3 do 4 impulsa što nije dovoljna razlučivost.

Dolazimo do zaključka da moramo povećati vrijeme mjerenja impulsa na barem 200 do 300ms te povećavamo razlučivost za 200ms na 13 impulsa razlike između minimalne i maksimalne brzine. Za 300ms na 20 impulsa razlike.

Određivanje vremena mjerenja vršimo pomoću Timer/Counter2, dok impulse broji Counter0.

Određivanje vremena mjerenja Timer2 izvest ću tako zvanim CTC načinom rada. CTC (Clear timer on compare) očisti vrijednost timerskog registra TCNT na komparacijski signal.

https://ww1.microchip.com/downloads/en/DeviceDoc/ATmega48A-PA-88A-PA-168A-PA-328-P-DS-DS40002061B.pdf

Stranica 155.

Frekvenciju sustava od 16MHz podijelit ćemo s N=1024 pa dobivamo frekvenciju timera 15625Hz. Dobivena frekvencija timera može se podijeliti s 125 što će biti TOP vrijednost do koje broji timer i tada u jednoj sekundi dobivamo 125 prekida. Ako želimo mjeriti impulse u vremenu od 200ms tada brojimo 25 prekida, za trajanje mjerenja 304ms do 38 prekida.

Slika 2.

Na priključku PD4 izveden je ulaz T0 periferne alternativne jedinice brojača vanjskih događaja.

Da bi smo brojili vanjske događaje (impulse s enkodera) moramo isprano postaviti priključak.

Slika 3.

Slika 4.

Za jednoliko gibanje v=0,8m/s kotači se trebaju okretati brzinom od 218o/min pa možemo izračunati potreban broj impulsa enkodera za tu brzinu.

U vremenu od 200ms mora se generirati:

Ukoliko je broj impulsa u 200ms manji od 29 tada je potrebno povećati komparacijski registar OCR1A i OCR1B, odnosno za veći broj od 29 impulsa u 200ms potrebno je vrijednost komparacijskih registara smanjiti.

U analognoj regulacijskoj tehnici izvedenoj PID članovima regulaciju možemo ostvarili P (proporcionalnim) članom koji ima funkciju pojačanja ovisno o regulacijskom odstupanju i namještenoj vrijednosti P člana. Vrijednost P člana odnosno pojačanja određuje se eksperimentalno (Ziegler-Nichols metodom), te daje vrijednosti kod regulacije brzine DC motora između 1,5 i 3 što omogućuje rad motora bez oscilacija.

Ukoliko primijenimo rezultate (Ziegler-Nichols metode) na regulaciju brzine robota tada regulacijsko odstupanje (od očekivanog broja impulsa enkodera u vremenu od 200ms (29) oduzmemo broj stvarnih impulsa i dobivena vrijednost predstavlja regulacijsko odstupanje) pomnožimo s P pojačanjem (1,5 do 3), te dobivenu vrijednost dodamo ili oduzmemo od vrijednosti komparacijskog registra OCR1A i OCR1B.

#include <avr/io.h>

#include <avr/interrupt.h>

unsigned char a,b,c,d,e,f,g,h; //a=varijabla broja prekida, b=varijabla broj impulsa enkodera, c=zadani broj impulsa za neku brzinu

//d pomoćna varijabla, e i f varijable razlike u broju impulsa enkodera, g pomoćna varijabla, h varijabla petlje

unsigned int oc; //početna vrijednost komparacijskih registara prema zadanom broju impulsa c

ISR(TIMER2_COMPA_vect){

a++;

if (a==25) { //kad nabroji do 25 prekida proteklo je 200ms

b=TCNT0; //u varijablu b prebaci stanje registra TCNT0 (broja impulsa s enkodera) u 200ms

TCNT0=0; //očisti vrijednost registra TCNT0 broja impulsa

a=0; //a=0 omogućuje brojanje sljedećih 25 prekida odnosno vrijeme 200ms

h=0; //pomoćna varijabla vidi ISR(TIMER1_OVF_vect)

if (b<c)

e=(c-b)*2;

else if (b>c)

f=(b-c)*2;

}

ISR(TIMER1_OVF_vect){

if (OCR1A<oc && OCR1A<511 && g==0){ //izvodi se samo prilikom pokretanja varijabla g

OCR1A++;

OCR1B++;

if (OCR1A==oc){ //oc varijabla se izračunava ovisno o broju impulsa u vremenu mjerenja

TIFR2=(1<<OCF2A); //očisti zastavicu prekida

TIMSK2=(1<<OCIE2A); //pokreni prekid

g=1;

}

if (e>h && OCR1A<511 && g==1){ //izvodi program dok varijabla h je manja od e

OCR1A++;

OCR1B++;

h++; //za svaki prolaz povećaj varijablu h

}

else if (f>h && OCR1A>0 && g==1){

OCR1A--;

OCR1B--;

h++;

}

int main (void)

{

DDRB=255;

PORTD=255;

PORTC=255;

/*postavke timera2*/

TCCR2A=(1<<WGM21); //CTC način rada

TCCR2B=(1<<CS22) | (1<<CS21) | (1<<CS20); //pokretanje timera N=1024

OCR2A=125;

/*postavke timera0*/

TCCR0B=(1<<CS02) | (1<<CS01) | (1<<CS00); //postavi counter0 brojanje vanjskih impulsa na uzlani brid

/*postavke timera1 za PWM*/

TCCR1A=(1<<WGM11) | (1<<COM1A1) | (1<<COM1B1); //9-bitni fazni PWM, Povećanje vrijednosti komparacijskog registra povećava napon na izlazu

TCCR1B=(1<<CS11); //pokretanje timera s djeliteljem N=8 /*izlazni napon je 0 jer je po uključenju upisna vrijednost komparacijskih registara OCR1A i OCR1B nula*/

TIMSK1=(1<<TOIE1);

sei(); //globalni prekid

while (1)

{

if (d==0) { //izračun varijable oc za zadanu varijablu c koja određuje očekivani broj impulsa enkodera u vremenu

c=50;

oc=c*10/4; //za korišteni motor

d=1;

}

Opis programa

U komparacijske registre timera1 OCR1A i OCR1B upisana je nula, tako da je izlazni napon PWM-a nula i motori miruju. Nakon pretprocesorskih naredbi program odlazi u beskonačnu petlju i za zadani očekivani broj impulsa (varijabla c koja je zadana) izračunava se približna vrijednost komparacijskih registara. Varijabla „d“ osigurava jedan prolaz kroz grananje.

Izmjene komparacijskih registara izvode se u prekidu preljeva timera1 kod fazno korektnog PWM-a kad TCNT1 registar dosegne vrijednost nula. Prilikom svakog prekida program odlazi u ISR(TIMER1_OVF_vect) gdje povećava komparacijski registar za +1 dok vrijednost ne postigne vrijednost varijable „oc“. Kad komparacijski registri postignu vrijednost varijable „oc“ odnosno motori se vrte postavljenom brzinom (OCR1A==oc) pokreće se prekid ISR(TIMER2_COMPA_vect) koji svakih 200ms uzima broj impulsa enkodera radi regulacije brzine.

Ukoliko je broj impulsa manji od očekivanog broja za postavljenu brzinu tada se povećava komparacijski registar za dvostruku razliku između stvarnih broja impulsa enkodera i zadanog broja impulsa enkodera. Razlika u broju impulsa predstavlja regulacijsko odstupanje, dok broj kojim množimo je faktor proporcionalnog pojačanja K_P =2.

Izmjene komparacijskih registara radi regulacije brzine odvijaju se u prekidnoj rutini ISR(TIMER1_OVF_vect) gdje koristimo varijable uvjeta radi usmjeravanja programa "g" i varijablu broja prolaza "h".

Prekid ISR(TIMER1_OVF_vect) izvodi se frekvencijom 1,9KHz dok se prekid ISR(TIMER1_OVF_vect) izvodi frekvencijom 125Hz koja je više od 15 puta manja, te omogućuje izvođenje regulacije.

Primjer programa gdje se promjena komparacijskih registara ne izvodi u prekidnoj rutini preljeva registra TCNT1 već se izvodi u glavnoj funkciji , a regulacija u funkcijama povecaj() i smanji().

#include <avr/io.h>

#include <avr/interrupt.h>

unsigned char a,b,c,d,e,f,g,h; //a=varijabla broja prekida, b=varijabla broj impulsa enkodera, c=zadani broj impulsa za neku brzinu

//d pomoćna varijabla, e i f varijable razlike u broju impulsa enkodera, g pomoćna varijabla, h varijabla petlje

unsigned int oc; //početna vrijednost komparacijskih registara prema zadanom broju impulsa c

void povecaj(unsigned char);

void smanji(unsigned char);

ISR(TIMER2_COMPA_vect){

a++;

if (a==25){ //kad nabroji do 25 prekida proteklo je 200ms

b=TCNT0; //u varijablu b prebaci stanje registra TCNT0 (broja impulsa s enkodera) u 200ms

TCNT0=0; //očisti vrijednost registra TCNT0 broja impulsa

a=0; //a=0 omogućuje brojanje sljedećih 25 prekida odnosno vrijeme 200ms

h=0; //pomoćna varijabla vidi ISR(TIMER1_OVF_vect)

if (b<c){

e=(c-b)*2;

povecaj(e);} //poziv funkcije za povećanje brzine

else if (b>c){

f=(b-c)*2;

smanji (e);} //poziv funkcije za smanjenje brzine

}

int main (void)

{

DDRB=255;

PORTD=255;

PORTC=255;

/*postavke timera2*/

TCCR2A=(1<<WGM21); //CTC način rada

TCCR2B=(1<<CS22) | (1<<CS21) | (1<<CS20);//pokretanje timera N=1024

OCR2A=125;

/*postavke timera0*/

TCCR0B=(1<<CS02) | (1<<CS01) | (1<<CS00); //postavi counter0 brojanje vanjskih impulsa na uzlani brid

/*postavke timera1 za PWM*/

TCCR1A=(1<<WGM11) | (1<<COM1A1) | (1<<COM1B1); //9-bitni fazni PWM, Povećanje vrijednosti komparacijskog registra povećava napon na izlazu

TCCR1B=(1<<CS11); //pokretanje timera s djeliteljem N=8 /*izlazni napon je 0 jer je po uključenju upisna vrijednost komparacijskih registara OCR1A i OCR1B nula*/

sei(); //globalni prekid

/* Replace with your application code */

{

while (1)

if (d==0){ //izračun varijable oc za zadanu varijablu c koja određuje očekivani broj impulsa ankodera u vremenu

c=100;

oc=c*10/4; //za korišteni motor

while (oc>g){ //ubrzavanje robota do zadane brzine

OCR1A++;

OCR1B++;

g++;

}

TIFR2=(1<<OCF2A); //očisti zastavicu prekida

TIMSK2=(1<<OCIE2A); //pokreni prekid

d=1;

}

void povecaj (unsigned char dif) {

while (dif>h && OCR1A<511 //izvodi program dok varijabla h je manja od e

OCR1A++;

OCR1B++;

h++; //za svaki prolaz povećaj varijablu h

}

void smanji (unsigned char dif) {

while (dif>h && OCR1A>0)

OCR1A--;

OCR1B--;

h++;

}

Drugi način regulacije svodi se na drugi način mjerenja brzine motora. Mjerenje se može izvoditi Timer-om1 upotrebom jedinice za hvatanje, a da se pri tome koristi isti timer za generiranje PWM-a.

Jedinica za hvatanje

Jedinica za hvatanje koristi ICPn ulaz vanjskog događaja koji služi za okidanje jedinice za hvatanje na podešeni brid vanjskog događaja ili jedinicu za hvatanje može pokrenuti analogni komparator kao što se vidi na slici. Ukoliko signal okidanja nije elektronički pripremljen (titranje signala) tada koristimo „Noise canceler“ odnosno filter prije detektora brida.

Kad jedinica za hvatanje detektira brid koji smo postavili trenutno stanje registra TCNT upisuje se u registar jedinice za hvatanje ICRn u 16-bitnoj rezoluciji. Tada se to stanje upisuje u varijablu, te se na sljedeći detektirani brid upisuje novo stanje TCNTn registra. Iz razlike između novog i starog stanja može se izračunati vrijeme i brzina vrtnje motora ili kotača.

Slika 5.

Problem koji se pojavljuje kod mjerenja jedinicom za hvatanje je istovremeno korištenje timera1 za generiranje signala PWM-a. Pri tome se pojavljuje pogreška očitanja zbog korištenja faznog PWM-a.

Slika 6.

Prilikom upisivanja stanja TCNT registra u ICR ne znamo da li se događaj dogodio pri uzlaznom ili silaznom brojanju, te zbog toga nastaje u konačnici pogreška očitanja vremena.

Stoga preduvjet za eliminiranje pogreške mjerenja je korištenje brzog PWM-a kojem smanjimo frekvenciju. Tada postoji samo uzlazni brid signala tako da uvijek znamo ispravnu vrijednost upisanu u ICP1 registar.

Zadržavanje frekvencije PWM je drugi preduvjet što postižemo povećanjem rezolucije PWM na 10-bitnu i dobivamo istu frekvenciju brzog PWM-a kao što je i kod faznog PWM-a.

Ovom metodom mjerimo vrijeme trajanja jednog impulsa enkodera. Ukoliko je brzina vrtnje sporija tada impuls enkodera traje duže vrijeme i broj impulsa je veći i obrnuto.

Slika 7.

Kod detekcije uzlaznog brida signala enkodera upisuje se stanje TCNT1 registra u ICP1 registar na Slici 7. ICP11. U prekidnoj rutini ulazne jedinice za hvatanje stanje ICP1registra sprema se u varijablu što je prije moguće i resetira se varijabla koja broji prekide preljeva TCNT1 registra za potrebe hvatanja. Na drugi uzlazni brid impulsa enkodera ponovno se upisuje novo stanje u ICP1 registar na Slici 7. ICP1₂ i stanje varijable koja broji prekide preljeva TCNT1 registra se sprema.

Ukupan broj impulsa se izračunava prema formuli:

TOP – vrijednost do koje broji TCNT1 prije reseta (za 10-bitnu rezoluciju 1023)

n_TOV – broj prekida preljeva

Za primjer sa Slike 7. i postavljenu frekvenciju PWM-a

Potrebno je obratiti pažnju na broj impulsa koji može premašiti maksimalnu vrijednost varijable „Integer“ te je potrebno za broj impulsa predvidjeti „Long“ tip varijable.

#include <avr/io.h>

#include <avr/interrupt.h>

unsigned int a, b, oc=800, d, g1;

unsigned long f;

unsigned char c, g, g2, h, kp=2, pov, sma, i, j, k;

void korekcija (unsigned long)

ISR(TIMER1_OVF_vect){ //prekid prilikom preljeva registra

d++;

if (OCR1A<oc && OCR1A<1023 && k==0){OCR1A<oc && OCR1A<1023 && k==0){ //izvodi se samo prilikom pokretanja varijabla k

OCR1A++;

OCR1B++;

if (OCR1A==oc){ //oc varijabla se izračunava ovisno o broju impulsa u vremenu mjerenja

TIFR1|=(1<<ICF1); //očisti zastavicu prekida jedinice za hvatanje

TIMSK1|=(1<<ICIE1); //pokreni prekid jedinice za hvatanje

k=1;

}

if (pov>i && OCR1A<1023){

OCR1B++;

OCR1A++;

i++;

c=0;}

else if (sma>j && OCR1A>0){

OCR1A--;

OCR1B--;

j++;

c=0;}

}

ISR(TIMER1_CAPT_vect){ //prekid jedinice za hvatanje

if (c==0){ //varijabla koja određuje pohranu vrijednosti registra ICR1 za prvi odnosno drugi pozitivan brid

a=ICR1; //varijabla u koju se sprema vrijednost registra jedinice za hvatanje

d=0;

c=1;

}

else if (c==1){

b=ICR1; //varijabla u koju se sprema druga vrijednost ICR1 registra

g=d-1;

f=(1023-a)+(g*1024)+b; //izračun broja impulsa

korekcija(f);

}

int main (void)

{

DDRB=0b11111110;

PORTB=0b00000001;

PORTD=255;

/*postavke timera1 za PWM* i jedinicu za hvatanje/

TCCR1A=(1<<WGM11) | (1<<WGM10) | (1<<COM1A1) | (1<<COM1B1); //10-bitni fazni PWM, Povećanje vrijednosti komparacijskog registra povećava napon na izlazu

TCCR1B=(1<<ICES1) | (1<<CS11) | (1<<WGM12); //rastući brid jedinice za hvatanje ,pokrenut timer N=8

TIMSK1=(1<<TOIE1); //prekid preljeva registra TCNT1

sei(); //globalni prekid

void korekcija (unsigned long kor)}

if (kor<3000){ //13736 očekivani broj impulsa za brzinu 218 o/min

sma=(3000-kor)*kp; //regulacijsko odstupanje pomnoženo s faktorom proporcionalnosti

j=0;}

else if (kor>3000){

pov=(kor-3000)*kp;

i=0;}

}

Program je pisan za drugi tip motora tako da služi kao primjer.

U pretprocesorskom dijelu postavlja se brzi PWM 10-bitne rezolucije i pokreče se jedinica za hvatanje, ali ne i njen prekid. Izlaz enkodera mora biti spojen na ulaz jedinice za hvatanje na ICP1 odnosno na PB0. Priključak PB0 postavljen je kao ulazni s podignutim pull-up otpornikom. Varijablom "oc" postavlja se brzina na koju robot ubrzava, te kad u prekidnoj rutini ISR(TIMER1_OVF_vect) postigne traženu brzinu (OCR1A==oc) omogućuje se prekid jedinice za hvatanje.

Kad se detektira prvi uzlazni brid impulsa enkodera program odlazi u ISR(TIMER1_CAPT_vect) gdje ga varijabla c==0 usmjeruje u grananje. U varijablu "a" prebacuje se stanje ICR1 registra u koji se upisalo stanje TCNT1 registra na uzlazni brid impulsa enkodera. Varijabla "2d" koja broji prekide preljeva registra TCNT1 se poništava. Varijabla "c" mijenja stanje u c=1.

Prilikom detekcije drugog uzlaznog brida impulsa enkodera program odlazi ponovno u ISR(TIMER1_CAPT_vect) gdje u varijablu "b" prebacuje novo stanje TCNT1 registara jer je (c==1). U varijablu "g" upisuje broj prekida preljeva TCNT1 registra umanjenog za jedan (vidi Sliku 7.). Izračunava se ukupan broj impulsa između prvog i drugog uzlaznog brida enkodera u varijabli "f", koja je argument funkcije korekcija(f) koja se poziva.

U funkciji korekcija izračunavaju se regulacijski parametri brzine prema očekivanom broju impulsa jedinice za hvatanje. Korekcija se vrši u prekidnoj rutini preljeva registra TCNT1 prema uvjetima postavljenim u funkciji korekcija.

Program se odvija u prekidnim rutinama i jednoj funkciji. Poboljšanje programa ide u smjeru produljenja impulsa enkodera, tako da se smanji broj prekida jedinice za hvatanje. Korištenjem enkodera na motoru impulsi enkodera su kratki i često pokrećemo prekid jedinice za hvatanje. Impuls enkodera možemo produljiti ako enkoder izvedemo na kotaču.

Enkoder izveden na kotaču

Enkoder izveden na kotaču mobilnog robota ima manju frekvenciju impulsa i širi impuls. Kotač mobilnog robota ima na rupe koje služe refleksnom optičkom senzoru kao enkoder.

Slika 8.

Kotač sa slike ima 5 rupa pa pri brzini od 218o/min odnosno 3,633o/s ima frekvenciju:

Pri toj frekvenciji impuls enkodera traje Δt=55ms, dok na enkoderu u motoru traje Δt=6,8ms.

Očekivani broj impulsa jedinice za hvatanje kod 1 impulsa enkodera (1/5 kotača) pri brzini od 218o/min i frekvenciji impulsa 18.16Hz iznosi:

Očekivani broj prekida preljeva registra:

Pri ovoj metodi mjerenja brzine vrtnje regulacijsko odstupanje može biti prikazano relativno velikim brojem, pa ga je potrebno dijeliti da bi se dobio podatak upotrebljiv za korekciju PWM-a.

Na primjer za frekvenciju impulsa enkodera 18,20Hz broj impulsa je:

Odstupanje od izračunatog broja impulsa:

Brzine vrtnje kotača za novu frekvenciju impulsa enkodera od 18,20Hz:

Promjena brzine vrtnje Δn=0,4 o/min stvara razliku od 242 impulsa enkodera.

Promjena komparacijskog registra za ±242 ima za posljedicu promjenu napona motora za ±2,8V kod 10-bitnog PWM-a. Promjena napona na motoru od ±2,8V mijenja brzinu vrtnje motora za ±70 o/min, a potrebna korekcija iznosi 0,4 o/min.

Potrebna korekcija komparacijskih registara za promjenu brzine od 0,4 o/min iznosi ±1,4 odnosno ±1. Dobiveno regulacijsko odstupanje potrebno je podijeliti s 242.

Treći način mjerenja brzine vrtnje na kotaču može se izvesti tako da se impulsi enkodera dovode na ulaze vanjskog prekida. Impulse je prethodno potrebno elektronički pripremiti jer ulaz vanjskog prekida ne posjeduje „Noise canceler“ odnosno filter za otklanjanje titranja.

Na prvi rastući brid signala enkodera u prekidnoj rutini vanjskog prekida pokrenemo timer koji ne koristimo za PWM (Timer0 ili Timer2) u normalnom načinu rada, s omogućenim prekidom preljeva registra timera.

Na drugi rastući brid signala enkodera zaustavimo timer, prebacimo vrijednost TCNT registra u varijablu, te poništimo TCNT registar TCNTn=0.

Iz broja prekida i varijable u koju smo prenijeli stanje TCNT registra nakon zaustavljanja timera izračunamo broj impulsa timera od prvog do drugog rastućeg brida signala enkodera. Iz broja impulsa i frekvencije timera izračunamo vrijeme, te izvučemo podatak o brzini vrtnje.

Nadalje vršimo regulaciju kao što je opisano u prethodnim programima.

Regulacija brzine vrtnje pojedinog kotača

Mobilni robot s diferencijalnim pogonom ima dva motora koja neovisno pogone mobilni robot. Skretanje robota izvodi se kad brzine vrtnje kotača nisu iste, te ovisno o razlici brzina vrtnje i razmaku između kotača ovisi i skretanje robota.

U primjeru "Regulacija brzine vrtnje motora" pretpostavili smo da su oba motora istih karakteristika pa im je i brzina vrtnje ista. U realnoj primjeni to nije tako pa moramo zasebno mjeriti brzine vrtnje oba kotača.

Ukoliko koristimo mikroupravljač ATmega328P (Arduino Uno) koji ima tri timera nije moguće istodobno mjeriti brzinu na oba kotača što otežava programiranje i zahtjeva dodatne sklopove, te produljuje vrijeme mjerenja a time i reakciju regulacije.

Ukoliko koristimo mikroupravljač ATmega2560 (Arduino Mega) koji ima četiri 16-bitna timera i dva 8 bitna timera moguće je istovremeno mjerenje i generiranje signala PWM signala što u konačnici olakšava programiranje.

Razmak između kotača robota

Slika 1.

D - razmak između korača robota

D/2 - udaljenost kotača od simetrale robota

Što je razmak između kotača veći to će razlika u brzini vrtnje motora manje utjecati na skretanje motora. Drugim riječima ako je razmak između kotača veći razlike između brzina vrtnje motora moraju biti veće za određeno skretanje.

Slika 2.

Skretanje robota može se izvoditi naviše načina:

Zakretanje oko točke u ravnini gibanja robota
Zakretanje oko jednog kotača robota
Vrtnja oko centralne točke robota

Zakretanja oko točke u ravnini

Srednja brzina robota izračunava se:

Gdje je:

v_s – srednja brzina

n_s – srednja brzina okretaja

v_L , v_D - brzine lijevog i desnog kotača

r – polumjer kotača d/2

d – promjer kotača

n_L , n_D – broj okretaja lijevog i desnog kotača

Robot se zakreče oko centralne točke na sjecištu simetrale robota i simetrale osovina kotača.

Slika 3.

Robot se giba po pravcu
Robot se zakreče tako da centralna točka ima stalnu brzinu i zakretanje se vrši po radijusu zakretanja r_ZAK oko točke zakretanja T_ZAK. Da bi se brzina centralne točke zadržala konstantnom kao i kad se robot kretao po pravcu potrebno je brzinu vrtnje kotača mijenjati ta istu promjenu Δn. Za Δn se poveća brzina lijevog kotača, a za istu promjenu Δn se smanji brzina desnog kotača.
Kad robot dođe u novu poziciju, nastavi se kretati novim pravcem tako da se razlike brzina pojedinog kotača svedu na nulu Δn=0.

Vrlo često nije moguće zadržati centralnu brzinu konstantnom već ju je potrebno smanjiti zbog tehničkih ograničenja. Na primjer ukoliko motori rade s velikim faktorom vođenja, te se zahtijeva se velika promjena brzine motora da bi se izvelo zakretanje, nije moguće dovoljno ubrzati motor. Tada se mora smanjiti brzina da bi se izvelo traženo zakretanje.

Zakretanje oko jednog kotača

Jednostavnije zakretanje vrši se tako da jedan motor zadržava brzinu vrtnje dok drugi motor zakočimo. Točka oko koje se robot okreče seli se iz centralne pozicije u zakočeni kotač. Centralna točka robota kružno se giba oko zakočenog kotača.

Brzina kotača koji kruži oko drugog kotača je dvostruko veća od brzine centralne točke.

Često se koristi kod jednostavnijih putanji robota (praćenje crte).

Slika 4.

Robot se giba po pravcu
Kad centralna točka robota dođe na polovinu udaljenosti od razmaka između kotača desni motor se zaustavlja (zakoči), a lijevi motor nastavlja s vrtnjom
Kad se robot zakrene za traženi kut desni motor se nastavlja okretati i robot se giba po novom pravcu

Vrtnja oko centralne točke

Ukoliko promijenimo smjer vrtnje jednog kotača robot će se vrtjeti oko centralne točke.

Zakretanje u centralnoj točki

Slika 5.

Robot se giba po pravcu
Kad centralna točka robota (točka simetrale robota i kotača) dođe u točku zakretanja lijevi motor zadržava smjer vrtnje dok desni motor mijenja smjer vrtnje
Kad se robot zakrene za traženi kut desni motor se vrača u prvobitni smjer vrtnje i robot se nastavlja gibati po novom pravcu

Brzina centralne točke je nula jer se oko nje robot zakreče, odnosno ona je centar rotacije.

Program u kojem robot zadržava gibanje po pravcu (robot ide ravno)

Program pisan za Atmega2560 i odabrani motor s enkoderom. Iz tehničkih podataka motora može se odrediti minimalna udaljenost između korača robota. Procjenjuje se na 440mm.

Poveznica na tehničke podatke ATMega2560:

https://ww1.microchip.com/downloads/en/DeviceDoc/ATmega640-1280-1281-2560-2561-Datasheet-DS40002211A.pdf

Timer1 – generiranje faznog 9-bitnog PWM-a

Counter3 – Brojač impulsa enkodera lijevog kotača

Counter4 – Brojač impulsa enkodera desnog kotača

Timer5 – Generator vremena prikupljanja impulsa s enkodera t=200ms

Koristit će se podaci iz primjera „Regulacija brzine vrtnje motora“.

#include <avr/io.h>

#include <avr/interrupt.h>

unsigned int a,b;

unsigned char c=2; //c=pojačanje proporcionalnog člana Kp

/*najave vlastitih funkcija*/

void brzina (void);

void korekd (void);

void korekl (void);

ISR(TIMER5_COMPA_vect){

a=TCNT3; //broj impulsa enkodera lijevog motora

b=TCNT4; //broj impulsa enkodera desnog motora

TCNT3=0; //poništenje registra impulsa lijevog enkodera

TCNT4=0; //poništenje registra impulsa desnog enkodera

if (a==b) //ako je broj impulsa oba enkodera isti

brzina(); //poziv funkcije korekcije brzine

else if (a>b) //ako enkoder lijevog motora ima više impulsa

korekl(); //poziv funkcije korekcije brzine lijevog motora

else if (a<b) //ako enkoder desnog motora ima više impulsa

korekd(); //poziv funkcije korekcije brzine desnog motora

}

int main (void)

{ /*postavke priključaka*/

PORTA=255;

DDRB=255;

DDRC=255;

PORTD=255;

PORTE=255;

PORTF=255;

DDRG=255;

DDRH=255;

PORTJ=255;

PORTL=255;

PORTK=255;

/*Postavke PWM-a na timer1 kanal A i B*/

TCCR1A=(1<<WGM11) | (1<<COM1A1) | (1<<COM1B1); //9-bitni fazni PWM

TCCR1B=(1<<CS11); //pokretanje PWM-a N=8

/*Postavke Counter3 i Counter4 za brojanje impulsa enkodera*/

TCCR3B=(1<<CS32) | (1<<CS31) | (1<<CS30); //vanjski rastući impuls na T3

TCCR4B=(1<<CS42) | (1<<CS41) | (1<<CS40); //vanjski rastući impuls na T4

/*Postavke timera5 za mjerenje vremena 200ms*/

TCCR5B=(1<<WGM52) | (1<<CS52) | (1<<CS50); //CTC način za 200ms broji do 3125 N=1024

OCR5A=3125;//CTC TOP vrijednost za fCPU=16KHz, N=1024 t=200ms

TIMSK5=(1<<OCIE5A);

sei();

/* Replace with your application code */

while (1)

{

}

/*korekcija brzine robota*/

void brzina (void){

unsigned char i=0,j=0,el=0,ed=0;

if (a<29 && b<29){

el=(29-a)*c; //regulacijsko odstupanje lijevog kotača

ed=(29-b)*c; //regulacijsko odstupanje desnog kotača

while (el>i && OCR1<511){

OCR1A++;

i++;

}

while (ed>j && OCR1B<511){

OCR1B++;

j++;

}

else if (a>29 && b>29){

el=(a-29)*c; //regulacijsko odstupanje lijevog kotača

ed=(b-29)*c; //regulacijsko odstupanje desnog kotača

while (el>i && OCR1A>0){

OCR1A--;

i++;

}

while (ed>j && OCR1B>0){

OCR1B--;

j++;

}

/*korekcija brzine kad je lijevi kotač brži*/

void korekl (void){

unsigned char i=0,el=0;

el=(a-b)*3; //varijabla razlike u broju impulsa za lijevi motor

while (el>i && OCR1A>0 && OCR1B<511){

OCR1A--;

OCR1B++;

i++;

}

/*korekcija brzine kad je desni kotač brži*/

void korekd (void){

unsigned char j=0,ed=0;

ed=(b-a)*3; //varijabla razlike u broju impulsa za desni motor

while (ed>j && OCR1A<511 && OCR1B>0){

OCR1A++;

OCR1B--;

j++;

}

Opis programa

U prekidnu rutinu ISR(TIMER5_COMPA_vect) odlazi se svakih 200ms te se u varijable a i b prebacuju vrijednosti TCNT3 registra koji se puni impulsima enkodera lijevog motora i TCNT4 registra koji se puni impulsima iz enkodera desnog motora. Nakon provjera varijabli a i b kroz uvjete pozivaju se odgovarajuće funkcije. U funkciji brzina() vrši se korekcija brzine robota i ubrzavanje, a u ostalim funkcijama vrši se korekcija brzina kad se ustanovi razlika u brzinama vrtnje motora.

Rezolucija broja okretaja u ovisnosti o broju impulsa:

Ukoliko se registrira razlika od jednog impulsa enkodera maksimalna razlika brzina vrtnje pojedinog kotača je 7,52 o/min. U proračunu korekcije uzimam ½ vrijednosti rezolucije.

Slika 6.

Rezolucija pokazuje razliku u brzini koja se odnosi na jedan impuls odnosno cijelo vrijeme trajanja impulsa, a realna razlika je samo dio vremena trajanja impulsa, pa stoga se uzima ½ rezolucije kao približnu vrijednost.

Promjenom komparacijskog registra za ±1 mijenja se brzina vrtnje za 0,58 o/min.

Da bi smo promijenili brzinu vrtnje za pola vrijednosti rezolucije odnosno za 3,76 o/min moramo promijeniti OCR1A/B za ±6.

Ukoliko prilikom promjene brzine želimo zadržati srednju brzinu konstantnom tada brži kotač robota usporimo smanjenjem vrijednosti komparacijskog registra za 3, a sporiji kotač ubrzamo povećanjem vrijednosti komparacijskog registra za 3 što daje potrebnu promjenu komparacijskih registara od 6.

Zanima nas pogreška putanje robota s razlikom u brzini vrtnje pojedinog kotača od 3,76 o/min u vremenskom trajanju do rezultata slijedećeg mjerenja i korekcije od 0,2s.

Razmak između kotača robota iznosi 440mm. Razlika brzine vrtnje jednog kotača zakreče robot oko drugog kotača, odnosno možemo zamisliti da jedan kotač zakočimo , a drugi se vrti brzinom od 3,76 o/min.

Put koji prođe kotač promjera d=7cm uz brzinu vrtnje od n=3,76 o/min:

Iz formule za duljinu kružnog luka:

Slika 7.

Izračunavamo kut a gdje je duljina kružnog luka l=0,003m pređeni put korača, a polumjer rotacije razmak između kotača r=0,44m:

Novonastala pozicija robota:

Slika 8.

Na slici se vidi nova putanja robota, te ukoliko se nakon korekcije brzine na jednom kotaču nastavi gibanje robota po pravcu tada će se robot nastaviti kretati u krivom smjeru, odnosno odstupit će od predviđene putanje.

Potrebno je robot vratiti na prvobitnu putanju ubrzavanjem sporijeg kotača procijenjeno vrijeme (u primjeru 50ms).

/*korekcija brzine kad je lijevi kotač brži*/

void korekl (void) {

unsigned char i=0,el=0;

unsigned int ocb;

el=(a-b)*4; //varijabla razlike u broju impulsa za lijevi motor

while (el>i && OCR1A>0 && OCR1B<511){ //promjena komparacijskih registara dok nisu zadovoljeni uvjeti

OCR1A--;

OCR1B++;

i++;

}

ocb=OCR1B; //spremanje vrijednosti komparacijskog registra sporijeg kotača

i=0;

while (el>i && OCR1B<511){ //promjena komparacijskih registara dok nisu zadovoljeni uvjeti

OCR1B++;

i++;

}

_delay_ms(50); //procijenjeno vrijeme travanja povrata u prvobitnu putanju

OCR1B=ocb; //postavka vrijednosti komparacijskog registra na prije ispravljenu vrijednost

}

/*korekcija brzine kad je desni kotač brži*/

void korekd (void) {

unsigned char j=0,ed=0;

unsigned int oca;

ed=(b-a)*4; //varijabla razlike u broju impulsa za desni motor

while (ed>j && OCR1A<511 && OCR1B>0){

OCR1A++;

OCR1B--;

j++;

}

oca=OCR1A;

j=0;

while (ed>j && OCR1A<511){ //promjena komparacijskih registara dok nisu zadovoljeni uvjeti

OCR1A++;

j++;

}

_delay_ms(50); //procijenjeno vrijeme travanja povrata u prvobitnu putanju

OCR1A=oca; //postavka vrijednosti komparacijskog registra na prije ispravljenu vrijednost

}

Iz analize mjerenja i regulacije dolazimo do stvarne putanje robota. Što je broj impulsa u jedinici vremena veći to je rezolucija bolja i vrijeme mjerenja se može skratiti. U konačnici dobivamo precizniju zadanu putanju robota, odnosno kut zakretanja robota će biti manji.

Programom ubrzavamo robot do maksimalne brzine profila gibanja. Eliminiramo promjenu opterećenja kod jednolikog gibanja i omogućujemo robotu ravno gibanje po pravcu. Faza usporavanja nije izvedena u programu.

Na isti način moguće izvesti mjerenje jedinicom za hvatanje. Potrebno je promijeniti vrstu PWM-a, mjerenje impulsa izvoditi na kotačima.

Ukoliko koristimo mikroupravljač s manje mogućnosti ATMega328P koji je ugrađen u ArduinoUno tada je potrebno dodati vanjske elektroničke sklopove kojima eliminiramo smetnje i usmjerujemo podatke na ulaz Counter0. Na Counter0 moramo obraditi ulaze za oba motora.

Slika 9.

Prvih 200ms brojimo impulse s jednog enkodera, a drugih 200ms s drugog enkodera, te nakon obavljenog prikupljanja impulsa vršimo regulaciju. Regulacija je u konačnici manje precizna nego kod izvedbe s ATMega2560.

Na priključak „ODABIR MOTORA“ priključujemo izlaz mikroupravljača na koje proslijedimo izlaz CTC timera u izmjeničnom režimu rada ili ga programom u prekidnoj rutini proslijedimo na bilo koji izlaz. Vidi dokumentaciju proizvođača mikroupravljača.

Promjena putanje robota

U primjeru „Regulacija brzine vrtnje pojedinog kotača“ prikazan je način održavanja ravne putanje robota, ali osim ravne putanje robot mora ostvariti zakretanje. Zakretanje ovisi o zadanoj putanji i očitanjima senzora kojima detektiramo predmete u okolini robota.

Slijedeći izazov je zadati robotu zakretanje oko točke rotacije u ravnini po kojoj se robot kreče odnosno zadati polumjer R_zak i kut rotacije a.

Slika 1.

Pored toga potrebno je odrediti približnu poziciju robota u prostoru u odnosu na polaznu točku ili u donosu na neku referentnu točku u prostoru što se postiže mjerenjem puta pojedinog kotača.

Podatak o putu također dobivamo iz inkrementalnog enkodera motora, pa za naš robot vrijedi:

Gdje je:

i – prijenosni omjer reduktora (13,44)

l_k – duljina koju prođe kotač za broj impulsa enkodera n_enc [m]

d_k – promjer kotača [m]

n_okr – broj impulsa enkodera po jednom okretaju kotača (3)

n_enc – broj prikupljenih impulsa enkodera (podatak u TCNTn registru Counter-a)

C_enc – konstanta enkodera [m/imp]

Prilikom zakretanja robota mora se zadati kut zakretanja i polumjer oko točke rotacije.

Slika 2.

v_L – brzina lijevog kotača

v_D – brzina desnog kotača

D – razmak između kotača

R_zak – polumjer zakretanja

T_zak – točka zakretanja

w – kutna brzina zakretanja

α - kut zakretanja

Osnovne formule zakretanja:

Brzina lijevog i desnog kotača:

Kutna brzina:

Polumjer zakretanja:

Duljina kružnog luka centralne točke robota:

Duljina kružnog luka desnog kotača:

Duljina kružnog luka lijevog kotača:

Postupak proračuna koji se mora implementirati u program:

Pretpostavka s kojom se polazi u programiranje je zadržavanje brzine centralne točke robota konstantnom i prilikom zakretanja:

Uz zadani polumjer zakretanja potrebno je izračunati promjenu brzine okretaja desnog i lijevog kotača:

Polazi se iz formule:

ili direktno u o/min:

Brzina centralne točke je u stvari brzina robota. U našem primjeru je brzina robota v=0,8m/s odnosno brzina okretaja iznosi 218 o/min.

Maksimalna brzina koju motor može postići uz nominalni napon od 12V i moment M=0,2Nm iznosi 256 o/min (vidi „Izračuna napona motora“ tablica 2.“). Iz navedenog slijedi nemogućnost ubrzavanja kotača na proračunatu brzinu, odnosno nemoguće je zadržati srednju brzinu konstantnom .

Potrebno je izračunati maksimalnu srednju brzinu uz maksimalnu brzinu lijevog kotača kod nominalnog opterećenja i nominalnog napona uz faktor vođenja Ds=1 za lijevi kotač.

Brzina centralne točke kod maksimalne brzine lijevog kotača iznosi:

Brzina desnog kotača iznosi:

Brzina lijevog kotača n_L =256 o/min.

Provjera polumjera zakretanja:

Stvarna centralna brzina:

Provjerom polumjera zakretanja i centralne brzine provjerili smo proračun.

Nadalje slijedi proračun duljine puta za zadani kuta zakretanja koji se počinje računati od trenutka kad započne zakretanje do trenutka kad zakretanje završi uz konstantni polumjer zakretanja.

Slika 3.

U sljedećem razmatranju potrebno je robot zakrenuti za zadani kut zakretanja. Kut zakretanja dobivamo iz podatka enkodera o prevaljenom putu.

i duljine kružnog luka

Broj impulsa enkodera za kružni luk koji obavlja desni kotač za zadani kut zakretanja α:

odnosno lijevi kotač

Ukoliko zadamo da se robot mora zakrenuti za 30° izračunavamo potreban broj impulsa enkodera pojedinog kotača:

Izmijenjeni program iz primjera „Regulacija brzine vrtnje pojedinog kotača“ s ciljem dobivanja podatka o ukupnom putu pojedinog kotača. Potrebno je primijetiti izmjenu u ISR(TIMER5_COMPA_vect) gdje se ne poništava stanje TCNTn registara jer su ti podaci potrebni za izračun puta. U funkciji „void put()“ izračunava se pređeni put pojedinog kotača u [m], a funkcija se poziva po potrebi.

#include <avr/io.h>

#include <avr/interrupt.h>

#define ce 1/183.35 //konstanta enkodera

unsigned int a,b,a0,b0,a1,b1,el,ed,ovf3,ovf4;

unsigned char c=2; //c=pojačanje proporcionalnog člana Kp

unsigned long sl,sd;

/*najave vlastitih funkcija*/

void put (void);

ISR(TIMER3_OVF_vect){ //prekid preljeva registra

ovf3++;

}

ISR(TIMER4_OVF_vect){ //prekid preljeva registra

ovf4++;

}

ISR(TIMER5_COMPA_vect){

a0=TCNT3; //broj impulsa enkodera lijevog motora

b0=TCNT4; //broj impulsa enkodera desnog motora

if (a0<a1){ //ukoliko je došlo do preljeva TCNT3 registra

a=(65536-a1)+a0;

a1=a0;}

else{

a=a0-a1; //razlika između broja impulsa na lijevom kotaču

a1=a0;} //varijabla a1 poprima vrijednost varijable a0 prijašnjeg mjerenja

if (b0<b1){ //ukoliko je došlo do preljeva TCNT4 registra

b=(65536-b1)+b0;

b1=b0;}

else {

b=b0-b1; //razlika između broja impulsa na desnom kotaču

b1=b0;}

}

int main (void)

{ /*postavke priključaka*/

PORTA=255;

DDRB=255;

DDRC=255;

PORTD=255;

PORTE=255;

PORTF=255;

DDRG=255;

DDRH=255;

PORTJ=255;

PORTL=255;

PORTK=255;

/*Postavke PWM-a na timer1 kanal A i B*/

TCCR1A=(1<<WGM11) | (1<<COM1A1) | (1<<COM1B1); //9-bitni fazni PWM

TCCR1B= (1<<CS11); //pokretanje PWM-a N=8

/*Postavke Counter3 i Counter4 za brojanje impulsa enkodera*/

TCCR3B=(1<<CS32) | (1<<CS31) | (1<<CS30); //vanjski rastući impuls na T3

TCCR4B=(1<<CS42) | (1<<CS41) | (1<<CS40); //vanjski rastući impuls na T4

/*postavke prekida preljeva radi proračuna pređenog puta*/

TIMSK3=(1<<TOIE3); //omogućen prekid preljeva registra radi ukupnog broja impulsa enkodera lijevog kotača

TIMSK4=(1<<TOIE4); //omogućen prekid preljeva registra radi ukupnog broja impulsa enkodera desnog kotača

/*Postavke timera5 za mjerenje vremena 200ms*/

TCCR5B=(1<<WGM52) | (1<<CS52) | (1<<CS50); //CTC način za 200ms broji do 3125 N=1024

OCR5A=3125; //CTC TOP vrijednost za fCPU=16KHz, N=1024 t=200ms

TIMSK5=(1<<OCIE5A);

sei();

/* Replace with your application code */

while (1)

{

//kad je potrebna informacija o pređenom putu pojedinog kotača poziva se funkcija put();

}

void put (void){

sl=(ovf3*65536+TCNT3)*ce;

sd=(ovf4*65636+TCNT4)*ce;

}

Nadalje moramo izraditi funkcije zakretanja robota zadavanjem polumjera zakretanja i kuta za koji se robot mora zakrenuti. Robot zakreče tako da se u beskonačnoj while(1) petlji vrši ispitivanje senzora ili promjenu stanja nekog ulaznog priključaka, te se poziva funkcija zakretanja.

Kad robot zakreče tada moramo onemogućiti sustav koji regulira ravnu putanju robota, jer se to protivi regulaciji. Regulaciju isključujemo tako da onemogućimo prekid ISR(TIMER5_COMPA_vect) u kojem uzimamo uzorke impulsa enkodera svakih 200ms.

Osim navedenog potrebno je promjenu brzine izvesti promjenom OCR1A/B registra te program prilagoditi graničnim vrijednostima registara.

Da bi se program brže odvijao potrebno je definirati konstante koje se koriste u programu, a vezane su uz dimenzije robota, konstantu enkodera i slično.

Broj impulsa enkodera za brzinu vrtnje kotača 218 o/min iznosi 29.

#include <avr/io.h>

#include <avr/interrupt.h>

#define F_CPU 16000000UL // 16 MHz

#include <util/delay.h>

#define ce 0.00545 //konstanta enkodera

#define cent 0.8 //konstanta centralne brzine

#define ns 218 //konstanta centralne brzine vrtnje kotača

#define d 0.44 //konstanta razmak između kotača

#define d2 0.22 //konstanta 1/2 razmaka između kotača

unsigned int a,b,a0,b0,a1,b1,ovf3,ovf4;

unsigned char c=2,nprom,nimpl,nimpd; //c=pojačanje proporcionalnog člana Kp

unsigned long sl,sd;

/*najave vlastitih funkcija*/

void brzina (void);

void korekl (void);

void korekd (void);

void put (void);

void zakrecil (float,float);

void zakrecid (float,float);

ISR(TIMER3_OVF_vect){ //prekid preljeva registra

ovf3++;

}

ISR(TIMER4_OVF_vect){ //prekid preljeva registra

ovf4++;

}

ISR(TIMER5_COMPA_vect){

a0=TCNT3; //broj impulsa enkodera lijevog motora

b0=TCNT4; //broj impulsa enkodera desnog motora

if (a0<a1){ //ukoliko je došlo do preljeva TCNT3 registra

a=(65536-a1)+a0;

a1=a0;}

else{

a=a0-a1; //razlika između broja impulsa na lijevom kotaču

a1=a0;} //varijabla a1 poprima vrijednost varijable a0 prijašnjeg mjerenja

if (b0<b1){ //ukoliko je došlo do preljeva TCNT4 registra

b=(65536-b1)+b0;

b1=b0;}

else {

b=b0-b1; //razlika između broja impulsa na desnom kotaču

b1=b0;}

if (a==b) //ako je broj impulsa u vremenu od 200ms oba enkodera isti

brzina(); //poziv funkcije korekcije brzine

else if (a>b) //ako enkoder lijevog motora ima više impulsa

korekl(); //poziv funkcije korekcije brzine lijevog motora

else if (a<b) //ako enkoder desnog motora ima više impulsa

korekd(); //poziv funkcije korekcije brzine desnog motora

}

int main (void)

{ /*postavke priključaka*/

PORTA=255;

DDRB=255;

DDRC=255;

PORTD=255;

PORTE=255;

PORTF=255;

DDRG=255;

DDRH=255;

PORTJ=255;

PORTL=255;

PORTK=255;

/*Postavke PWM-a na timer1 kanal A i B*/

TCCR1A=(1<<WGM11) | (1<<COM1A1) | (1<<COM1B1); //9-bitni fazni PWM

TCCR1B= (1<<CS11);//pokretanje PWM-a N=8

/*Postavke Counter3 i Counter4 za brojanje impulsa enkodera*/

TCCR3B=(1<<CS32) | (1<<CS31) | (1<<CS30); //vanjski rastući impuls na T3

TCCR4B=(1<<CS42) | (1<<CS41) | (1<<CS40); //vanjski rastući impuls na T4

/*postavke prekida preljeva radi proračuna pređenog puta*/

TIMSK3=(1<<TOIE3); //omogućen prekid preljeva registra radi ukupnog broja impulsa enkodera lijevog kotača

TIMSK4=(1<<TOIE4); //omogućen prekid preljeva registra radi ukupnog broja impulsa enkodera desnog kotača

/*Postavke timera5 za mjerenje vremena 200ms*/

TCCR5B=(1<<WGM52) | (1<<CS52) | (1<<CS50); //CTC način za 200ms broji do 3125 N=1024

OCR5A=3125; //CTC TOP vrijednost za fCPU=16KHz, N=1024 t=200ms

TIMSK5=(1<<OCIE5A);

sei();

/* Replace with your application code */

while (1)

{

//kad je potrebna informacija o pređenom putu pojedinog kotača poziva se funkcija put();

if (!(PINJ&(1<<PINJ3)))

zakrecil(0.6,30);

}

/*korekcija brzine robota*/

void brzina (void){

unsigned char i=0,j=0,el=0,ed=0;

if (a<29 && b<29){

el=(29-a)*c; //regulacijsko odstupanje lijevog kotača

ed=(29-b)*c; //regulacijsko odstupanje desnog kotača

while (el>i && OCR1A<511){

OCR1A++;

i++;

}

while (ed>j && OCR1B<511){

OCR1++;

j++;

}

else if (a>29 && b>29){

el=(a-29)*c; //regulacijsko odstupanje lijevog kotača

ed=(b-29)*c; //regulacijsko odstupanje desnog kotača

while (el>i && OCR1A>0){

OCR1A--;

i++;

}

while (ed>i && OCR1B>0){

OCR1B--;

j++;

}

void korekl (void){

unsigned char i=0,el=0;

unsigned int ocb;

el=(a-b)*4; //varijabla razlike u broju impulsa za lijevi motor

while (el>i && OCR1A>0 && OCR1B<511){ //promjena komparacijskih registara dok nisu zadovoljeni uvjeti

OCR1A--;

OCR1B++;

i++;

}

ocb=OCR1B; //spremanje vrijednosti komparacijskog registra sporijeg kotača

i=0;

while (el>i && OCR1B<511){ /ubrzavanje sporijeg kotača (povrat robota u prvobitnu putanju)

OCR1B++;

i++;

}

_delay_ms(50); //procijenjeno vrijeme travanja povrata u prvobitnu putanju

OCR1B=ocb; //postavka vrijednosti komparacijskog registra na prije ispravljenu vrijednost

}

void korekd (void){

unsigned char j=0,ed=0;

unsigned int oca;

ed=(b-a)*4; //varijabla razlike u broju impulsa za desni motor

while (ed>j && OCR1A<511 && OCR1B>0){

OCR1A++;

OCR1B--;

j++;

}

oca=OCR1A;

j=0;

while (ed>j && OCR1A<511){

OCR1A++;

j++;

}

_delay_ms(50);

OCR1A=oca;

}

void put (void){

sl=(ovf3*65536+TCNT3)*ce;

sd=(ovf4*65636+TCNT4)*ce;

}

void zakrecil (float rad , float kut){

int oca,ocb,nd,nl,nsz,nimpl,nimpd;

oca=OCR1A; //zapamti vrijednost komparacijskih registara prije zakretanje

ocb=OCR1B;

TIMSK5&=~(1<<OCIE5A); //onemogući prekid uzimanja uzoraka za regulaciju svakih 200ms

/*provjera brzine i prilagodba na zadani polumjer zakretanja*/

nd=ns*(1+(d2/rad)); //izračunaj potreban broj okretaja uz zadržavanje centralne brzine robota

if (nd>256){ //ako je veća od maksimalne brzine za moment

nsz=256/(1+(d2/rad)); //izračunaj novu srednju brzinu uz maksimalnu brzinu za moment

nl=nsz*(1-(d2/rad)); //izračunaj brzinu lijevog kotača

nd=256;} //brzina desnog kotača (maksimalna za moment)

else

nl=ns*(1-(d2/rad)); //ako izračunata brzina nd je manja od maksimalne tada izračunaj nl

/*izračun vrijednosti OCR1n*/

if (nd==256){ //ako je brzina desnog kotača jednaka maksimalnoj za moment

OCR1B=511; //faktor vođenja desnog kotača Ds=1

OCR1A=nl/0.5;} //0,5=256/511 (koeficijent smjera pravca OCR1n i broja okretaja)

else {

OCR1B=nd/0.5;

OCR1A=nl/0.5;}

/*izračun broja impulsa enkodera za zadani kut*/

nimpd=(rad+d2)*96; //96=3,14*30/180*ce

nimpl=(rad-d2)*96;

/*brojanje impulsa enkodera za zadani kut*/

a0=TCNT3; //broj impulsa enkodera lijevog motora

b0=TCNT4; //broj impulsa enkodera desnog motora

a1=a0+nimpl; //potreban broj impulsa zakretanja za zadani kut lijevog motora

b1=b0+nimpd; //potreban broj impulsa zakretanja za zadani kut desnog motora

if (a0<a1 && b0<b1){ //ako razlika broja impulsa ne generira prekid preljeva registra TCNT3 i 4

while (a<a1 && b<b1){ //dok je zadovoljen uvjet robot zakreče i prikuplja broj impulsa

a=TCNT3;

b=TCNT4;}

OCR1A=oca; //vrati PWM na prije zakretanja

OCR1B=ocb;

TIFR5&=~(1<<OCF5A); //poništi zastavicu CTC-a

TIMSK5=(1<<OCIE5A);} //omogući prekid CTC-a odnosno regulaciju brzine

else if (a0>a1 || b0>b1){ //ako razlika broja impulsa generira prekid preljeva registra TCNT3 ili 4

while (a>a0 || b>b0){

a=TCNT3;

b=TCNT4;}

while (a<a1 || b<b1){

a=TCNT3;

b=TCNT4;}

OCR1A=oca; //vrati PWM na prije zakretanja

OCR1B=ocb;

TIFR5&=~(1<<OCF5A); //poništi zastavicu CTC-a

TIMSK5=(1<<OCIE5A);} //omogući prekid CTC-a

}

void zakrecid (float rad , float kut){

int oca,ocb,nd,nl,nsz,nimpl,nimpd

oca=OCR1A;

ocb=OCR1B;

TIMSK5&=~(1<<OCIE5A); //onemogući prekid

/*provjera brzine i prilagodba na zadani polumjer zakretanja*/

nl=ns*(1+(d2/rad));

if (nl>256){

nsz=256/(1+(d2/rad));

nd=nsz*(1-(d2/rad));

nl=256;}

else

nd=ns*(1-(d2/rad));

/*izračun vrijednosti OCR1n*/

if (nl==256){

OCR1A=511;

OCR1B=nd/0.5;} //0,5=256/511

else {

OCR1A=nl/0.5;

OCR1B=nd/0.5;}

/*izračun broja impulsa enkodera za zadani kut*/

nimpl=(rad+d2)*96; //96=3,14*30/180*ce

nimpd=(rad-d2)*96;

/*brojanje impulsa enkodera za zadani kut*/

a0=TCNT3; //broj impulsa enkodera lijevog motora

b0=TCNT4; //broj impulsa enkodera desnog motora

a1=a0+nimpl; //potreban broj impulsa zakretanja za zadani kut lijevog motora

b1=b0+nimpd; //potreban broj impulsa zakretanja za zadani kut desnog motora

if (a0<a1 && b0<b1){ //ako razlika broja impulsa ne generira prekid preljeva registra TCNT3 i 4

while (a<a1 && b<b1){ //dok je zadovoljen uvjet robot zakreče i prikuplja broj impulsa

a=TCNT3;

b=TCNT4;}

OCR1A=oca; //vrati PWM na prije zakretanja

OCR1B=ocb;

TIFR5&=~(1<<OCF5A); //poništi zastavicu CTC-a

TIMSK5=(1<<OCIE5A);} //omogući prekid CTC-a odnosno regulaciju brzine

else if (a0>a1 || b0>b1){ //ako razlika broja impulsa generira prekid preljeva registra TCNT3 ili 4

while (a>a0 || b>b0){

a=TCNT3;

b=TCNT4;}

while (a<a1 || b<b1){

a=TCNT3;

b=TCNT4;}

OCR1A=oca; //vrati PWM na prije zakretanja

OCR1B=ocb;

TIFR5&=~(1<<OCF5A); //poništi zastavicu CTC-a

TIMSK5=(1<<OCIE5A);} //omogući prekid CTC-a

}

Određivanje pozicije robota

Pređeni put robota određujemo brojanjem impulsa enkodera kao što je objašnjeno u primjeru „Promjena putanje robota“, ali nije spomenuta mogućnost promjene smjera vrtnje kotača. Posljedica promjene smjera vrtnje oba kotača je promjena smjera kretanja robota. U tom slučaju je potrebno od ukupnog puta oduzeti put kojim se robot gibao u suprotnom smjeru. Smjer vrtnje motora također određujemo pomoću enkodera koji posjeduju dva izlaza impulsa međusobno fazno pomaknutih.

Slika 1.

Utvrđivanje smjera kretanja radi se tako da se utvrdi koji impuls enkodera (A ili B) se prvi pojavio. Ma primjer za kretanje u smjeru strelice „D“ prvo se pojavi „A“ impuls pa onda „B“, dok za kretanje u smjeru strelice „L“ prvo se pojavi „B“ impuls pa onda „A“.

Obradom signala impulsa enkodera određuje se smjer vrtnje motora, a pređeni put brojenjem jednog izvora impulsa, dakle brojenjem impulsa iz izvora „A“ ili „B“.

Kad se robot giba u smjeru napredovanja impulsi se zbrajaju, a kad se giba u suprotnom smjeru oduzimaju.

Razvijeni su mnogi algoritmi za obradu impulsa enkodera radi utvrđivanja smjera kretanja te povećavanja odnosno smanjivanja vrijednosti varijable ovisno o smjeru kretanja. Ti algoritmi najčešće se odvijaju u beskonačnoj while(1) petlji, ili koriste vanjske prekide odnosno prekide koji detektiraju promjenu stanja priključka (pin change).

Kod primjene u mobilnoj robotici poželjno je brojanje impulsa enkodera prepustiti alternativnoj (perifernoj) jedinici bez prevelikog upletanja CPU u brojanje. Osim navedenog potrebno je naznačiti da brojač uvijek broji prema gore, odnosno svaki impuls povećava vrijednost TCNTn registra i ne može ga smanjivati. Smanjenje se odvija u varijabli.

#include <avr/io.h>

#include <avr/interrupt.h>

#define F_CPU 16000000UL // 16 MHz

#include <util/delay.h>

unsigned int a,b,a0,b0,a1,b1,ovf3,ovf4,smal,smad;

ISR(INT0_vect){ //priključen B izlaz enkodera

if (!(PINE&(1<<PINE6))){ //ako registra PINE6 u nuli (T3)

TCCR3B=0; //zaustavi brojač

smal++;} //povećaj varijablu

else

TCCR3B=7; //pokreni brojač

}

ISR(INT1_vect){

if (!(PINH&(1<<PINH7))){ //ako je registra PINH7 u nuli (T4)

TCCR4B=0; //zaustavi brojač

smad++;} //povećaj varijablu

else

TCCR4B=7; //pokreni brojač

}

ISR(TIMER3_OVF_vect){ //prekid preljeva registra

ovf3++;

}

ISR(TIMER4_OVF_vect){ //prekid preljeva registra

ovf4++;

}

ISR(TIMER5_COMPA_vect){

if (smal>0){ //ako je varijabla suprotnog smjera veća od nule

a0=TCNT3; //broj impulsa enkodera lijevog motora

a0=a0-smal; //oduzmi od broja impulsa

TCNT3=a0; //unesi novu vrijednost u brojač

smal=0;

}

else

a0=TCNT3;

if (smad>0){

b0=TCNT4; //broj impulsa enkodera desnog motora

b0=b0-smad;

TCNT4=b0;

smad=0;}

else

b0=TCNT4;

if (a0<a1){ //ukoliko je došlo do preljeva TCNT3 registra

a=(65536-a1)+a0;

a1=a0;}

else {

a=a0-a1; //razlika između broja impulsa na lijevom kotaču

a1=a0;} //varijabla a1 poprima vrijednost varijable a0 prijašnjeg mjerenja

}

int main (void)

{

/*postavke priključaka*/

PORTA=255;

DDRB=255;

DDRC=255;

PORTD=255;

PORTE=255;

PORTF=255;

DDRG=255;

DDRH=255;

PORTJ=255;

PORTL=255;

PORTK=255;

/*Postavke PWM-a na timer1 kanal A i B*/

TCCR1A=(1<<WGM11) | (1<<COM1A1) | (1<<COM1B1); //9-bitni fazni PWM

TCCR1B= (1<<CS11);//pokretanje PWM-a N=8

/*Postavke Counter3 i Counter4 za brojanje impulsa enkodera*/

TCCR3B=(1<<CS32) | (1<<CS31) | (1<<CS30); //vanjski rastući impuls na T3

TCCR4B=(1<<CS42) | (1<<CS41) | (1<<CS40); //vanjski rastući impuls na T4

/*postavke prekida preljeva radi proračuna pređenog puta*/

TIMSK3=(1<<TOIE3); //omogućen prekid preljeva registra radi ukupnog broja impulsa enkodera lijevog kotača

TIMSK4=(1<<TOIE4); //omogućen prekid preljeva registra radi ukupnog broja impulsa enkodera desnog kotača

/*Postavke timera5 za mjerenje vremena 200ms*/

TCCR5B=(1<<WGM52) | (1<<CS52) | (1<<CS50); //CTC način za 200ms broji do 3125 N=1024

OCR5A=3125; //CTC TOP vrijednost za fCPU=16KHz, N=1024 t=200ms

TIMSK5=(1<<OCIE5A);

EICRA=(1<<ISC00) | (1<<ISC01) | (1<<ISC10) | (1<<ISC11); //postavke brida

Uzlazni brid

EIMSK=(1<<INT0) | (1<<INT1); //omogući prekid

sei();

/* Replace with your application code */

while (1)

{

}

Nakon pred-procesorskih naredbi program svakih 200ms odlazi u ISR(TIMER5_COMPA_vect) prekidnu rutinu gdje prikuplja podatke brojača. Na svaki uzlazni brid kanala B enkodera lijevog kotača program odlazi u ISR(INT0_vect). Kanal B enkodera priključen je na alternativnu (perifernu) jedinicu INT0 koja detektira uzlazni brid. Na priključku PE6 priključen je A kanal istog enkodera. U rutini vanjskog prekida INT0 ispituje se registar PIN, bit 6 grupe priključaka E (PORT-a E) koji je postavljen kao ulazni s priključenim PULL-UP otpornikom. Ispitivanje se provodi kao uvjet grananja i ukoliko je rezultat PINE6==0 zaustavlja se brojač i povećava se vrijednost varijable „smal“, odnosno program je ustanovio da je kanal B enkodera u logičkoj „0“, odnosno robot se giba unazad. Vidi Slika 1. Ukoliko uvjet nije zadovoljen odnosno PINE==1 tada se omogućuje povećanje stanja brojača. U prekidnoj rutini ISR(TIMER5_COMPA_vect) se oduzima stanje varijable „smal“ od stanja TCNT3.

Ukoliko senzor nema dva kanala tad je moguće podatak o smjeru kretanja robota dobiti iz upravljačkih signala H-mosta.

Određivanje pozicije u koordinatnom sustavu ovisi o početnoj poziciji robota.

Slika 2.

Na Slici 2. prikazane su početne i očekivane pozicije robota (crtkano) za različite početne pozicije. Određivanje stvarne pozicije robota u koordinatnom sustavu ovisi o početnoj poziciji i putu koji napravi robot.

Osim početne pozicije za procjenu nove pozicije robota u koordinatnom sustavu osim podatka o putu potreban je i podatak o vrsti gibanja, odnosno zakretanja u svakom trenutku.

Proračun, odabir i upravljanje H-mostom

H-most se najčešće koristi u integriranoj izvedbi (IC) na tržištu potrošačke elektronike u ponudi se mogu pronaći IC H-mostovi s različitim karakteristikama.

Potraga za odgovarajućim H-mostom kreće od:

nominalnog napona napajanja H-mosta koji se prosljeđuje na motore
nominalne struje H-mosta
nominalnog upravljačkog napona.

U mobilnoj robotici edukativne namijenjene koriste se naponi napajanja od 12V do 24V, a struju je potrebno odrediti prema odabranom motoru.

Slika 1.

Struja motora kod maksimalnog opterećenja dana je u tehničkim podacima (Slika 1.), a ukoliko nije izračunava se iz tehničkih podataka:

Stvarna struja opterećenja može biti manja od maksimalne struje opterećenja proračunate iz tehničkih podataka jer robot nije kontinuirano opterećen maksimalnim momentom. Struja kod maksimalnog opterećenja nije potezna struja motora, već je to struja kod maksimalnog momenta i nominalnog napona. Potezna struja (struja kratkog spoja) je nekoliko puta veća od struje kod maksimalnog opterećenje i može kontrolirati postepenim porastom napona.

Upravljački napon H-mosta je napon TTL razine odnosno 5V.

Poveznice na tehničke podatke H-mostova:

https://www.ti.com/lit/ds/symlink/drv8841.pdf?ts=1609843725146&ref_url=https%253A%252F%252Fwww.ti.com%252F

https://www.infineon.com/dgdl/Infineon-TLE9201SG-DS-v01_00-en.pdf?fileId=db3a304345087709014518190f481cec

https://www.infineon.com/dgdl/Infineon-TLE8209-2SA-DS-v01_02-EN.pdf?fileId=5546d4624a0bf290014a0f5818836a1e

https://www.nxp.com/docs/en/data-sheet/MC33926.pdf

https://www.silergy.com/productsview/SY21605ABC

https://www.sparkfun.com/datasheets/Robotics/L298_H_Bridge.pdf

https://www.sparkfun.com/datasheets/Components/l293.pdf

Poveznice daju uvid u raznoliku ponudu H-mostova na tržištu potrošačkih elektroničkih komponenata. Za mobilni robot kojeg razvijam odlučio sam se za poprilično staru izvedbu H-mosta prvenstveno zbog kućišta u kojem se prodaje i dvostrukog H-mosta u jednom kućištu. To je H-most L298 koji je još uvijek popularan u mobilnoj robotici.

Vrlo sličan, malo napredniji H-most je DRV8841 kojeg možemo nabaviti samo u SMT kućištu što otežava lemljenje.

Detaljni tehnički podaci za L298 u poveznici iz koje ću uzimati podatke koji su bitni za proračun.

Slika 2.

Tablica maksimalnog opterećenja (Slika 2.) daje podatke o MAKSIMALNOM opterećenju a ne o NOMINALNOM opterećenju.

Nominalno opterećenje je TRAJNO opterećenje kojim možemo opteretiti neki uređaj a da pri tome ne dođe do kvara na uređaju.

U tablici maksimalnog opterećenja navedeno je i vrijeme trajanja takvog opterećenja. Iz tablice je vidljivo da maksimalna struja kod trajnog opterećenja iznosi 2A što je više nego izračunata struja za robota (1,65A) tako da možemo koristiti taj H-most za robota kojeg proračunavamo. Upravljački naponi su do 7V, a maksimalni napon motora 50V.

Bitan podatak je pad napona (Total Drop) za određenu struju odnosno napon zasićenja kod određene struje (Slika 3). Taj podatak nam ukazuje koliki pad napona možemo očekivati na elektroničkoj komponenti kada kroz nju teče određena struja, odnosno na motorima možemo očekivati toliko manji napon od napona izvora za faktor vođenja Ds=1.

Taj podatak je bitan za proračun snage koja se gubi (rasipa, disipira) na elektroničkoj komponenti te za korekciju faktora vođenja kod PWM-a.

Pad napona pomnožen sa strujom daje rasipnu snagu ili snagu disipacije, koja je pak važna zbog proračuna hlađenja komponente.

Maksimalna struja koju moramo osigurati za savladavanje maksimalnog momenta služi za odabir tipa H-mosta, a efektivna struja za određeni profil gibanja služi za proračun rasipne (disipirane) snage na H-mostu.

Kada bi kroz H-most trajno tekla maksimalna struja tada bi smo na temelju te struje proračunali i snagu disipacije, ali pošto to nije slučaj jer se moment tokom gibanja mijenja tad moramo izračunati efektivnu struju.

Efektivnu struju dobivamo iz efektivnog momenta koji iznosi MRMS=0,16Nm. Vidi „Odabir motora mobilnog robota.

Mehanička snaga za elektivni moment iznosi:

Vidi „Izračun napona motora“.

Da bi smo izračunali električnu snagu potrebno je iz tehničkih podataka motora izračunati faktor iskoristivosti pri maksimalnom opterećenju.

Podaci iz tablice su :

M_MAX = 0,45Nm

n=200 o/min kod maksimalnog opterećenja

P_MAXel =19,8W

Nadalje možemo izračunati električnu efektivnu snagu:

Iz dobivene efektivne snage izračunavamo efektivnu struju motora koja prolazi kroz H-most uz napone koje smo izračunali u primjeru „Izračuna napona motora“ za momente M3 i M4 jer su ti momenti vremenski najduže prisutni. Naponi koji se mijenjaju da bi ostvarili ubrzanje ili usporavanje nemaju toliki utjecaj na proračun efektivne struje jer kratkotrajni u odnosu na jednoliko gibanje.

Prosječna vrijednost napona kod jednolikog gibanja je 9,87V. [(10.5+9,24)/2]

Slika 3.

Iz dijagrama ovisnosti pada napona na H-mostu o struji H-mosta može se očitati da za efektivnu struju 0,8A pad napona iznosi 1,2V, te izračunati snagu gubitaka na H-mostu.

H-most L298 je dvostruki most pa izračunata snaga disipacije je za jedan motor odnosno za pola H-mosta. Ukupna disipacija je dvostruko veća pa iznosi 2W.

Disipaciju snage računali smo prvenstveno zbog proračuna hlađenja H-mosta te gubitaka kod proračuna baterija. Snaga potrošena na IC-u poprilično može zagrijati kućište komponente ukoliko komponenta nije hlađena, a može dovesti i do njenog temperaturnog uništenja ukoliko komponenta nema termičku zaštitu. Proračun hladila mora biti što točniji, jer hladila zauzimaju prostor na robotu i povećavaju njegovu masu.

Shematski se može zagrijanost komponente i hladila prikazati koristeći elektroničke komponente:

Slika 4.

Rth(j-case) – temperaturni otpor prijelaza topline između silicija i kućišta

Rth(case-hs) – temperaturni otpor između kućišta komponente i hladila

Rth(hs-amb) – temperaturni otpor između hladila i okoline

Cj, Ccase, Chs – temperaturna inercija

Izvor topline (Heat Source) prikazan je kao izvor struje, i nadalje se može zaključiti da se temperatura silicija mijenja kako se mijenja struja izvora, odnosno temperaturne inercija je mala.

Promjena temperature kućišta komponente određena je otporom Rth(j-case) i inercijom Ccase te na nju ne možemo djelovati jer je tvornički određena i dana je u tehničkim podacima. Na temperaturni otpor Rth(case-hs) kućište – hladnjak utječemo tako da poliramo površinu hladnjaka na koju ćemo smjestiti komponentu i koristimo odgovarajuće termalne paste koje smanjuju termički otpor.

Slika 5.

Potrebno je izračunati temperaturu silicija ukoliko je temperatura okoline 25°C i rasipa se izračunata snaga na H-mostu.

Slika 6.

Izračunata temperatura je ispod maksimalne temperature rada što zadovoljava kriterije.

Temperatura na R_htj-case za izračunatu disipaciju:

Temperatura kućišta:

Temperatura kućišta IC komponente je previsoka i kod izravnog dodira izaziva opekline. Dodavanjem hladila smanjit će se temperatura kućišta, ali i temperatura silicija što dovodi do produljenja životnog vijeka elektroničke komponente.

Za odabrano hladilo temperaturni otpor R_{th HS-amb} hladilo-ambijent iznosi 17°C/W. Temperaturni otpor kućište-hladilo ovisi o:

Kvaliteti obrade dodirnih površina kućišta i hladila (kvalitetno poliranje)
Izolacijskom materijalu između kućišta i hladila (tinjac (liskun) ili silikonski)
Dodavanje termički vodljive paste - smanjuje toplinski otpor
Dodirnoj površini komponente i hladila
Sili pritezanja vijka komponente za hladilo

Ukoliko nije potrebna izolacija između kućišta i hladila smanjuje se toplinski otpor.

Podaci za kućište Multiwatt15 nisu pronađeni, ali se na osnovu drugih dostupnih podataka može procijeniti temperaturni otpor kućište-hladilo na R_{th case-HS} = 0,8°C/W.

Temperatura silicija s dodanim hladilom:

Temperatura hladila:

Na poveznici se nalazi kalkulator i detaljna objašnjenja termičkog proračuna:

https://www.giangrandi.ch/electronics/thcalc/thcalc.shtml

Upravljanje H-mostom:

Slika 7.

Na (Slici 7.) je prikazana ½ mosta. Zaštitne diode je potrebno dodati kao vanjske komponente što je nedostatak ovog H-mosta. U tablici stanja dani su upravljački signali za pojedine funkcije odnosno za pojedine načine rada motora.

Primjedbe

Ništa ne razumijete? Greška koju ne znate ispraviti? Upišite komentar:

Email (neobavezan):