lucrare de licenŢĂ - acse...
TRANSCRIPT
LUCRARE DE LICENŢĂ
Controlul unui robot prin intermediul
protocolului de comunicaţie bluetooth
Absolvent
Grigore Mihai
Coordonator ştiinţific
Conf.Dr.Ing. Ciprian Lupu
Bucureşti, 2013
Universitatea Politehnică București Facultatea de Automatică și Calculatoare
Departamentul de Automatică și Ingineria Sistemelor
1
CUPRINS
1.SCOPUL LUCRĂRII ......................................................................................................................................... 2
2.FUNDAMENTARE TEHNICĂ ........................................................................................................................... 4
2.1.DESCRIEREA PROBLEMEI ...................................................................................................................................... 4
2.2.ANALIZA SOLUȚIILOR ........................................................................................................................................... 5
2.3.ELEMENTE HARDWARE ........................................................................................................................................ 6
2.3.1.Platforma Arduino ................................................................................................................................. 6
2.3.1.Motoare de curent continuu ................................................................................................................ 10
2.3.4.Drivere motoare de curent continuu .................................................................................................... 11
2.3.2.Servomotare ........................................................................................................................................ 11
2.3.3.Acumulatori ......................................................................................................................................... 13
3.FUNDAMENTAREA TEORETICĂ .....................................................................................................................15
3.1.PROTOCOALE DE COMUNICAȚIE WIRELESS ............................................................................................................. 15
3.2.PACHETUL ANDROID SDK .................................................................................................................................. 17
3.3.ANALIZA SISTEMELOR DE REGLARE AUTOMATĂ CU REGULATOR PI .............................................................................. 20
3.4.FILTRUL KALMAN ............................................................................................................................................. 22
4.STUDIU DE CAZ ............................................................................................................................................28
4.1.ARHITECTURA HARDWARE ................................................................................................................................. 28
4.1.1.Motorul Micro Metal Gearmotor 100:1 ............................................................................................... 28
4.1.2.Servomotorul ROB-09065 .................................................................................................................... 29
4.1.3.Driverul L293D ..................................................................................................................................... 30
4.1.4.Stabilizatoarele de tensiune LM7805 și LD33CV .................................................................................. 32
4.1.5.Acumulatorul LiPo Zippy Flightmax ..................................................................................................... 35
4.1.6.Senzor MPU-6050 ................................................................................................................................ 36
4.1.7.Modulul Bluetooth RN-42 .................................................................................................................... 39
4.1.8.Realizarea circuitului electric de alimentare și control al motoarelor ................................................. 41
4.1.9.Conexiunea elementelor hardware cu platforma Arduino Mega ........................................................ 44
4.1.10.Asamblarea componentelor .............................................................................................................. 45
4.2.CONTROLUL AUTOMAT AL PLATFORMEI ................................................................................................................ 47
4.2.1. Achiziția datelor de la senzorul MPU-6050 ......................................................................................... 48
4.2.2. Prelucrarea datelor cu filtrare Kalman ............................................................................................... 51
4.2.3. Algoritmul de control automat al platformei ...................................................................................... 53
4.3.DEZVOLTAREA INTERFEȚEI GRAFICE PENTRU CONTROLUL ROBOTULUI .......................................................................... 61
4.3.1.Dispozitivul de control al robotului ...................................................................................................... 61
4.3.2.Implementarea aplicației pentru controlul robotului .......................................................................... 62
4.4.IMPLEMENTAREA ALGORITMULUI DE CONTROL PE ARDUINO ..................................................................................... 69
4.CONCLUZII ...................................................................................................................................................71
7.BIBLIOGRAFIE ..............................................................................................................................................72
2
1.Scopul lucrării
În această lucrare este prezentată realizarea unui robot mobil de tip
vehicul utilitar. Controlul acestuia se face folosind un dispozitiv portabil cu
posibilitate de comunicație bluetooth. Dispozitivul prin intermediul căruia se
realizează controlul robotului este un telefon inteligent pe care am implementat
o interfață grafică. Astfel, controlul robotului este intuitiv și facil. Pentru a putea
implementa și alte funcționalități pe robot, am montat pe acesta o platformă care
este controlată manual sau poziționată automat la un anumit unghi.
Figura 1 - Controlul robotului prin bluetooth
Am ales realizarea unui astfel de robot pentru multiplele utilizări ale
acestuia. În domeniul militar sau de apărare se folosesc roboți controlați de la
distanță pentru inspectarea unui teren, a unei clădiri, a unui pachet suspect, fără
a pune în pericol viețile omenești. În industrie sau în alte domenii ce includ
activități de căutare se poate folosi un astfel de robot pentru a inspecta locuri în
3
care accesul este dificil, spațiile sunt strâmte sau există un potențial pericol.
Roboții controlați de la distanță se pot dovedii foarte utili și în cazul ajutorului
persoanelor cu diverse dizabilități, în special de vedere. Având în vedere
domeniile de utilizare prezentate, am ales montarea unei platforme mobile cu un
grad de libertate pe robotul realizat. Pe aceasta se pot monta ulterior diverse
camere video, camere foto, dispozitive audio sau senzori, în funcție de scopul
folosirii robotului.
În cel de-al doilea capitol se va face o scurtă analiză a problemei și a
soluțiilor existente. Capitolul trei cuprinde studiul de caz cu prezentarea pașilor
parcurși pentru realizarea acestui robot. Primul pas a fost alegerea unei
arhitecturi hardware. Am prezentat astfel modul de funcționare al componentelor
pe care le-am folosit, avantajele și dezavantajele acestora, comunicația și
conexiunile dintre diverse componente, realizarea circuitului electric de
alimentare și control al motoarelor și asamblarea robotului. Al doi-lea pas a fost
realizarea controlului automat al platformei. Am prezentat astfel modul de
achiziție al datelor de la senzorul accelerometric care oferă feedbackul necesar
pentru controlul automat al robotului, filtrarea datelor cu ajutorul filtrului Kalman
și realizarea algoritmului de control automat al platformei. Al trei-lea pas a fost
realizarea interfeței grafice pentru controlul robotului. Am prezentat astfel
protocolul de comunicație bluetooth dintre robot și dispozitivul de control,
uneletele software utilizate pentru realizarea aplicației ce include interfața
grafică, implementarea acestei aplicații în mediul de dezvoltare Eclipse. Ultimul
pas a fost implementarea algoritmului de control pe microcontrollerul platformei
Arduino care este practic ”creierul” robotului.
4
2.Fundamentare tehnică
2.1.Descrierea Problemei
Comunicația dintre anumiți roboți și userul care îi controlează trebuie sa fie
bidirecțională uneori pentru a trasnsmite comenzi și pentru a primi anumite
informații din partea robotului. Comunicațiile wireless au fost dezvoltate pentru a
simplifica modul în care dispozitivele electronice comunică între ele și pentru a
reduce numarul firelor atașate acestora, făcându-le astfel mult mai mobile. În
general, se utilizează controlul roboților prin infraroșu. Această metodă de
control poate fi costisitoare și greu de implementat, pentru că necesită patru
dispozitive: emițătorul și receptorul robotului, emițătorul și receptorul
dispozitivului de control. Un alt dezavantaj al acestei comunicații este faptul că
telecomanda trebuie să fie îndreptată către robot pentru transmiterea
comenzilor.
Cu ajutorul tehnologiei bluetooth însă, comunicația bidirecțională implică
utilizarea a două dispozitive de transmisie/recepție: unul montat pe robot și altul
pe dispozitivul de control. Standardul de comunicație „Bluetooth” a fost creat
pentru o realizare simplă și automată a conexiunii wireless, fără comenzi din
partea utilizatorului. Avantajele principale ale acestei metode de comunicare sunt
prețul scăzut al chip-urilor bluetooth și consumul redus de energie. Astfel,
comunicația bluetooth a ajuns să fie astăzi utilizată de foarte multe dispozitive
electronice. Deoarece există domenii multiple în care roboții controlați de la
distanță sunt foarte folositori, chiar indispensabili, am realizat un astfel de robot
controlat printr-o interfață de comunicație bluetooth.
Comunicația se face la distanțe de până la 100 de metri chiar și prin
obstacole precum pereții. Ca și telecomandă poate fi folosit un telefon inteligent
(Smartphone) care are implementat deja modulul de comunicație prin bluetooth.
Potrivit IFR (International Federation of Robotics) există peste un milion de
roboți industriali, peste 75.000 de roboți mobili pentru diverse servicii, și peste 4
milioane de roboți personali în întreaga lume. Potrivit JARA (Japan Robotic
Association) în anul 2025 vor exista peste 100 de milioane de roboți personali.
Este clar că domeniul robotic este într-o continuă dezvoltare și că aceștia vor
deveni cu timpul o parte importantă din viețile noastre.
5
2.2.Analiza soluțiilor
O echipă de la Universitatea Gulf Coast din Florida propune un robot
controlat prin bluetooth, însă controlul se face prin intermediul tastaturii unui
calculator sau prin intermediul unui joystick. Calculatorul și robotul folosesc câte
un modul de bluetooth pentru schimbul de date. Astfel joystickul sau tastatura
transmit comenzile la calculator, calculatorul transmite comenzile la robot prin
modulul de bluetooth. Robotul poate transmite informații preluate de la diverși
senzori către calculator. Dezavantajul acestui robot este faptul că dispozitivul de
control nu este mobil.
Figura 2.2.1. - Exemplu de robot controlat prin bluetooth [5]
Un alt exemplu de robot controlat prin bluetooth este prezentat în
imaginea următoare. Acesta este un robot cu șenile. Este lent, dar foarte potrivit
pentru terenuri accidentate. Controlul acestuia se realizează prin intermediul unui
smartphone. Comenzile se transmit prin înclinarea telefonului după cum
sugerează săgețile din imagine:
6
Figura 2.2.2. - Exemplu de robot controlat prin bluetooth [6]
2.3.Elemente hardware
În acest capitol voi descrie câteva date și caracteristici fundamentale ale
elementelor hardware principale pe care le-am integrat în arhitectura robotului.
Aceste noțiuni m-au ajutat în realizarea alegerii optime a acestor elemente.
2.3.1.Platforma Arduino
Arduino este o platformă electronică bazată pe o arhitectură hardware și
software flexibilă și ușor de utilizat. Comunicarea cu mediul înconjurător se face
prin recepția semnalelor de la diverși senzori și prin controlul motoarelor,
luminilor sau a altor elemente de execuție. Programarea microprocesorului se
poate face utilizând limbajul „Arduino Programming” și un mediu de dezvoltare
open-source. Arduino Mega 2560 este bazată pe microcontrolerul Atmega2560.
Aceasta dispune de conexiune USB, conector jack de alimentare, conector ICSP
(In-Circuit Serial Programming), 54 pini de intrare/ieșire din care 14 pot fi folosiți
și ca ieșiri PWM, 16 intrări analogice, 4 perechi de pini UART (Universal
Asynchronous Receiver/Transmiter), oscilator cu cristal de 16 Mhz și un buton de
reset.
7
Figura 2.3.1 - Arduino Mega2560 [10]
Alimentarea se poate face fie prin portul USB, fie prin alte surse externe.
Alegerea sursei de alimentare se face automat. Ca surse externe se pot folosi
atât baterii sau acumulatori, cât și un transformator de curent alternativ-curent
continuu cu conector jack de 2,1 mm. Conectarea bateriei se face la pinii Gnd și
Vin ai conectorului „Power”.
Tensiunea suportată de placă este între 6 si 20 de volți. Dacă se
alimentează la o tensiune mai mică de 7 volți, este posibil ca stabilizatorul de
tensiune intern să nu asigure o tensiune de 5 volți constantă și astfel se ajunge
la instabilitatea platformei Arduino. Prin alimentarea acesteia la o tensiune mai
mare de 12 volți se riscă supraîncalzirea sau chiar deteriorarea stabilizatorului de
tensiune și poate chiar a altor componente. Având în vedere acestea, tensiunea
de alimentare optimă a plăcuței poate fi între 7 si 12 volți.
Pinii conectorului „Power” sunt următorii: „VIN”, „5V”, „3V3”, „GND” și
„IOREF”. „VIN” se folosește la alimentarea platformei dintr-o sursă externă.
Acesta se poate folosi ca sursă de 5 volți în cazul alimentării platformei la
conectorul jack. Pinul „5 V” oferă o tensiune de 5 volți care provine de la
stabilizatorul de tensiune. Alimentarea acestui pin cu 5 volți are ca rezultat
bypas-ul stabilizatorului de tensiune. Nu se recomandă acest lucru pentru o bună
funcționare a platformei. Pinul „3V3” oferă o tensiune de 3,3 volți generată de alt
stabilizator inclus în platformă. Curentul maxim oferit este de 50 mA. „IOREF”
generează tensiunea de referință cu care operează microcontrolerul. Acest pin se
poate folosi pentru selectarea anumitei surse de alimentare.
Microcontrolerul Atmega2560 are o memorie de 256 KB de tip Flash
pentru încărcarea codului, o memorie de 8 KB de tip SRAM (Static Random Acces
Memory) și o memorie de 4 KB de tipul EEPROM (Electrically Erasable
8
Programmable Read-Only Memory) care poate fi accesată folosind librăria
EEPROM. Memoria Flash a acestuia rezistă la 10.000 de cicluri de citire/scriere.
ATmega2560 este un microprocesor care dispune de un set vast de
instrucțiuni și 32 de regiștri funcționali. Toți acești regiștrii sunt conectați direct
la UAL (Unitatea Aritmetică Logică), permițând ca doi regiștrii independenți să fie
accesați într-o singură instrucțiune, executată într-un singur ciclu al semnalului
de ceas. Memoria flash a acestuia permite scrierea concomitent cu citirea.
Conține un soft care poate selecta unul din cele 6 moduri disponibile de
economisire a energiei. Modul idle al acestuia oprește CPU (Control Processing
Unit) dar permite funcționarea SRAM, a timerelor sau contoarelor, a portului SPI
(Serial Peripheral Interface) și a întreruperilor de sistem. Modul „Power-down”
salvează conținutul regiștrilor, dar oprește oscilatorul, blocând astfel toate
funcțiile cipului până la următoarea întrerupere sau comandă reset realizată
hardware. În modul „Power-save”, timerul asincron rulează, iar restul
dispozitivului este pe modul „sleep”. Modul „Noise Reduction” al circuitului ADC
(Analog to Digital Converter) oprește CPU și modulele de intrare/ieșire, în timp
ce timerul asincron si converter-ul își continuă funcționarea. În modul „Standby”
oscilatorul cu cristal rulează în timp ce restul dispozitivului este pe modul „sleep”.
Aceste lucruri permit o pornire rapidă combinată cu o reducere semnificativă a
consumului de energie. În modul „Extended Standby” atât oscilatorul principal
cât și timerul asincron funcționează.
Arduino Mega poate fi foarte ușor programat cu ajutorul softului „Arduino”.
Folosind câteva funcții de bază ale acestui program, precum „pinMode()”,
„DigitalWrite()” sau „DigitalRead()”, cei 54 de pini digitali pot fi folosiți atât ca
intrări, cât și ca ieșiri. Aceștia operează la 5 volți si pot oferi un maxim de 40 mA.
Fiecare pin are o rezistență de tip pull-up de 20-50 Kohmi, astfel fiecare are o
funcție specială:
Comunicație serială – perechile (0,1), (19,18), (17,16), (15,14) se pot
folosi ca pinii Tx, Rx ai unei comunicații seriale (atât transmisie cât și
recepție de date).
Întreruperi externe – folosind funcția „attachIntrerupt()”, pinii 2, 3, 18,
19, 20 și 21 pot fi configurați pentru diverse tipuri de întreruperi.
PWM – pinii 2-13 si 44-46 pot fi utilizați pentru generarea unui semnal de
tip PWM pe 8 biți, folosind funcția „analogWrite()”.
SPI – pinii 50, 51, 52, 53 suportă comunicație SPI (Serial Peripheral
Interface) folosind librăria SPI a softului „Arduino”.
LED – pinul 13 este conectat la un LED inclus în platformă. Când pinul 13
are valoarea HIGH (5 volți) Led-ul este aprins; când are valoarea LOW (0
volți), led-ul este stins.
TWI – pinii 20 și 21 suportă comunicație de tip TWI (Two Wire
Communication) folosind librăria TWI.
9
Platforma Arduino Mega include și 16 pini analogici cu o rezoluție de 10 biți
(1024 de valori diferite). Aceștia măsoară tensiunea de la 0 la 5 volți, dar limita
de 5 volți poate fi schimbată folosind pinul AREF și funcția analogReference().
Platforma conține și un pin de reset, care îndeplinește această funcție
atunci când tensiunea pinului trece din HIGH în LOW. Acest pin se folosește de
obicei atunci când se adaugă un buton care blochează resetul inclus în platformă.
Comunicarea cu calculatorul se face foarte ușor, Arduino Mega2560 având
multe facilități în acest sens. Sunt disponibile 4 unități UART hardware pentru
comunicație serială TTL (Transistor to Transistor Logic) de 5 volți. Unul din
aceste dispozitive este Atmega16U2 care comunică prin portul USB al platformei
cu calculatorul. Acesta recunoaște portul de comunicație automat pentru
sistemele de operare Linux si OSX și doar în urma instalării unui fișier .inf pentru
Windows. Există și două led-uri Tx si Rx care se vor aprinde intermitent la
transmiterea datelor prin cipul Atmega16U2 și mai departe dinspre sau către
calculator. Librăria „SoftwareSerial” permite comunicația serială cu orice pin
digital al platformei. Arduino Mega suportă și comunicații TWI si SPI.
Programarea microprocesorului se poate face foarte ușor folosind
programul open-source „Arduino”. Acesta poate rula pe Windows, Linux sau Mac
OSX. Mediul de programare al acestui soft a fost dezvoltat în limbajul Java.
Microcontrolerul Atmega2560 de pe platforma Arduino Mega conține un
bootloader încărcat pentru a uploada codul scris în softul Arduino fără un
programator hardware. Comunicația se face utilizând protocolul STK500. Aceasta
nu este singura metodă de programare. Încărcarea codului se poate realiza și
prin ICSP.
Arduino Mega2560 conține o componentă activă de tip „polyfuse”
resetabilă, care protejează portul USB al calculatorului de scurtcircuitări sau de
curent în exces. Deși majoritatea calculatoarelor dispun de această protecție, se
realizează încă un „strat” protector. Dacă se aplică portului USB un curent mai
mare de 500 mA, conexiunea se întrerupe automat până când problema este
rezolvată și curentul reintră in limitele normale.
Următorul tabel conține principalele caracteristici ale platformei Arduino
Mega2560:
Microcontroller Atmega2560
Tensiunea de operare 5 V
Tensiunea de alimentare recomandată 7-12 V
Limitele maxime ale tensiunii de alimentare
6-20 V
Numărul de Intrări/ Ieșiri digitale 54 (15 din 54 pot genera semnal PWM)
Numărul de intrări analogice 16
10
Curentul din fiecare pin de intrare/ieșire 40 mA
Curentul din pinul de 3.3 V 50 mA
Memoria Flash 256 KB (8KB folosiți de bootloader)
SRAM 8KB
EEPROM 4KB
Frecvența ceasului 16 MHz
Tabel 3.1.1 - Principalele caracteristici ale platformei Arduino Mega2560 [10]
2.3.2.Motoare de curent continuu
Motoarele de curent continuu sunt foarte utilizate atât in robotică, cât și în
multe alte domenii. Sunt ieftine, ușor de folosit și se pot cumpăra în diverse
forme și mărimi. Pentru alegerea motoarelor de curent continuu care vor
propulsa robotul, am calculat cuplul necesar al motoarelor folosind următoarea
formulă simplificată:
Unde:
C - reprezintă cuplul necesar deplasării robotului
R - reprezintă raza roților robotului
m - reprezintă masa robotului
a - reprezintă accelerația pe care o va avea robotul
În scopul determinării unui cuplu potrivit, am ales accelerația de 1m/s2,
ceea ce va duce la o viteză de 1 m/s după o secundă de mers a robotului. Am
calculat masa aproximativă a robotului ca fiind de 0.7 Kg și am ales roți cu raza
de 3 cm.
Înlocuind în formula (1) aceste valori, a rezultat un cuplu de 2.1 N*cm,
deci 1.05 N*cm pentru fiecare roată. Pentru că în majoritatea specificațiilor
tehnice ale motoarelor valoarea cuplului este dată în Kg*cm, se face
transformarea in această unitate de măsură înmulțind cu un factor de 9.81
(accelerația gravitațională). Am ales astfel două motoare cu un cuplu mai mare
de 0.1 Kg*cm fiecare.
11
2.3.3.Drivere motoare de curent continuu
Pentru controlul motoarelor de curent continuu se folosesc drivere de
control care primesc semnalul PWM de la microcontroller, amplifică acest semnal
și îl transmit mai departe către motor.
Aceste drivere cuprind în general două punți H de control. O astfel de
structură permite rotirea motorului în ambele sensuri prin controlul a 4
tranzistori:
Figura 2.3.3.1 - Punte H pentru controlul unui motor de curent continuu
Un canal (o punte H) poate oferi controlul bidirecțional pentru un motor,
cu un curent de până la 1A la tensiuni de 4.5V – 36V. Aceste canale pot controla
atât motoare de curent continuu, cât și motoare pas cu pas. Acest driver are
incluse diodele pentru protecția la spike-uri.
2.3.4.Servomotare
Un servomotor este un motor care permite un control foarte precis al
poziției unghiulare. Acesta este compus dintr-un motor de curent continuu cuplat
printr-un angrenaj de roți dințate la un senzor care oferă informații legate de
poziția rotorului. Reglarea unghiului dorit se face prin intermediul unui chip de
control din interior acestui motor.
12
În imaginea de mai jos sunt prezentate componentele din interiorul unui
servomotor și anume: motorul de curent continuu în partea din stânga - jos,
angrenajul cu roți dințate în partea de sus, senzorul unghiular în partea dreaptă
la mijloc și chipul de control în partea dreaptă-jos.
Figura 2.3.4.1 - Componentele unui servomotor [7]
Pentru controlul poziției motorului, se trimite un semnal PWM către chipul de
control printr-un fir separat de liniile de alimentare ale motorului. Aceste motoare
se alimentează uzual la o tensiune de 5V±25%. Dacă se aplică o tensiune prea
mare asupra acestora, circuitul interior se poate defecta.
Figura 2.3.4.2 - Timpii de control ai motorului [7]
13
Timpii de control sunt esențiali pentru servomotoare. În primul rând,
circuitul servomotorului așteaptă primirea unui puls pentru a aștepta în
continuare semnal PWM pentru control. Pentru semnalul PWM, lungimea minimă
a unui puls trebuie să fie de minim 1ms și aproximativ 40ms pauză între
semnale. Dacă semnalele sunt transmise la un interval prea mic de timp, motorul
va deveni instabil, dacă semnalele sunt transmise la un interval prea mare de
timp, motorul va intra în stand-by, iar apoi unghiul nu va ma fi cel dorit. Astfel,
timpul mort va fi calculat prin scăderea lungimii comenzii din 40ms.
Stabilitatea acestui motor este dată de circuitul interior care compară
semnalul PWM primit cu un timer a cărui perioadă depinde de rezistența unui
potențiometru care este legat la axul ce rotește motorul. Astfel, este asigurat
feedbackul care asigură stabilitatea sistemului. Circuitul interior al
servomotorului include și o punte H pentru a roti motorul de curent continuu în
ambele sensuri.
Motivul principal pentru care am ales folosirea unui servomotor este
precizia unghiulară ridicată, care este esențială pentru controlul platformei. Alte
avantaje ale folosirii servomotorelor sunt faptul că se utilizează în buclă deschisă,
rezultând un control foarte simplu al acestora, și faptul că Softul Arduino pe care
voi implementa controlul are o librăria ”Servo” dedicată trimiterii de semnale
PWM pentru un control facil al servomotoarelor.
2.3.5.Acumulatori
Pentru alimentarea robotului am ales să folosesc acumulatori de tipul LiPo
(Litiu – Polimer). Aceștia sunt reîncărcabili și au timpul de reîncărcare mic. În
general, acumulatorii LiPo sunt alcătuiți din mai multe celule, fiecare având
tensiuni cuprinse între 2.7V și 4.23V. Există astfel diverse tipuri de LiPo, cu
anumite dimensiuni, diferite tensiuni de ieșire și autonomii.
Principalele avantaje ale acestui tip de acumulator sunt: faptul că se poate
modela foarte ușor în diferite forme și modele și că are o greutate foarte mică.
Datorită acestui lucru, sunt folosiți pe scară largă.
Acești acumulatori au dezavantajul că pot lua foc sau pot chiar exploda
dacă nu sunt manevrați corespunzător și că sunt puțin mai scumpi decât alți
acumulatori (NiCad, NiMH).
De obicei, acumulatorii LiPo au mai multe celule de câte 3.7V fiecare,
legate în serie pentru a oferi diverse tensiuni. Există astfel acumulatori LiPo de
3.7V, 7.4V, 11.1V, 14.8V ș.a.m.d. , tensiunea oferită fiind unul din parametrii
acestora. Un alt parametru important este capacitatea acestora, care se măsoară
în mAh și reprezintă câtă energie pot oferi într-o oră după ce au fost încărcați la
14
maxim. De aceea este important ca acumulatorul să aibă o capacitate mai mare
decât necesar pentru o autonomie mai mare. Pentru a avea suficientă
autonomie, dar și o tensiune corespunzătoare, am ales să folosesc acumulatori
de 14.8V, 2000 mAh.
Rata de descărcare reprezintă cât de repede poate fi descărcată o baterie
în siguranță.Această valoare are ca unitate de măsură [C]. O baterie care are 10
C, înseamnă că se poate descărca de 10 ori mai repede decât capacitatea
bateriei. Dacă această valoare este mai mică decât necesarul circuitului pe care îl
alimentează, atunci bateria sau circuitul se pot defecta. Cu cât aceasta valoare
este mai mare, cu atât acumulatorii sunt mai scumpi, de aceea trebuie facut un
compromis între nivelul de siguranță și preț. De aceea am avut în calcul la
alegerea acumulatorilor o rată de descărcare de cel puțin 15C.
Cel mai dăunător lucru la durata de viață a acestor acumulatori este
supradescărcarea. Dacă o celulă de 3.7V se descarcă până coboară sub pragul de
3V, viața acumulatorului scade substanțial. Este foarte important ca tensiunea
acumulatorului să nu scadă sub 80% din valoarea inițială.
15
3.Fundamentarea teoretică
3.1.Protocoale de comunicație wireless
Comunicația wireless se referă la orice comunicație care nu implică fire sau
cabluri pentru transferul de informații. De cele mai multe ori, acest transfer de
date se realizează prin unde radio.
Bluetooth este un protocol de comunicație wireless pentru distanțe mici.
Comunicația se realizează prin intermediul undelor radio în banda 2400-2480
Mhz. Acest protocol a fost formalizat de IEEE ( Institute of Electrical and
Electronics Engineers) prin standardul IEEE 802.15.1.
Comunicația bluetooth se utilizează pentru diverse dispozitive mobile sau
fixe (telefoane, laptopuri, imprimante, calculatoare personale), realizând o rețea
PAN (Personal Area Network) cu un nivel ridicat de securitate. Prin Bluetooth se
pot interconecta foarte multe dispozitive fără probleme de sincronizare.
Comunicația se realizează prin transmiterea datelor divizate în pachete.
Fiecare pachet este transmis pe unul din cele 79 de canale cu lungimea de bandă
de 1Mhz. Viteza de transmisie a datelor poate atinge și 3Mbit/s. Comunicația este
de tipul Master-Slave. Un dispozitiv master poate comunica cu până la 7
dispozitive slave într-o rețea. Toate dispozitivele slave au același semnal de ceas
dat de master. Rolurile se pot schimba pe parcursul comunicării, astfel un
dispozitiv slave poate deveni master.
Există mai multe clase de bluetooth, dar toate sunt perfect compatibile
între ele - Bluetooth v1.0/v1.2/v2.0/v2.1/v3.0/v4.0 . Acestea au fost dezvoltate
începând cu 1994 – v1.0 - și au evoluat până la ultima versiune – v4.0 - apărută
în anul 2010.
Principalul avantaj al comunicației prin bluetooth este consumul foarte
redus de energie și prețul scăzut al implementării modulului electronic aferent.
Alt avantaj major se datorează transmisiei datelor prin intermediul undelor radio,
astfel dispozitivele care comunică nu trebuie să fie în același câmp vizual.
Distanța de comunicare este de aproximativ 10m pentru protocolul bluetooth de
clasa 2.1 (clasa suportată de modulul instalat pe robot și de smartphone-ul care
va transmite datele).
Un alt avantaj al comunicării prin bluetooth este ușurința interconectării
dispozitivelor. Modulele de bluetooth se pot afla fie în modul oprit, fie în modul
16
conectat, fie în modul detectabil. Atunci când se află în modul detectabil,
dispozitivul va transmite următoarele informații la cerere:
Numele dispozitivului
Clasa de bluetooth a dispozitivului
Lista de servicii
Informații tehnice (producător, caracteristici etc.)
Orice dispozitiv bluetooth poate căuta dispozitive cărora să le adreseze
cereri de conectare și orice dispozitiv poate fi configurat să răspundă acestor
cereri. Dacă dispozitivul care primește cererea de conectare cunoaște adresa
expeditorului, acestea se pot conecta direct. În unele cazuri este nevoie de
”pairing” înainte de a se realiza conexiunea.
Adresele dispozitivelor se reprezintă pe 48 de biți și sunt unice. Pentru
interconexiuni realizate de useri, se folosesc numele dispozitivelor, nu adresele
acestora. Numele dispozitivului poate fi schimbat de user, valoarea standard a
acestuia fiind de obicei numele produsului setat din fabrică.
Pentru un nivel ridicat de siguranță în comunicarea prin bluetooth se
dorește de cele mai multe ori recunoașterea dispozitivului înainte de transmisia
datelor. Pentru acest lucru este de obicei nevoie de intervenția userului pentru a
confirma recunoașterea dispozitivului și a realiza procesul denumit ”pairing”.
Odată ce pairingul a fost realizat, următoarele interconectări ale dispozitivelor se
vor face automat.
Realizarea conexiunii dintre telefon și robot se face automat, fără acest
proces de pairing. Aplicația ce rulează pe telefon a fost realizată în așa fel încât
telefonul să caute disponibilitatea modului de bluetooth de pe robot, prin
recunoașterea adresei acestui modul. Dacă modulul de bluetooth este pornit,
conexiunea se realizează direct, fără confirmarea userului aplicației. Conexiunea
se face imediat după lansarea aplicației și este foarte rapidă (durează doar
câteva secunde).
Am ales comunicarea prin bluetooth în primul rând pentru raportul
consum/distanță de comunicare. Consumul este foarte mic, iar distanța de
comunicare de 10 m este mai mult decât suficientă pentru proiectul realizat. Alt
avantaj al acestei comunicări este compatibilitatea dintre dispozitive, astfel
smartphone-ul folosit și modulul de bluetooth al robotului sunt perfect
compatibile. Conexiunea acestor două dispozitive se realizează rapid și este
foarte stabilă – nu se întrerupe decât la depășirea distanței de comunicare
admise, care este de aproximativ 10m.
17
3.2.Pachetul Android SDK
Dezvoltarea aplicației s-a realizat în mediul de dezvoltare Eclipse folosind
limbajul Java. Am ales platforma Eclipse pentru că oferă un mediu de
programare (un plug-in) care facilitează dezvoltarea aplicațiilor pentru Android
numit Android SDK (Android Software Development Kit).
Platforma ASDK este cel mai bine descrisă ca fiind o colecție de
componente precum:
Sistemul de operare de bază preluat de la Linux
Un mediu de programare Java
Compilator, debugger și emulator
Mașină Virtuală pentru rularea aplicațiilor
O caracteristică aparte a sistemului de operare Android este că se bazează
pe un element de design inovator pentru o astfel de arhitectură numit ”intenție”.
O intenție este o construcție care permite unei aplicații să îndeplinească o
anumită sarcină, ca de exemplu: ”Este nevoie de o aplicație care să afișeze
unghiul de înclinare al telefonului”. Aplicațiile se înregistrează oficial ca fiind
capabile să îndeplinească anumite cerințe. Pentru exemplul de mai sus, un
exemplu de aplicație care îndeplinește cerința ar fi: ”Aplicație care afișează
unghiurile curente de înclinare a telefonului pe două axe”. Acesta este un
exemplu de filtru de intenție sau IntentFilter. Astfel, aplicațiile etalează
disponibilitatea lor de a îndeplinii diverse operații numite IntentFilters.
Principalele tipuri de aplicații pentru Android sunt următoarele:
Aplicație de tipul activitate – aceasta se prezintă utilizatorului printr-
o interfață ce poate cuprinde căsuțe de text, etichete, butoane
ș.a.m.d.
Aplicație de tip serviciu – precum și în sistemele de operare pentru
calculatoare sau laptopuri, există aplicații care rulează în fundal,
aplicații care nu au interfață.
Aplicație de management de date – aplicație care sustrage diverse
date, precum contactele dintr-un telefon; aceste contacte sunt toate
afișate în agenda telefonului, care poate fi privită ca o aplicație.
Pentru că aplicația pe care am dezvoltat-o este de tipul activitate, voi
explica detaliat ce cuprinde o astfel de aplicație. Aceasta poate cuprinde una sau
mai multe activități. Fiecare activitate este de obicei corelată cu câte un ecran al
aplicației. Un ecran al aplicației reprezintă elementele care sunt afișate pe
ecranul telefonului la un moment dat. O aplicație poate avea mai multe ecrane,
fiecare cu diverse funcționalități. Fiecare activitate poate fi pornită prin apelul
18
unor metode precum startActivity() sau startSubActivity(). În fiecare
caz, o intenție este trecută ca parametru al acestor metode.
Acest tip de aplicație folosește diverse elemente numite ”Views” care fac
parte din interfața propriu-zisă. Acestea se încadrează în unul din următoarele
designuri de organizare a interfeței:
Liniar Vertical – fiecare elemente urmează după predecesorul său în
cadrul unei singure coloane;
Liniar Orizontal – fiecare elemente urmează imediat după
predecesorul său în cadrul unei singure linii;
Relativ – poziția fiecărui element este descrisă de poziția unei
element din jurul său;
Tabel – Există un tabel cu mai multe linii și coloane, fiecare element
al tabelului conținând un element al interfeței.
Elementele unei interfețe pot fi: Button, ImageButton, EditText,
TextView (similar unei etichete) , CheckBox, RadioButton, List, Grid,
DatePicker, TimePicker, Spinner, AutoComplete (este ca un EditText,
dar cu funcția de autocomplete). Fiecare element poate avea unul sau mai
multe atribute.
Mediul de programare Eclipse este ușor de folosit și prin pachetul Android
SDK oferă un template foarte util pentru dezvoltarea de aplicații pentru Android.
Principala clasă a porgramului este apelată atunci când activitatea a fost creată
prima oară, adică atunci când aplicația este deschisă. Clasa principală este o
extensie a clasei Activity localizată în pachetul android.app.
Funcția setContentView(R.layout.main) este prezentă în clasa
principală și responsabilă cu crearea interfeței primare folosind ca argument
R.layout.main. Acesta este un identificator ce face parte din clasa R.java
care este creată automat după ce aplicația a fost compilată. R.java conține
astfel de identificatori pentru resursele aplicației.
Principalele resurse ale aplicației sunt:
Drawables - acest folder conține fișiere grafice precum icon-urile;
Layouts – acest folder conține fișiere .XML care reprezintă layoutul
și elementele interfeței;
Values – conține un fișier numit string.xml cu diverse etichete
pentru denumirile din cadrul aplicației.
Aplicația conține un fișier numit main.xml care reprezintă aspectele
vizuale ale interfeței primare. Pe baza acestui fișier, identificatorul
R.layout.main, prezentat mai sus, este creat.
19
Pentru a facilita crearea fișierului main.xml, pachetul Android SDK pune la
dispoziție utilizatorului o interfață pentru crearea aspectului aplicației. Astfel,
interfața aplicației poate fi creată doar prin drag&drop datorită pachetului
Android SDK. Interfața pusă la dispoziția programatorului de acest pachet are
următorul aspect:
Figura 3.2.1 - Interfața pentru crearea layoutului aplicației
Un alt fișier important al aplicației este AndroidManifest.xml, care
reprezintă descrierea implementării aplicației Android. Acesta conține toate
activitățile, serviciile sau conținuturile incluse în aplicație, împreună cu
IntentFilters suportate de aplicație.
Acest fișier conține: marcajul <manifest> care importă anumite clase,
marcajul <application> are un atribut care face referire la o resursă, marcajul
<activity> include android:label care conține numele aplicației și
<intent-filter> despre care am vorbit anterior.
Pentru numeroasele avantaje oferite, am folosit pachetul Android SDK
pentru dezvoltarea aplicației de control al robotului.
20
3.3.Analiza sistemelor de reglare automată cu regulator PI
Proiectarea unui sistem de reglare automată presupune parcurgerea
etapelor de modelare, proiectare, simulare, testare și implementare. Voi
prezenta etapele de proiectare a unui regulator de tip PI. Structura generală a
unui sistem de reglare automată cu un grad de libertate este prezentată în figura
următoare:
Figure 3.3.1 – Structura de reglare automată cu un grad de libertate [1]
R reprezintă regulatorul procesului
P reprezintă obiectul condus
r reprezintă intrarea sistemului
e reprezintă eroarea sistemului – diferența dintre ieșirea măsurată și
referință
u reprezintă comanda procesului
yp reprezintă ieșirea procesului
v reprezintă perturbația sistemului
n(s) reprezintă zgomotul de măsurare al ieșirii
Cel mai simplu regulator este cel de tip P, funcția atașată acestuia fiind
următoarea:
Dacă la această componentă P se adaugă o componentă de tip integral (I)
se obține un regulator de tip PI cu următoarea funcție de transfer:
Pentru simulări și testări, se determină un model matematic al procesului
ce trebuie reglat. Acest model poate fi descris sub forma unei funcții de transfer:
21
Să presupunem ca modelul procesului reglat este de ordinul întâi:
Atunci funcția de transfer a căii directe va fi dată de relația:
Vom avea astfel o eroare de regim staționar egală cu zero datorită
integratorului pe calea directă:
Astfel, perturbațiile de tip treaptă vor fi rejectate. Pentru a limita
amplitudinea ieșirii în raport cu perturbația, valoarea raportului dintre constanta
de proporționalitate a regulatorului și constanta de integrare trebuie să fie cât
mai mic. Pentru a obține însă viteze de răspuns ridicate în raport cu referința,
acest raport trebuie să fie cât mai mare. Ajustarea acestor parametrii se va face
astfel ținând cont de performanțele impuse pentru procesul ce trebuie reglat.
Următoarea figură prezintă influența parametrilor KR și Ti asupra
răspunsului la intrarea de tip treaptă, atunci când sistemul este în buclă închisă:
Figura 3.3.2 – Influența parametrilor regulatorului PI asupra răspunsului sistemului la
treaptă
22
3.4.Filtrul Kalman
Pentru că valorile preluate de la senzorul MPU-6050 sunt afectate de
zgomote și nu pot fi folosite ca atare, vom efectua filtrarea acestora printr-un
filtru Kalman.
Filtrul Kalman este un algoritm care folosește o serie de măsurători care
sunt afectate de zgomote cu variații aleatorii și care realizează o estimare a
acestor măsurători pentru un nivel de precizie cât mai ridicat. Precizia se
datorează faptului că filtrul Kalman folosește și măsurătorile anterioare pentru
estimarea stării curente. Am folosit acest algoritm pentru a determina unghiul
de înclinare al platformei folosind datele măsurate de senzorul giroscopic și de
accelerometru.
Problema datelor înregistrate de accelerometru este că valoarea unghiului
determinat de acesta este o valoare corectă doar după o anumită perioadă de
timp (după realizarea mai multor măsurători), mai ales dacă robotul pe care a
fost montat senzorul este în mișcare. Problema giroscopului este că deși la
început valoarea unghiului indicat de acesta este corectă, odată cu trecerea
timpului valoarea unghiului își pierde precizia. Putem spune că pe o perioadă
scurtă de timp, valoarea senzorului giroscopic este mai aproape de cea reală, iar
pe o perioadă mai lungă de timp, valoarea accelerometrului este mai aproape de
valoarea reală a unghiului.
Filtrul Kalman realizează o estimare statistică optimă a stării sistemului,
bazându-se pe măsurători. Estimarea se realizează prin cunoașterea zgomotului
măsurătorilor, dar și zgomotul sistemului în sine numit zgomotul procesului.
Pentru a calcula aceste valori, zgomotul trebuie să aibă o distribuție Gaussiană și
media zero. Majoritatea zgomotelor aleatoare au aceste două proprietăți.
Modelul poate fi considerat un Model Markov ascuns pentru că starea
sistemului în sine este ascunsă și poate fi observată doar prin intermediul unei
observații zk. Din cauză că starea curentă este estimată în funcție de stările
anterioare, Filtrul Kalman are nevoie de un anumit timp de stabilizare. Starea
sistemului la momentul k este descris de următoarea ecuație:
Unde:
reprezintă ieșirea filtrului Kalman: va fi unghiul , dar și influența
bazată pe măsurătorile giroscopului și accelerometrului; această influență
reprezintă eroarea giroscopului.
23
[
]
Matricea F de dimensiunea 2x2 are următoarele valori:
[
]
Mărimea de intrare uk reprezintă în acest caz măsurătorile giroscopului în
grade pe secundă la timpul k, aceasta este rata . Ecuația (3.4.1) devine:
Matricea B este definită ca:
[ ]
Wk este zgomotul procesului care are o distribuție Gaussiană cu media
zero și covarianța Qk la momentul k.
Pentru că estimarea erorii girsocopului și a accelerometrului sunt
independente, vom considera următoarea matrice de covarianță:
[
]
Matricea de covarianță depinde de timpul curent k, de aceea apare
multiplicarea cu în această matrice. Practic, eroarea crește cu cât diferența de
timp dintre măsurători este mai mare. Constantele de mai sus trebuie cunoscute
pentru o bună funcționare a filtrului Kalman. Cu cât aceste valori sunt mai mari,
cu atât crește zgomotul în estimarea stării, dar dacă, de exemplu, unghiul
estimat începe să își piardă valoarea inițială, trebuie crescut , iar dacă
estimarea este prea înceată, trebuie scăzută valoarea Qθ.
Măsurătoarea (observația) la un moment de timp k este dată de formula:
Unde:
H se numește modelul de observație și este folosit pentru a transforma
spațiul stărilor în spațiul observat (măsurat). Adevăratul spațiu nu poate fi
observat, deoarece accelerometru nu poate măsura cu precizie de 100%
unghiul, deci matricea H va fi:
[ ]
Zgomotul măsurătorii trebuie să aibă o distribuție Gaussiană de asemenea
cu media zero și R covarianță corespunzătoare:
24
R reprezintă varianța măsurătorii, nu reprezintă matricea de covarianță și
este definit ca:
[ ]
Presupunem că zgomotul măsurătorii este același și nu depinde de timpul
k:
Dacă varianța R este prea mare, atunci filtrul va răspunde foarte încet
pentru că scade încrederea în noile măsurători; dacă este prea mic, semnalul
filtrat va fi zgomotos pentru că încrederea în valorile măsurate de accelerometru
este prea mare.
În continuare, voi prezenta ecuațiile de estimare a stării sistemului la
timpul k - . În primul rând, vom folosi un filtru pentru a estima starea curentă
în funcție de stările anterioare și de măsurătorile giroscopului:
În continuare, se estimează matricea de covarianță a erorii anterioare
prin intermediul matricei anterioare - :
Această matrice se calculează pentru a estima câtă încredere avem în
valoarea estimată la momentul curent. Această matrice de covarianță este de
2x2 :
[
]
Dacă la început nu se cunoaște această matrice, se poate inițializa în felul
următor:
[
], unde L este o valoare mare.
Diferența dintre măsurătorile zk și starea anterioară este numită
inovația măsurării sau reziduul:
)
Covarianța reziduului estimează cât de multă încredere putem avea în
măsurătorile bazate pe matricea de covarianță anterioară a erorilor și pe
varianța R. Matricea H este folosită pentru a reprezenta matricea în spațiul
observabil. Valoarea lui S este direct proporțională cu zgomotul măsurătorilor
curente:
25
În continuare se calculează amplificarea Kalman sau câștigul:
Acum putem updata estimarea ulterioară a stării curente. Această ecuție
reprezintă corectarea estimației , care a fost calculată prin măsurătoarea
giroscopului și a stărilor anterioare, prin măsurătoarea curentă dată de
accelerometrul:
Ultimul pas va fi updatarea matricei de covarianță a erorilor anterioare:
Pentru a implementa codul aferent filtrului Kalman pe platforma Arduino,
este nevoie de implementarea ecuațiilor prezentare mai sus în limbaj C, de aceea
voi aduce ecuațiile în forme cât mai simple pentru a ușura implementarea
ulterioară.
Primul pas este implementarea ecuației (3.4.6) :
[
]
[
] [
]
[ ]
[
]
[
]
[ ]
[
]
[
]
[
]
[
]
Al doi-lea pas este implementarea ecuației (3.4.7):
[
]
[
] [
]
[
] [
]
[
]
[
]
[
] [
]
[
]
[
]
[
]
[
]
[
]
26
[
]
[
]
Pasul 3 este implementarea ecuației (3.4.8):
[ ] [
]
Pasul 4 este implementarea ecuației (3.4.9):
[ ] [
]
[ ]
Pasul 5 este implementarea ecuației (3.4.10):
[
] [
]
[ ]
[
] [
]
[
]
[
]
Pasul 6 este implementarea ecuației (3.4.11):
[
]
[
]
[
]
[
]
[
]
[
]
Pasul 7 este implementarea ecuației (3.4.12):
27
[
]
([
] [
]
[ ]) [
]
[
]
([
] [
] ) [
]
[
]
[
]
[
]
Codul implementat aferent ecuațiilor (3.4.13) – (3.4.19) va fi descris în
capitolul 4.4.
28
4.Studiu de caz
4.1.Arhitectura hardware
În acest capitol, voi realiza o scurtă descriere a componentelor ce
alcătuiesc robotul. Voi insista atât asupra noțiunilor importante, cât și asupra
problemelor cu care m-am confruntat în utilizarea fiecărei componente.
4.1.1.Motorul Micro Metal Gearmotor 100:1
Motorul ales pentru tracțiunea robotului este ”Micro Metal Gearmotor
100:1”. Acesta are o forma lunguiață de 9.27 mm. Acest motor deține și un
mecanism de reducție al turației cu roți dințate ce determină un raport de 100:1
la axul de 3mm. Pe acest ax se prinde direct roata, precum în imaginea
următoare:
Figura 4.1.1.1 – Motorul de curent continuu cuplat la roata de tracțiune a robotului
29
Acest motor va opera la o tensiune variabilă între 2-6V printr-un semnal
PWM generat de un driver. Cuplul acestui motor este de 0.15 Kg*cm și este
conform calculelor efectuate anterior. Cuplul de start al motorului este de 0.55
Kg*cm. Fară sarcină motorul dezvoltă o turație de 120±10% RPM (Rotații pe
minut). În sarcină motorul dezvoltă un cuplu de 95±10% RPM. Dimensiunea
acestui motor este foarte mică, ceea ce determină o greutate a acestuia de
numai 10 g.
Am ales utilizarea acestui motor pentru că este ieftin, mic, ușor, dar în
special pentru că dezvoltă un cuplu mai mare decât cel necesar pentru robot,
determinând astfel și un grad crescut de fiabilitate al acestuia.
Controlul acestui motor se realizează prin transmiterea de semnale PWM
(Pulse-Width Modulation) către driverul de control, care amplifică aceste semnale
și le transmite către motor. Valoarea medie a tensiunii aplicate prin acest semnal
PWM se controlează prin raportul lungimii semanlului HIGH și al lungimii
semnalului LOW din cadrul unei perioade. Graficul următor reprezintă tensiunile
medii rezultate în urma aplicării diverselor semnale PWM:
Figura 4.1.1.2 - Exemple semnale PWM
Programul Arduino pe care am implementat controlul motorului deține o
funcție pentru transmiterea semnalului PWM. Aceasta are ca parametru un
număr întreg ce poate lua valori între -255 și 255, în funcție de tensiunea medie
a semnalului pe care îl generăm.
4.1.2.Servomotorul ROB-09065
Modelul de servomotor pe care l-am ales pentru controlul platformei este
ROB-09065. Acesta este un model mic și ieftin de servomotor cu 3 pini – 2
pentru alimentare și unul pentru control. Motorul include și câteva elemente
pentru montare.
30
Figura 4.1.2 - Servomotorul ROB-09065 [15]
Caracteristicile acestui motor sunt:
Tensiune de alimentare: 4.8-6V
Viteză de operare: 0.1 sec/60º
Cuplu: 1.4Kg/cm
Rază de operare: ~170º
Dimensiuni: 22.8 x 11.8 x 20.6 mm
Greutate: 9g
4.1.3.Driverul L293D
Pentru controlul motoarelor de curent continuu am ales folosirea driverului
L293D. Acesta are ca avantaje o alimentare la tensiuni între 4.5V și 36V,
protecție ESD (Elecrostatic discharge) internă, oprire în cazul supraîncălzirii, filtre
pentru zgomotele semnalelor de intrare, o intensitate de ieșire de 1A pe canal și
o intensitate de ieșire de vârf de 2A pe canal.
Acest driver este un circuit integrat cu 16 pini și are următoarea
arhitectură: ieșirea primului canal prin 1Y și 2Y; intrarea primului canal prin 1A și
2A; ieșirea celui de-al doilea canal prin 3Y și 4Y; intrarea celui de-al doilea canal
prin 3A și 4A; alimentarea circuitului la 5V prin VCC1; alimentarea pentru
semnalul de ieșire către motor la VCC2; pini de enable pentru cele două canale.
31
Figura 4.1.3.1 - Arhitectura driverului L293D [17]
Schema electrică de control implementată cu acest driver este
următoarea:
Figura 4.1.3.2 - Schema electrică de control a unui motor de curent continuu [17]
32
Tabel 4.1.3.1 - Tabel funcțional pentru controlul motoarelor de curent continuu [17]
După cum se observă în schemele de mai sus, controlul bidirecțional
pentru un motor de curent continuu se va face în felul următor: în primul rând
pinii de enable vor fi activați atunci când se va dori deplasarea motorului; pentru
o deplasare într-un sens se va scrie valoarea HIGH (5V) pe pinul de intrare 1A și
valoarea LOW (0V) pe pinul de intrare 2A; pentru deplasarea în sens opus, se va
scrie LOW pe pinul 1A și HIGH pe pinul 2A. Atunci când ambii pini de intrare au
aceeași valoare, motorul nu se va deplasa. Dacă pinii enable nu sunt activi
(HIGH), ieșirea va avea impedanță foarte mare, deci motorul nu se va deplasa.
Semnalul de intrare va fi un semnal PWM provenit de la platforma Arduino Mega
și va corespunde tabelului de mai sus.
Valorile maxime de tensiune, curent si temperatură care se pot aplica
acestui driver de motoare sunt prezentate în tabelul următor:
Parametru Descriere Valoare Unitate de măsură
VCC1 Tensiune Alimentare 36 V
VCC2 Tensiune Alimentare Ieșire 36 V
VI Tensiune de intrare 7 V
VO Interval Tensiune de Ieșire -3 : VCC2+3 V
IO Curent de Ieșire 1 A
TJ Temperatura maximă de joncțiune 150 ºC
Tabel 4.1.3.2 - Valori maxime aplicate la L293N [17]
4.1.4.Stabilizatoarele de tensiune LM7805 și LD33CV
Stabilizatorul de tensiune se utilizează pentru a menține tensiunea la o
valoare dorită, valoare care va oscila într-un anumit interval în funcție de
stabilizatorul ales.
33
Pentru robot am folosit un stabilizator de 5V pentru alimentarea driverelor
de motoare, un stabilizator de 3.3V pentru alimentarea senzorului MPU6050 și un
stabilizator de 12V pentru alimentarea motorului pas cu pas. Stabilizatorul de
3.3V este LD33CV. Stabilizatoarele de 12V și 5V fac parte din clasa de
stabilizatoare LM78XX. Pentru fiecare stabilizator în parte, „XX” reprezintă
tensiunea de ieșire a stabilizatorului. Am astfel două stabilizatoare de tensiune:
LM7805 și LD33CV.
LD33CV este un regulator de tensiune de 3.3V capabil să ofere la ieșire un
curent de până la 800 mA. Diferența dintre tensiunea de intrare și cea de ieșire
este disipată prin caldură ca la orice stabilizator de tensiune. Acesta va avea o
tensiune de 5V la intrare și va alimenta senzorul MPU6050 care necesită un
curent de până la 20 mA. Pinii se conectează precum în figura următoare:
Figura 4.1.4.1 - Stabilizatorul de 3.3V - LD33CV [19]
Stabilizatoarele de tensiune din clasa LM78XX permit un curent de ieșire
de până la 1A. Acestea au protecție la supraîncălzire, la scurtcircuit și oferă o
eroare mică pentru tensiunea de ieșire de ±4% pentru o temperatură între -40ºC
și 125º C.
Pentru introducerea corespunzătoare a acestora într-un circuit electric, se
folosește următoarea schemă cu atribuirea pinilor:
Figura 4.1.4.2 - Stabilizatorul LM78XX [19]
34
Valorile maxime pentru care circuitul este în siguranță sunt foarte
importante și trebuie să se țină cont de ele. Nu se recomandă sub nicio formă
depășirea acestor valori:
Simbol Parametru Valoare Unitate de măsură
Vi Tensiune de intrare 35 V
Rjc Rezistență termică de joncțiune 5 ºC/W
Topr Temperatură de operare -40 : +125 ºC
Tstg Interval de temperatură curentă -65 : +150 ºC
Tabel 4.1.4.1 - Valori maxime absolute [19]
Folosirea acestor stabilizatoare de tensiune se face împreună cu doi
condensatori: unul de 0.33 uF și unul de 0.1 uF. Schema electrică este
următoarea:
Figura 4.1.4.3 - Schema electrică LM78xx [19]
Principalele caracteristici pe care le-am avut în vedere la achiziționarea
stabilizatoarelor folosite sunt prezentate în tabelele următoare:
Simbol Parametru Condiții Min Uzual Ma
x
Unitate
de măsură
Vo Output Voltage Tj=+25ºC 4.8 5 5.2 V
Rline Line Regulation Tj=+25ºC
Vo=7V to 25V
- 4 100 mV
Rload Load Regulation Tj=+25ºC
Io=5mA to 1.5A
- 9 100 mV
35
Iq Quiescent Current Tj=+25ºC - 5 8 mA
RR Ripple Rejection F=120Hz
Vo=8V to 18V
62 73 - dB
Vdrop Dropout Voltage Tj=+25ºC
Io=1A
- 2 - V
Isc Short Circuit
Current
Vi=35V
Tj=+25ºC
- 230 - mA
Ipk Peak Current Tj=+25ºC - 2.2 - A
Tabel 4.1.4.2 - Parametrii LM7805 [19]
4.1.5.Acumulatorul LiPo Zippy Flightmax
Pentru robot am ales utilizarea a doi acumulatori Zippy Flightmax
1000mAh 2s1p 20C. Aceștia oferă un amperaj de 2A, suficient pentru o
autonomie foarte bună a robotului.
Figura 4.1.5.1 - Acumulator LiPo cu 2 celule [18]
Acest acumulator are următoarele specificații tehnice:
Dimensiuni 75mm x 12mm x 35mm
Capacitate 1000mAh
Tensiune 7.4V
Rata maximă de descărcare 30C
Conectori 3 pini pentru încărcarea fiecărei celule JST pentru descărcare
36
Timp de încărcare 1h pentru un curent de intrare de 1A
Masa 55 grame
Tabel 4.1.5.2 Specificații Zippy Flightmax 2s1p [18]
4.1.6.Senzor MPU-6050
Acest senzor este de tipul MPU (Motion Processing Unit) și este folosit ca
senzor de mișcare pentru tablete, controlere de jocuri, telecomenzi indicatoare și
alte produse. Acesta dispune de un sistem microelectromecanic giroscopic pe 3
axe, de un sistem microelectromecanic accelerometric pe 3 axe și un accelerator
hardware DMP (Digital Motion Processor) cu un port auxiliar I2C care poate
comunica cu diverși senzori, precum un senzor magnetometric.
Figura 4.1.6.1 - MPU-6050 [20]
MPU-6050 înglobează trei convertoare analog-numerice pe 16 biți pentru
valorile de ieșire ale giroscopului și alte trei convertoare analog-numerice tot pe
16 biți pentru valorile de ieșire ale accelerometrului. Pentru precizie ridicată, atât
la mișcări lente cât și la mișcări rapide se pot alege diverse scări de valori pentru
giroscop: ±250, ±500, ±1000 și ±2000 º/sec (dps) și diverse scări de valori
pentru accelerometru: ±2g, ±4g, ±8g și ±16g.
Un buffer de tip FIFO (Firs In First Out) de 1024 octeți ajută la scăderea
consumului de energie prin citirea rapidă a valorilor senzorului și stocarea
acestora în acest buffer, timp în care unitatea de procesare intră într-o stare cu
consum redus.
Utilizând toată puterea de procesare și componentele senzorilor, MPU-
6050 suportă o mare varietate de aplicații avansate de mișcare on-chip. Acesta
permite procesarea de mișcare cu consum redus în diverse aplicații fără cerințe
37
mari de nivel de procesare. Pentru a oferi o ieșire integrată de valori
MotionFusion, unitatea DMP reduce nivelul de procesare al procesorului de
sistem.
Comunicația cu toți registrii dispozitivului se realizează prin interfața I2C
(Inter Integrated Circuit) la 400kHz. Printre altele, MPU-6050 include și un
senzor de temperatură și un oscilator cu variații de ±1% pentru intervalul de
temperatură specific.
Pentru alimentare, acesta suporta tensiuni VDD în intervalul 2.37V –
3.46V. Acesta are și un pin de alimentare folosit în interfața I2C ca nivel logic ce
poate fi conectat la 1.8V±5% sau la VDD.
Prin tehnologiile foarte avansate folosite, acest dispozitiv are o dimenisune
de numai 4x4x0.9mm și totuși oferă performanțe de nivel înalt, zgomot redus cu
ajutorul filtrelor folosite, nu este greu de folosit și este de asemenea și foarte
ieftin. Acestea sunt principalele motive pentru care am ales folosirea acestui
senzor. La aceste motive s-a adăugat și faptul că acest dispozitiv este perfect
compatibil cu platforma Arduino Mega pe care am ales să o utilizez.
Platforma pe care este montat dispozitivul este mică și foarte ușoară.
Aceasta este prezentată în figura următoare:
Figura 4.1.6.1 - MPU-6050 montat pe o platformă pentru conectare facilă cu Arduino [20]
Pinii de conectare la acest senzor sunt:
VDD – alimentare (3.3V)
GND - împământare
INT – pentru utilizarea întreruperilor
SYNC - sincronizare
SCL – pentru comunicația prin I2C cu Arduino Mega
SDA - pentru comunicația prin I2C cu Arduino Mega
VIO - pentru comunicația prin I2C (3.3V)
CLK - ceas
38
ASCL - pentru comunicația prin I2C cu alte dispozitive
ASDA - pentru comunicația prin I2C cu alte dispozitive
Pentru a realiza comunicația acestui senzor cu Arduino, este necesară doar
conectarea pinilor SCL și SDA prin care se face schimbul de date, conectarea
pinilor VDD și VIO la pinul de 3.3V oferit de Arduino sau la o sursă exterioară de
tensiune și, bineînțeles, conectarea pinului GND la masa sursei de alimentare.
Pentru o bună funcționare a acestui dispozitiv, nu trebuie depășite
următoarele valori maxime admise:
Parametru Valoare
Alimentare (VDD) -0.5V : +6V
Vio -0.5V : VDD+0.5V
REGOUT -0.5V : 2V
Tensiunea de intrare pentru INT, SCL,
SDA.
-0.5V : VDD+0.5V
Accelerația 10.000g pentru 0.2ms
Temperaturi suportate -40ºC : +105 ºC
ESD ( Electrostatic Discharge) 2kV (Human Body Model) 200V (Machine Model)
Tabel 4.1.6.1 - Valori maxime admise [20]
Microelectromecanismul pe 3 axe al giroscopului din cadrul MPU 6050
deține o serie de avantaje precum:
Ieșiri digitale corespunzătoare celor 3 axe – X,Y și Z - ce reprezintă
valoarea unui unghi scalat la un anumit interval setat de programator;
Pin-ul SYNC folosit pentru sincronizarea cu diferite dispozitive ce folosesc
prelucrare de imagini, video, sau GPS;
Convertor analog-numeric pe 16 biți pentru fiecare axă;
Utilizarea unui senzor de temperatură reduce necesitatea calibrării
senzorului, îndeplinește deci funcția de autocalibrare;
Reducerea zgomotelor prin folosirea de filtre;
Filtru trece-jos programabil digital;
Curent de operare de 3.6 mA;
Curent în standby de 5uA;
Calibrarea factorului de sensibilitate.
Microelectromecanismul pe 3 axe al accelerometrului din cadrul MPU 6050
deține o serie de avantaje precum:
39
Ieșiri digitale corespunzătoare celor 3 axe – X,Y și Z - cu posibilitatea
scalării în mai multe intervale;
Convertor analog-numeric pe 16 biți pentru fiecare axă ce oferă valori la
intervale foarte mici de timp fară utilizarea unui multiplexor extern;
Curent de operare de 500uA;
Există posibilitatea de operare și la un curent mai mic: 10uA la 1.25Hz,
20uA la 5Hz, 60uA la 20Hz și 110uA la 40Hz;
Detecția orientării si semnalizarea acesteia;
Detecția loviturii;
Întreruperi programate de utilizator;
Întreruperi pentru cădere liberă;
Întrerupere pentru forțe G mari;
Alte avantaje ale senzorului MPU-6050 ar fi următoarele:
Magistrala I2C suplimentară pentru citirea senzorilor externi precum
magnetometru;
Un curent de doar 3.9 mA atunci când toate cele 6 axe și procesorul de
mișcare sunt în funcțiune;
Tensiune de alimentare între 2.37V și 3.46V;
Dimensiune și greutate foarte mici;
Buffer de 1024 octeți ce permite intrarea în stare de standby și apoi citirea
valorilor pentru economisirea de energie;
Senzor digital de temperatură;
Filtre programabile digital pentru giroscop, accelerometru și senzorul de
temperatură;
Toleranță la șocuri de 10.000g;
Comunicație la 400kHz prin protocolul I2C.
4.1.7.Modulul Bluetooth RN-42
Acest modul de bluetooth este montat pe o platformă pentru o conectare
ușoară cu Arduino Mega. În principiu, acest modul trebuie doar alimentat și
conectat prin doi pini de comunicație pentru realizarea unui schimb de date
serial. Acesta este un modul de clasa a 2-a, deci poate comunica până la distanțe
de 20 m.
40
Figura 4.1.7.1 - Modulul bluetooth RN-42 [21]
Acesta este foarte mic (15mm x 50mm x 5mm) și consumă foarte puțină
energie. El oferă o viteză de transfer de până la 3Mbps. Suportă multe protocoale
de comunicație, precum Bluetooth 2.1/2.0/1.2/1.1 . Dispune și de o antenă de
dimensiuni mici, deși există și variante fără antenă pentru diverse aplicații.
Acesta suportă transmisii de date prin conexiuni UART (Universal
Asynchronous Receiver/Transmitter) sau USB (Universal Serial Bus), ambele în
modul HCI ( Host Controller Interface).
Câteva avantaje ale acestui modul de comunicație bluetooth sunt
următoarele:
Viteza este de la 1200bps până la 921kbps;
Comunică la distanțe de până la 20m;
Are o putere de transmitere de 4dBm;
Comunică în intervalul de frecvență 2402-2480 MHz – rezultă deci
79 de canale de comunicație la intervale de 1MHz;
Comunicația este sigură, realizându-se o criptare pe 128 de biți;
Corectează erorile de transmitere pentru un nivel ridicat al
integrității pachetelor de date.
Specificațiile electrice de consum mediu ale modulului RN-42 sunt
prezentate în tabelul următor:
41
Parametru Minim Nominal Maxim Unitate de măsură
Tensiune de alimentare 3 3.3 3.6 V
Curent mediu consumat
Modul ON 40 mA
Modul Conectat Idle (cu detecție
conexiuni)
25 mA
Modul Conectat Idle (cu detecția
conexiunilor la 100 mili secunde)
12 mA
Conectat cu transfer de date 40 45 50 mA
Modul Idle – Deep Sleep (consum foarte redus)
26 mA
Tabel 4.1.7.2 - Specificații electrice de consum [21]
Distanța de transmitere a datelor variază în funcție de numărul de
obstacole astfel:
Cu un obstacol (ex.:un perete) distanța la care poate comunica este
de 18m;
Cu două obstacole (ex.: doi pereți) distanța la care poate comunica
este de 16.5m;
Cu trei obstacole (ex.: trei pereți) distanța la care poate comunica
este de 11m.
Acest modul este foarte ușor de folosit. Alimentarea se poate face la
Arduino la tensiunea de 5V, pentru ca RN-42 dispune de un regulator de tensiune
de 3.3V. Pentru comunicația cu Arduino, acesta are doi pini- Rx si Tx care se
folosesc pentru recepție și transmisie de date. Pinul Rx al modulului se
conectează la pinul Tx al platformei Arduino, iar pinul Tx al modulului se
conectează la pinul Rx al platformei Arduino.
4.1.8.Realizarea circuitului electric de alimentare și control al motoarelor
Atât circuitul electric de alimentare, cât și circuitul electric cu driverele
pentru conntrolul motoarelor pas cu pas și de curent continuu, au fost realizate
pe o placă de cablaj perforată de aproximativ 2cm x 12cm. Conexiunile dintre
acest circuit electric și restul componentelor (motoare, acumulatori, Arduino) se
fac prin fire tată-tată de diverse culori. Astfel, testarea funcționării
componentelor s-a putut realiza pentru fiecare în parte, separat, pentru o
depanare ușoară.
42
Figura 4.1.8.1 - Circuitul electric de alimentare și de control al motoarelor robotului
Prima parte a circuitului cuprinde pinii de conexiune a acumulatorilor LiPo,
stabilizatoarele de tensiune de 12V și 5V și pinii de ieșire la care se pot conecta
direct firele de alimentare pentru Arduino și senzori. Fiecare din acești pini a fost
marcat cu o etichetă corespunzătoare pentru a evita greșelile de conexiune
dintre componente.
Figura 4.1.8.2- Circuitul de alimentare al robotului
43
A doua parte a circuitului cuprinde driverul ce controlează motoarele de
curent continuu de tracțiune. Intrările și ieșirile acestui driver se conectează la
componentele corespunzătoare printr-un conector mamă cu 8 pini. Pinii au fost
numerotați cu cifre de la 1 la 8 și au următoarele semnificații:
Pinii 1 și 4 se conectează la pinii 3, respectiv 2 al paltformei Arduino și
reprezintă comanda PWM trimisă de Arduino către primul canal al
driverului ce controlează motorul 1 de curent continuu al robotului;
Pinii 2 și 3 sunt pinii la care se conectează motorul 1 și reprezintă ieșirea
primului canal al driverului ce transmite semnalul PWM amplificat către
motorul 1;
Pinii 5 și 8 se conectează la pinii 4, respectiv 5 al platformei Arduino și
reprezintă comanda PWM trimisă de Arduino către al doilea canal al
driverului ce controlează motorul 2 de curent continuu al robotului;
Pinii 6 și 7 sunt pinii la care se conectează motorul 2 și reprezintă ieșirea
celui de-al doilea canal al driverului ce transmite semnalul PWM amplificat
către motorul 2.
Figura 4.1.8.3 - Driverul pentru motoarele de curent continuu
Pentru a nu deteriora driverele de control la lipirea acestora pe placa de
cablaj, am folosit două socluri pentru circuite integrate cu 16 pini. Aceste socluri
44
ajută și la o înlocuire rapidă a driverului în cazul defectării acestuia. Celelalte
componente ale circuitului au fost lipite direct pentru că sunt mai puțin sensibile
la temperaturi ridicate.
4.1.9.Conexiunea elementelor hardware cu platforma Arduino Mega
Platforma Arduino Mega controlează indirect, prin intermediul unui driver
cu punte H, motoarele de tracțiune ale robotului, servomotorul care modifică
unghiul de înclinare al platformei, primește date de la senzorul giroscopic montat
pe platformă și comunică cu telefonul printr-un modul de comunicație bluetooth.
Alimentarea a fost făcută la unul din acumulatorii LiPo utilizați.
Figura 4.1.9.2 - Schema de conexiuni cu ArduinoMega2560
45
4.1.10.Asamblarea componentelor
Asamblarea robotului a constat în montarea tuturor componentelor
prezentate anterior pe o platformă de plastic. Pentru că cea mai importantă
funcție a robotului este deplasarea, am început cu montarea elementelor care
asigură această funcție: motoarele și roțile care au fost atent fixate în partea
posterioară a robotului și bila care susține partea din față a robotului.
Figura 4.1.10.1 - Montarea roților și a motoarelor pe platforma de plastic
Pasul următor a fost fixarea bateriilor LiPo între două placuțe de plexiglas
prinse cu șuruburi. Acest ansamblu a fost fixat în partea din față a robotului,
pentru a compensa greutatea roților și a servomotorului ce va fi montat în partea
din spate.
Figura 4.1.10.2 - Fixarea acumulatorilor LiPo
46
Deasupra acumulatorilor a fost fixată cea mai importantă componentă,
care este „creierul” robotului – paltforma Arduino Mega. Mufa USB a fost
îndreptată spre exteriorul robotului pentru programarea sau reprogramarea facilă
a robotului.
Figura 4.1.10.3 - Montarea platformei Arduino Mega
Ultimele componente montate au fost ansamblul motorului pas cu pas și
circuitul cu driverele de control al motoarelor și stabilizatoarele de tensiune.
Figura 4.1.10.4 - Montarea servomotorului și a circuitului de alimentare si control al
motoarelor
47
Ultimul pas a fost conexiunea tuturor componentelor prin fire tată-tată de
diverse culori, montarea a două butoane ON/OFF cu reținere și fixarea carcasei
deasupra platformei de plastic.
Figura 4.1.10.5 - Aspectul final al robotului
4.2.Controlul automat al platformei
Controlul platformei se face cu ajutorul unui servomotor. Au fost
implementate două moduri de control al platformei:
Prima modalitate de control este cu ajutorul smartphoneului care are rol
de telecomandă: în funcție de unghiul la care telefonul va fi înclinat,
platforma se va înclina sub același unghi. Acest control se va efectua în
buclă închisă. Accelerometrul telefonului va măsura unghiul la care acesta
se înclină și va transmite datele prin bluetooth la platforma Arduino.
Aceasta va da comanda către motor prin intermediul driverului L293N.
A doua modalitate de control va fi automată. Platforma Arduino primește
un unghi la care platforma să rămână constant înclinată (o referință) și cu
ajutorul senzorului MPU-6050 se va menține acest unghi printr-un control
în buclă închisă. Pentru că motorul de acționare este unul pas cu pas (cu
mișcări foarte precise) și pentru că angrenajul de acționare a platformei
48
este cu roți dințate, a fost implementat un regulator P pentru acest sistem
de control automat.
Principalele probleme care vor fi abordate în acest capitol sunt achiziția
datelor de la senzorul MPU-6050 și implementarea algoritmului de control pe
Arduino.
4.2.1. Achiziția datelor de la senzorul MPU-6050
Datele vor fi transmise de senzor către Arduino printr-o interfață I2C.
Aceste date reprezintă valori reale scalate ale unghiului de înclinare al platformei.
Pentru fiecare din cele 3 axe – X,Y și Z - vor exista două valori reale: una de la
senzorul giroscopic și una de la accelerometru.
Regiștrii interni sau memoria dispozitivului MPU-6050 pot fi accesate doar
prin interfața I2C (Inter – Integrated Circuit) la 400kHz. Acest tip de comunicație
se realizează prin intermediul a două fire: un fir pentru schimbul serial de
semnale ce reprezintă date (SDA) și un fir prin care se schimbă serial informații
de ceas (SCL). În general firele sunt bidirecționale. Dispozitivele care comunică
printr-o astfel de interfață sunt fie „master”, fie „slave”. Master-ul pune adresa
dispozitivului slave pe magistrală, iar dispozitivul slave care își citește adresa pe
magistrală trimite un mesaj de recunoaștere către master.
MPU-6050 mereu este în postura de slave atunci când comunică cu un
procesor de sistem. Adresa senzorului este b110100X, având deci lungimea de 7
biți. Bitul LSB (Least Significant Bit) este determinat de nivelul logic pe pinul
AD0, care este standard setat pe 0. Acest lucru permite ca doi senzori de acest
tip sa fie conectați simultan la aceeași magistrală.
Comunicația pe magistrala I2C începe atunci când masterul pune condiția
de START pe magistrală. Această condiție este definită ca tranziția liniei SDA din
HIGH în LOW în timp ce linia SCL este HIGH. Linia se consideră ocupată până
când masterul pune condiția de STOP, care este definită ca trecerea din LOW în
HIGH a liniei SDA în timp ce linia SCL este LOW.
49
Figura 4.2.1.1 - Condițiile de start și stop în comunicația I2C [20]
Lungimea unui cuvânt transmis prin I2C este de 8 biți. Nu există nicio
limită de cuvinte transmise într-o sesiune de transfer. Fiecare cuvânt transferat
trebuie sa fie însoțit de un semnal ACK (Acknowledge). Semnalul de ceas pentru
ACK este generat de master, timp în care dispozitivul slave generează semnalul
SDA. Acest semnal SDA este trecerea din HIGH în LOW și menținerea acestei
stări pe toată durata porțiunii de semnal HIGH de pe firul SCL.
Dacă dispozitivul slave este ocupat și nu poate transmite sau primi încă un
cuvânt de date decât după îndeplinirea altui task, se poate menține SCL LOW,
forțând astfel masterul să rămână în starea de așteptare. Transferul se reia în
momentul în care dispozitivul slave e pregătit și eliberează linia SCL.
Figura 4.2.1.2 - Semnalul de acknowledge pe magistrala I2C [20]
După începerea comunicației cu condiția de START îndeplinită, dispozitivul
master trimite o adresa pe 7biți ce aparține unui dispozitiv slave, urmată de un
bit care reprezintă necesitatea masterului de citire sau scriere. După trimiterea
adresei, masterul eliberează linia SDA și așteaptă semnalul de SCK de la slave.
Fiecare cuvânt transmis trebuie urmat de un bit de acknowledge.
Dispozitivul master poate genera o condiție de start pentru a comunica cu
alt slave fară să genereze o condiție de STOP înainte. Astfel, o trecere din LOW în
HIGH a liniei SDA cu linia SCL HIGH definește condiția de STOP. Toate
schimbările liniei SDA ar trebui să aibă loc atunci când SCL este LOW, mai puțin
în cazul condiților de START și STOP.
50
Pentru a scrie regiștrii interni ai modulului MPU-6050, masterul transmite
condiția de START, urmată de adresa dsipozitivului slave – AD - și de bitul de
scriere (W=0). La al 9-lea ciclu de ceas, când semnalul este HIGH, MPU-6050
transmite semnalul de acknowledge, după care masterul pune adresa registrului
– RA (Register Address) - pe magistrală. După ce MPU-6050 confirmă primirea
acestei adrese prin semnal ACK, transferul de date continuă până la semnalul de
STOP.
Pentru a nu întrerupe transferul de date pentru trimiterea fiecărei adrese
de registru, dispozitivul slave incrementează această adresă atâta timp cât
primește date.
Master START AD+W RA DATA STOP
Slave ACK ACK ACK
Tabel 4.2.1.1 - Succesiunea de scriere a unui singur cuvânt [20]
Master START AD+W RA DATA DATA P
Slave ACK ACK ACK ACK
Tabel 4.2.1.2 - Succesiunea de scriere a mai multor cuvinte [20]
Următorul tabel descrie câțiva termeni importanți folosiți în comunicația
I2C:
Semn Descriere
S Condiția de START: SDA trece din HIGH
în LOW în timp ce SCL este HIGH
AD Adresa dispozitivului Slave
W Bitul de scriere (0)
R Bitul de citire (1)
ACK Acknowledge: linia SDA este LOW, în
timp ce linia SCL este HIGH la al 9-lea ciclu de ceas
NACK Not-Acknowledge : linia SDA rămâne HIGH la al 9-lea ciclu de ceas
RA Adresa registrului intern MPU-6050
DATA Datele transmise sau recepționate
P Condiția de STOP: SDA trece din LOW în HIGH în timp ce SCL este HIGH
Tabel 4.2.1.3 - Termeni I2C [20]
51
Pentru utilizarea acestui protocol de comunicație pe microcontrollerul
Arduino, se va folosi librăria „Wire”. Această librărie are implementate câteva
funcții precum „begin()”, „requestFrom()”, „beginTransmission”,
„endTransmission()”, „write()”, „available()”, „read()”, „onReceive()”,
„onRequest()”. Utilitatea acestora va fi explicată în capitolul ce descrie
implementarea codului. Platforma Arduino Mega are doi pini speciali pentru
această comunicație: SDA (pinul 20) și SCL (pinul 21) și este perfect compatibilă
cu senzorul MPU-6050.
4.2.2. Prelucrarea datelor cu filtrare Kalman
Conform capitolului 3.4, pentru implementarea filtrului Kalman este nevoie
de parcurgerea a șapte pași, fiecare corespunzător unor anumite ecuații. Voi
prezenta în continuare variabilele și implementarea comenzilor pentru realizarea
unei funcții de filtrare Kalman pentru Arduino.
Variabilele folosite sunt următoarele:
Unghi – valoarea filtrată a unghiului;
bias – reprezintă eroarea giroscopului;
gyro – valoarea indicată de giroscop;
rate – valoarea indicată de giroscop minus eroarea acestuia;
acc – valoarea indicată de accelerometru;
timp – timpul care a trecut de la o filtrare la alta;
P(2x2) – matricea de covarianță a erorii;
K(2,1) – amplificarea Kalman;
Y – reziduul;
S – covarianța reziduului;
Q_unghi=0.001 – reprezintă varianța accelerometrului;
Q_bias=0.003 – reprezintă varianța erorii;
R_unghi=0.03 – zgomotul măsurătorilor.
Ultimele trei variabile sunt constante. Am preluat valoarea optimă a
acestora calculată pentru senzorul MPU-6050 de pe siteul
https://github.com/TKJElectronics/KalmanFilter/blob/master:
Pasul 1 este updatarea unghiului în funcție de timpul timp și de eroarea
giroscopului conform ecuației (3.4.13):
rate=gyro-bias;
unghi=unghi + timp*rate;
Pasul 2 este calculul matricei de covarianță conform ecuației (3.4.14):
52
P[0][0] += timp * (timp*P[1][1] - P[0][1] - P[1][0] +
Q_unghi);
P[0][1] -= timp * P[1][1];
P[1][0] -= timp * P[1][1];
P[1][1] += Q_bias * timp;
Pasul 3 este calculul covarianței reziduului conform ecuației (3.4.15):
S = P[0][0] + R_unghi;
Pasul 4 este updatarea valorilor filtrului conform ecuației (3.4.16):
K[0] = P[0][0] / S;
K[1] = P[1][0] / S;
Pasul 5 este updatarea reziduului conform ecuației (3.4.17):
Y = acc - unghi;
Pasul 6 este calcularea unghiului și a erorii conform ecuației (3.4.18) :
unghi += K[0] * Y;
bias += K[1] * Y;
Ultimul pas este updatarea matricii de covarianță conform ecuației
(3.4.19):
P[0][0] -= K[0] * P[0][0];
P[0][1] -= K[0] * P[0][1];
P[1][0] -= K[1] * P[0][0];
P[1][1] -= K[1] * P[0][1];
În urma implementării acestei funcții în programul Arduino, am obținut un
set de date pe care le-am reprezentat grafic mai jos. Linia roșie reprezintă
valorile unghiului filtrat, linia verde reprezintă valorile unghiului indicat de
giroscop, linia albastră reprezintă valorile unghiului indicat de accelerometru:
53
Figura 4.2.2.1 – Diferența între valorile filtrare și valorile nefiltrate ale unghiului
platformei
4.2.3. Algoritmul de control automat al platformei
Majoritatea sistemelor de reglare automată presuspun menținerea unei
variabile la o valoare constantă. În acest caz, variabila reglată este valoarea
unghiului de înclinare al platformei pe axa longitudinală a robotului.
Sistemul reglat este reprezentat de servomotorul care acționează asupra
platformei, senzorul accelerometric care măsoară unghiul de înclinare al
robotului, interfața grafică de pe telefon prin care utilizatorul trimite referința
sistemului și microcontrollerul Arduino care generează comanda către
servomotor. Servomotorul este deci elementul de execuție al sistemului.
Perturbația sistemului este înclinarea robotului și este măsurată folosind senzorul
MPU-6050. Pentru feedbackul necesar reglării, se însumează unghiul înclinării
robotului (perturbația) cu unghiul de înclinare al platformei față de robot.
Unghiul platformei se cunoaște datorită faptului că servomotorul are un sistem
intern de reglare a poziției axului, astfel știm exact cum acționează asupra
platformei.
54
Figura 4.2.3.1 - Schema generală de control
r reprezintă referința trimisă de utilizator prin bluetooth către Arduino;
aceasta poate lua valori între -60º și +60º;
e reprezintă eroarea sistemului – diferența dintre referință și ieșirea
sistemului;
u reprezintă comanda calculată de regulator și trimisă către servomotor;
y reprezintă ieșirea sistemului, adică unghiul platformei față de orizontală;
yp reprezintă unghiul dintre platformă și șasiul robotului;
v reprezintă perturbația sistemului – înclinarea șasiului robotului;
Pentru a ușura proiectarea algoritmului de reglare automată, am apelat la
uneltele de simulare din cadrul programului Matlab. Pentru simularea sistemului
este nevoie de cunoașterea modelului matematic al procesului ce trebuie reglat.
Am ales identificarea experimentală ca metodă de determinare a acestui model.
Am analizat astfel funcționarea sistemului în buclă închisă și am aproximat
comportamentul fizic cu un model matematic corespunzător.
Pentru identificarea parametrilor modelului am trasat caracteristica statică,
adică dependența în regim staționar a ieșirii în funcție de intrare și apoi
caracteristica dinamică, adică dependența de timp a ieșirii procesului.
55
Figura 4.2.3.2 - Caracteristica statică a procesului în buclă deschisă
Se observă pe grafic valorile ieșirii în funcție de intrare marcate cu steluță
albastră și aproximarea acestei caracteristici trasată printre aceste puncte.
Aproximarea a fost realizată folosind comanda polyfit din programul Matlab.
Comanda a generat coeficienții polinomului caracteristic al acestui proces:
1.0058 și -0.8674. Pe acest grafic se observă o caracteristică aproximativ liniară
a ieșirii în funcție de intrare. Astfel voi putea folosi comenzi în acest interval
pentru determinarea caracteristicii dinamice.
Pentru realizarea caracteristicii dinamice am aplicat servomotorului
comanda u0=40 pentru poziționarea platformei la 40º, am așteptat intrarea în
regimul staționar, iar apoi am aplicat o treaptă ust=120, pentru poziționarea
platformei la 120º. Valorile ieșirii au fost citite la intervale de 40 de milisecunde,
fiind vorba de un proces foarte rapid.
56
Figura 4.2.3.3 - Caracteristica dinamică a procesului în buclă deschisă
Punctul marcat pe grafic reprezintă punctul de intrare în regimul staționar,
avem astfel timpul tranzitoriu tt=680 ms. Se observă că procesul are un
comportament ce poate fi caracterizat de o funcție de transfer de ordinul I de
forma:
Pentru determinarea constantei Kp vom aplica următoarea formulă:
Valorile yst și y0 au fost citite de pe graficul de mai sus și înlocuite în
formula (3.2.3.2), rezultând astfel valoarea constantei Kp:
Valoarea constantei Ti s-a determinat trasând tangenta în originea
graficului de mai sus (caracteristicii dinamice). Constanta de timp se va măsura
din origine până în momentul corespunzător intersecției tangentei cu valoarea de
regim staționar yst. Această constantă trebuie sa fie aproximativ din timpul
tranzitoriu. Am determinat astfel Ti=0.18. Funcția de transfer corespunzătoare
procesului este:
57
Folosind în programul Matlab comanda step(Hp) am determinat raspunsul
la treaptă al acestui sistem pentru a verifica asemănarea cu caracteristica
dinamică a procesului. Diferențele dintre acestea se datorează zgomotului datelor
înregistrate de senzorul accelerometric, precum și perioadei necesare stabilizării
valorilor înregistrate de senzor.
Figura 4.2.3.4 - Răspunsul la treaptă al modelului determinat
Pentru proiectarea unui sistem de reglare automată se impune definirea
unor cerințe de performanță necesare pentru pentru alegerea unei soluții de
automatizare optime. Pentru sistemul de reglare al platformei, principalele
obiective sunt eroarea staționară egală cu zero și un timp tranzitoriu mai mic de
600 ms în cazul rejectării perturbațiilor de tip treaptă. Funcția de transfer a
procesului este de ordinul întâi, astfel, pentru a avea eroare staționară zero și
pentru a controla ușor timpul tranzitoriu, am ales să introduc în sistem un
regulator PI. Legea de reglare PI este dată de următoarea formulă:
(
)
KR se numește coeficient de proporționalitate;
Ti este constanta de integrare.
58
Pentru a obține valorile optime ale parametrilor de reglare - KR și Ti, am
simulat răspunsul la treaptă al sistemului în buclă închisă cu diverse valori ale
acestora. Am realizat următoarea schemă în mediul de simulare simulink al
programului Matlab:
Figura 4.2.3.5 - Schema simulink a sistemului în buclă închisă
Servomotorul acceptă comenzi în intervalul [10, 170]. Pentru a nu aplica
comenzi foarte mari servomotorului, am ales KR=1.5. După fixarea valorii pentru
constanta proporțională, am variat constanta de integrare pentru atingerea unui
timp tranzitoriu cât mai mic. Am ales valoarea optimă a acestei constante:
Ti=0.16. Funcția de transfer a regulatorului implementat este următoarea:
Pentru parametrii regulatorului astfel aleși, am obținut un timp tranzitoriu
de 380 ms. Acest timp oferă o bună rejecție a perturbațiilor, îmbunătățind
considerabil performanțele sistemului. Am obținut următorul răspuns la intrarea
de tip treaptă a sistemului în buclă închisă:
59
Figura 4.2.3.6 – Răspunsul sistemului în buclă închisă la intrarea de tip treaptă
Se observă în următorul grafic încadrarea comenzii în limitele normale:
Figura 4.2.3.7 Comanda sistemului în buclă închisă la intrarea de tip treaptă
Următorul pas a fost discretizarea regulatorului, folosind comanda c2d() a
programului Matlab. Parametrii acestei comenzi sunt: sistemul care trebuie
60
discretizat – HPI , perioada de eșantionare – 0.04 s și metoda de discretizare –
”Tustin”. Am obținut astfel următoarea funcție de transfer:
Pentru implementarea acestui regulator am folosit următoarea formulă:
[ ]
[ ]
Prin introducerea formei discretizate a funcției de transfer în formula de
mai sus, am obținut următoarea ieșire a regulatorului, care reprezintă de fapt
comanda procesului:
[ ] [ ] [ ] [ ]
Am obținut astfel o foarte bună rejectare a perturbațiilor în urma
implementării acestui algoritm de control automat pentru platformă. Acest
rezultat se poate observa în graficul următor, care reprezintă ieșirea sistemului în
timp, în prezența perturbațiilor:
Figura 4.2.3.8 - Răspunsul sistemului în prezența perturbațiilor
61
4.3.Dezvoltarea interfeței grafice pentru controlul robotului
Controlul robotului se realizează prin intermediul unui smartphone, care
comunică prin intermediul protocolului de comunicație bluetooth cu modulul de
bluetooth RN-42 conectat la platforma Arduino. Smartphone-ul transmite
informații în funcție de valorile indicate de senzorul accelerometric. Aceste
informații sunt recepționate de modulul RN-42 și trimise mai departe către
Arduino. Informațiile sunt analizate și utilizate pentru a controla fie motoarelor
de tracțiune, fie motorul care controlează platforma. Smartphone-ul va avea rol
de telecomandă pentru robot.
Pentru a realiza această transmisie și prelucrare a datelor, am dezvoltat
atît o aplicație suportată de sistemul de operare al smartphone-ului – Android
4.0.4, cât și o funcție de recepție și prelucrare a datelor pentru Arduino.
Descrierea acestora se va realiza în cadrul acestui capitol.
4.3.1.Dispozitivul de control al robotului
Dispozitivul care va controla robotul este un smartphone – Samsung
Galaxy Gio S5660. Acesta utilizează un procesor de 800MHz Qualcomm Turbo și
o memorie RAM de 278 de MB pentru un nivel de performanță ridicat.
Figura 4.3.1.1 - Samsung Galaxy Gio S5660 [26]
62
Motivul pentru care am ales utilizarea acestui smartphone este pentru că
deține un senzor accelerometric care permite controlul facil al robotului. Controlul
se va realiza doar prin mișcarea telefonului pe axele X (lungime) și Y(lațime).
Menținerea acestuia la orizontală nu va transmite nicio comandă către robot.
Robotul va primi comenzi în momentul înclinării telefonului. Cu cât unghiul de
înclinare va fi mai mare, cu atât viteza robotului va fi mai mare.
Acest telefon utilizează sistemul de operare Android 4.0.4, numit și Ice
Cream Sandwich. Android se bazează pe sistemul de operare freeeware Linux.
Acesta este conceput în special pentru telefoane, tablete sau alte dispozitive cu
ecran de tipul touchscreen. Android este un sistem de operare ce poate fi folosit
gratuit. Principalul avantaj al acestuia este că își poate extinde foarte ușor
funcționalitățile prin dezvoltarea de aplicații Java.
4.3.2.Implementarea aplicației pentru controlul robotului
Pentru implementarea aplicației, am folosit pachetul Android SDK al
mediului de dezvoltare Eclipse descris în capitolul anterior. Voi prezenta în
continuare pașii urmați în dezvoltarea aplicației.
Primul pas în dezvoltarea aplicației a fost crearea unei interfețe grafice în
limbajul XML. Interfața este formată din 3 ecrane: primul ecran pentru controlul
robotului, al doilea pentru trimiterea unei referințe pentru reglarea automată a
platformei și al treilea ecran pentru controlul manual al platformei. Toate aceste
ecrane au în comun 4 butoane plasate în partea de deasupra a ecranului, care
formează meniul aplicației: butonul ”Control Robot” care intră în ecranul de
control al robotului; butonul ”Control Platformă” care intră în ecranul de control
al platformei; butonul ”Resetează Aplicația” care resetează aplicația și butonul
”Închide Aplicația” care închide aplicația.
Figura 4.3.2.1 - Meniul interfeței
Când aplicația este rulată, intră în mod automat în ecranul de control al
robotului:
63
Figura 4.3.2.2 - Ecranul de control al robotului
Pe lângă meniul aplicației, acest ecran conține două câmpuri care indică
puterea aplicată fiecărui motor în procente și un buton principal care, atunci când
este apăsat, trimite date de control la robot (controlul robotului se face numai
dacă acest buton este apăsat).
Dacă se apasă în meniu butonul ”Control Platformă”, pe interfață va
apărea al doilea ecran al aplicației:
64
Figura 4.3.2.3 - Ecranul de trimitere a referinței pentru reglarea automată a platformei
Pe lângă meniul aplicației, acest ecran conține un buton pentru selecția
modului de control al robotului: automat sau manual, un câmp în care se poate
introduce referința pentru reglarea automată a platformei (o valoare între -60 și
60 care reprezintă unghiul de referință al platformei) și un buton pentru
trimiterea acestei referințe.
Dacă se selecteaza modul de control manual al platformei, pe interfață va
apărea al treilea ecran al aplicației, care este asemănător cu primul ecran:
65
Figura 4.3.2.4 - Ecranul de control manual al platformei
Acesta conține un câmp ce indică unghiul platformei și un buton pentru
trimiterea datelor de control al platformei. Aceste date reprezintă unghiul de
înclinare al telefonului, care este trimis către robot dacă se apasă butonul din
partea de jos a ecranului.
Al doilea pas a fost implementarea clasei și metodelor utilizate de aplicație.
Singura clasă a aplicației este MainActivity.java . Aici sunt declarate la
început variabilele utilizate: butoanele, câmpurile de text, câmpurile de
introducere text, câteva variabile folosite pentru a prelua datele de la senzorul
accelerometric și câteva variabile pentru transmiterea datelor prin bluetooth,
printre care și adresa modulului de bluetooth al robotului: ”00:06:66:4B:40:FD”.
Prima metodă se numește onCreate() și este automat apelată la
pornirea aplicației. Aici se selectează layoutul aplicației și sunt selectate
elementele care apar pe ecranul de pornire. Se realizează conexiunea bluetooth
și se verifică starea conexiunii. Pentru acest lucru se creează următoarele
obiecte:
btAdapter - are valoarea adaptorului de bluetooth local; este primul
pas care trebuie făcut pentru a realiza o conexiune, o împerechere
sau căutarea dispozitivelor bluetooth;
66
btScoket – acest obiect reprezintă tipul interfeței de comunicație
care pentru Android este de obicei RFCOMM (radio frequency
communication); atunci când se realizează conexiunea cu un
dispozitiv, se va returna un obiect care reprezintă un socket;
outStream – este o stivă prin intermediul căreia se transmit datele
prin bluetooth.
În metoda onCreate() se inițializează și variabilele prin care se preiau
datele de la senzorul accelerometric. Se va crea obiectul sensorManager, prin
intermediul căruia se pot accesa senzorii telefonului și obiectul sensor, care
reprezintă chiar senzorul accelerometric. Tot în această funcție sunt declarate
funcționalitățile fiecărui buton:
Butonul controlRobot - schimbă valoarea unei variabile de tip boolean
numită layoutControlRobot în ”true” care indică tipul ecranului
curent; apăsarea acestui buton selectează totodată elementele care apar
pe ecranul de control al robotului;
Butonul control Platformă – schimbă valoarea variabilei
layoutControlRobot în ”false” și selectează elementele care apar pe
ecranul de control automat al robotului;
Butonul inchide – închide aplicația prin apelul funcției finish();
Butonul reseteaza – resetează aplicația prin memorarea activității
curente în variabila intent, apelarea funcției finish(), urmată de
apelul funcției startActivity(intent);
Butonul butonPrincipal - schimbă valoarea unei variabile de tip boolean
atunci când este apăsat, pentru a permite transmiterea datelor către
robot - este butonul prezent în controlul robotului și în controlul manual
al platformei;
Butonul trimiteUnghi – trimite robotului referința pentru reglarea
automată a platformei; aici se realizează și codarea acestei referințe;
modul de codare va fi explicat în acest capitol;
Butonul mod – în funcție de starea acestuia, sunt selectate elementele
aferente modului de control automat sau manual al aplicație; tot aici se
setează o valoare pentru variabila modAutomat de tip boolean și se
trimite cuvântul cu valoare ”255” pentru modul automat și ”254” pentru
modul manual.
67
Metoda onSensorChanged() verifică dacă valorile senzorului
accelerometric se schimbă, obiectul sensor primește noile valori și apelează
metoda calculeaza(), care are ca parametrii valorile senzorului pentru axele
Ox și Oy.
Metoda calcuelaza(Float x, Float y) realizează codificarea valorilor
ce trebuiesc trimise către robot și apelează metoda de transmitere a acestora
prin bluetooth. Codificarea se realizează pe cuvinte de 8biți pentru o transmitere
rapidă a acestora. Dacă ecranul curent este cel de control al robotului, cuvîntul
codificat are următoarea formă:
0 Cod motor (1 bit) Bitul de semn (1 bit) Puterea motorului (5 biți)
Tabel 4.3.2.1 - Codificarea cuvântului transmis pentru controlul robotului
Primul bit 0 indică faptul că este vorba de un cuvânt de control al
robotului;
Cod motor poate lua valorile ”0” pentru motor stânga și ”1” pentru
motor dreapta;
Bitul de semn este ”1” pentru mersul înapoi și ”0” pentru mers
înainte;
Puterea motorului are o valoare între 0 și 31; această putere
reprezintă forma semnalului PWM care este evaluată în softul
Arduino de valori între 0 și 255; eu am ales transmiterea unei puteri
maxime de 200 pentru menajarea motoarelor și consumul redus de
energie, astfel valoarea transmisă reprezintă incremente de
aproximativ 6.5, valoare ce asigură un control suficient de sensibil;
Această valoare pentru puterea motoarelor este calculată în funcție de
valorile senzorului accelerometric. Astfel, semnul valorilor de pe axa Ox
determină sensul de deplasare al robotului: dacă telefonul se înclină în față,
valorile senzorului sunt negative și variabila sens ia valoarea 0. Robotul se va
deplasa în față prin setarea valorii 0 pentru bitul de semn înainte de trasnmiterea
cuvântului de control. Similar se procedează și pentru deplasarea în spate a
robotului.
În funcție de unghiul de înclinare, senzorul trimite valori între -9,5 și 9,5.
Pentru controlul facil al robotului valorile între -1.5 și 1.5 sunt considerate 0
pentru a nu determina mișcare robotului la înclinări ușoare ale telefonului, iar
intervalele de valori pentru control sunt (-7,-1.5) pentru mersul înainte și (1.5 ,
68
7) pentru mersul înapoi. Aceste valori se scaleaza la intervalul 0 - 31 de valori
naturale. Aceste valori nu sunt valorile finale transmise, pentru că fiecare motor
va primi valori diferite pentru deplasarea stanga/dreapta a robotului.
Sensul și valoarea senzorului pe axa Oy determină diferența de putere
dintre motorul din stânga și motorul din dreapta: dacă telefonul este înclinat spre
stânga, motorul drept are putere mai mare, deci robotul se deplasează spre
stânga. Similar și pentru deplasarea spre dreapta a robotului. Astfel valoarea
indicată de senzor pentru axa Oy determină procentual diferența de putere dintre
motoare.
Dacă ecranul curent este cel de control al platformei, cuvântul codificat are
următoarea formă:
1 Sensul de deplasare (1 bit) Unghiul de înclinare (6 biți)
Tabel 4.3.2.2 - Codificarea cuvântului de control manual al platformei
Bitul 1 indică faptul că este vorba de un cuvânt destinat controlului
platformei;
Sensul de deplasare al platformei este specificat prin următorul bit
care este ”1” pentru deplasarea în față și ”0” pentru deplasarea în
spate;
Unghiul de înclinare este o valoare pe 6 biți între 0 și 60 (nu 0 – 63)
pentru că valorile cuvintelor de 255 și 254 sunt păstrate pentru
indicarea modului de control al robotului.
Pentru calcularea unghiului de înclinare al paltformei se ia în considerare
doar valoarea senzorului de pe axa Ox. Scalarea acestor valori este identică cu
scalarea pentru controlul motoarelor, fără a lua în considerare valorile senzorului
pentru înclinarea pe axa Oy.
După calculul acestor cuvinte de control, este apelată metoda
updateView() care afișează valorile puterii motoarelor sau a unghiului de
înclinare al platformei transmis. Această metodă apelează metoda sendData()
pentru transmiterea cuvintelor prin bluetooth.
Metoda sendData() plasează aceste cuvinte într-o coadă de tip FIFO de
așteptare de unde sunt transmise automat, respectând criteriul ”first in, first
out”. Se verifică dacă transmisia a fost realizată și se transmite o eroare în cazul
eșuării transmisiei.
Alte două metode importante ale aplicației sunt onResume() și
onPause(). Acestea sunt apelate automat la blocarea și revenirea din starea
69
de blocare a telefonului. Astfel, când telefonul este blocat, se întrerupe orice
activitate, inclusiv conexiunea bluetooth dintre telefon și robot, iar atunci când se
deblochează telefonul, se realizează reconectarea dispozitivelor și reluarea
celorlalte taskuri.
Metodele onCreate(), onResume() și onPause() sunt suprascrise
peste metodele standard ale aplicaței Android.
Codul implementat pentru realizarea acestei aplicații este anexat pe CD-ul
de la sfârșitul lucrării.
4.4.Implementarea algoritmului de control pe Arduino
Pentru implementarea algoritmului de control pe platforma Arduino am
folosit softul Arduino 1.0.3. Acesta este special conceput pentru a ușura
conexiunea și controlul componentelor cu microcontrollerul Atmega2560.
Codul implementat pe Arduino cuprinde trei mari părți: includerea
librăriilor folosite și declararea variabilelor, funcția setup() care rulează o
singură dată la pornirea robotului și funcția loop() care rulează la infinit
precum o buclă while cu condiția mereu adevărată. În funcțiile setup() și
loop() se pot apela și alte funcții implementate de programator pentru o
dezvoltare facilă a programului.
Pentru achiziția datelor de la senzorul accelerometric și giroscopic, am
utilizat librăriile ”MPU6050.h”, ”I2Cdev.h” și ”Wire.h”, iar pentru controlul
servomotorului am utilizat librăria ”Servo.h”.
Principalele varibile folosite în programul dezvoltat sunt myservo care este
de tipul Servo și implementează un obiect pentru controlul servomotorului,
accelgyro care se tipul MPU6050 și se folosește la preluarea datelor senzorului
MPU6050, 6 variabile pentru preluarea datelor de la senzor, câteva variabile
folosite la prelucrarea datelor cu filtrul Kalman, doi vectori: u[] și y[] - pentru
implementarea regulatorului PI și câteva variabile folosite pentru diverse funcții.
Funcția setup() cuprinde comanda de început pentru comunicația
bluetooth, comanda de start pentru comunicația cu senzorul MPU6050,
inițializarea obiectului accelgyro, atașarea pinului 6 la obiectul Servo, inițializarea
poziției servomotorului la 0 grade, câteva comenzi pentru calibrarea senzorului
MPU6050, și setarea pinilor de control al motoarelor ca ieșiri.
70
Primul pas din funcția loop() este verificarea dacă există date primite
prin bluetooth. Dacă există, se preia cuvântul și se decodifică pentru a afla dacă
este adresat controlului robotului sau platformei sau dacă reprezintă un cuvânt
pentru activarea modului de control automat sau manual al platformei. Pentru
fiecare din aceste cazuri, se apelează funcțiile corespunzătoare prezentate în
continuare.
La sfarșitul oricărui ciclu loop() se apelează funcția reglareUnghi().
Această funcție verifică dacă este activat modul manual sau automat de control
al platformei. Dacă platforma se controlează automat, se calculează comanda
motorului folosind algoritmul PI și se transmite comanda către servomotor. Dacă
platoforma se controlează manual, se dă comanda corespunzătoare referinței
primite prin bluetooth către servomotor.
Pentru controlul automat se apelează funcția kalmanXAngle() care
returnează unghiul indicat de senzorul accelerometric pe axa longitudinală
robotului. Pentru calculul acestui unghi, se apelază accelgyto.getMotion6()
care are 6 parametrii în care se returnează valorile pe cele 3 axe oferite de
senzorul accelerometric și de senzorul giroscopic. În continuare, se apelează
metoda kalmanX.getAngle() pentru filtrarea valorilor senzorului MPU6050.
Această funcție conține și un timer care înregistrează timpul scurs între două
măsurători pentru acuratețea filtrării.
Cuvintele primite prin bluetooth pot fi adresate motoarelor de tracțiune ale
robotului, astfel se apelează funcția prelucreazăComandaMotoare() atunci
când se primesc aceste cuvinte. Prelucrarea presupune decodificarea cuvântului
pentru a afla sensul motorului și scalarea puterii motorului de la intervalul 0-31
la intervalul 0-200. Doar după ce se primește o pereche de cuvinte, unul adresat
motorului din stanga și unul adresat motorului din dreapta, se transmite
comanda către motoare. Transmisia se face aproape simultan și se realizează
prin funcțiile analogWrite(pin, valoare) și digitalWrite(pin,
valoare) care transmit fiecare câte un semnal, pentru perechea de pini
destinată fiecărui motor. Astfel, transmiterea semnalului PWM se face prin
scrierea valorii 0 (LOW) pe unul din pinii motorului și a unei valori între 0 și 200
pe celălalt pin. Pinii se aleg în funcție de sensul de acționare dorit al motorului.
Codul implementat pe platforma Arduino este anexat pe CD-ul de la
sfârșitul lucrării.
71
4.Concluzii
Am reușit construcția unui robot ce este controlat prin comunicația
wireless - bluetooth cu ajutorul unui dispozitiv mobil – un smartphone cu
sistemul de operare Android. Am studiat diverse arhitecturi de roboți și diverse
componente hardware (motoare, servomotoare, acumulatori) pentru
achiziționarea elementelor potrivite. Am tratat fiecare componentă în parte
explicând rolul ei, o descriere sumară a funcționării și de ce am ales modelul
respectiv. Am analizat metoda de reglare automată cu regulator PI și filtrarea
Kalman. Am folosit pachetul Android SDK în mediul de dezvoltare Eclipse pentru
implementarea unei interfețe grafice pe telefon. Am implementat un filtru
Kalman pentru filtrarea datelor oferite de senzorul accelerometric și de senzorul
giroscopic. Am realizat reglarea automată a platformei la un anumit unghi
folosind un algoritm PI. Am implementat algoritmul de control al robotului în
limbajul ”Arduino Programming”.
Exsită diverse arhitecturi de control al roboților prin bluetooth. Scopul meu
a fost să implementez și o interfață grafică pentru interacțiunea facilă dintre
utilizator și robot. Controlul acestuia este astfel intuitiv și plăcut. Un alt mare
avantaj este că dispozitivul de control este un smartphone. Mulți oameni folosesc
deja aceste telefoane, deci este eliminat costul achiziționării unui dispozitiv
suplimentar de control. Potrivit unui studiu realizat în anul 2012, unul din 5
români folosesc un smartphone. În unele țări există procentaje de până la 80%
de oameni care dețin un smartphone.
Un alt plus al robotului realizat, față de cei prezenți, este platforma mobilă,
ce poate fi poziționată la un anumit unghi. Se vor putea implementa astfel
diverse funcționalități robotului, în funcție de domeniul în care este folosit. Un
exemplu ar fi montarea unei camere video, a unui dispozitiv foto sau audio pe
această platformă pentru realizarea unui robot spion sau de cercetare. Acesta
poate fi folosit și în domeniul militar sau de apărare pentru inspectarea unei
anumite zone periculoase sau a unor pachete suspecte prin montarea unui braț
robotic pe platformă.
72
7.Bibliografie
[1] I. Dumitrache, “Ingineria Reglării Automate”, Editura Politehnica - București,
vol. 1, 2010
[2] G. Welch, G. Bishop, ”An Introduction to the Kalman Filter”, SIGGRAPH, 2001
[3] „Kalman Filter Design”, http://www.mathworks.com/help/control/ref/
kalman.html, accesat la data de 12.03.2013
[4] D. Simon, „Kalman Filtering”, http://www.innovatia.com/software/papers/
kalman.htm, accesat la data de 15.03.2013
[5] N.Skoro, „Bluetooth Robot Controller”, http://itech.fgcu.edu/faculty/zalewski/
projects/files/BluetoothRobotControllerNov2006.pdf, accesat la data de
25.01.2013
[6] J. Fedorynski, „NXT Remote Control”, http://www.appbrain.com/app/nxt-
remote-control/org.jfedor.nxtremotecontrol, accesat la data de 30.01.2013
[7] D. Sawicz, „Hobby Servo Fundamentals”, http://www.princeton.edu
/~mae412/TEXT/NTRAK2002/292-302.pdf, accesat la data de 15.05.2013
[8] „Drive Motor Sizing Tutorial", RobotShop, iunie 2008
[9] „Understanding RC LiPo Batteries”, http://www.rchelicopterfun.com/rc-lipo-
batteries.html, accesat la data de 02.04.2013
[10] „Arduino Mega”, http://arduino.cc/en/Main/arduinoBoardMega, accesat la
data de 25.02.2013
[11] „Bluetooth Wireless Technology”, http://www.bluetooth.com/Pages/
Basics.aspx, accesat la data de 05.04.2013
[12] I. Poole, „Bluetooth Technology”, http://www.radio-
electronics.com/info/wireless/bluetooth/bluetooth_overview.php, accesat la data
de 05.04.2013
[13] F. Ableson, „Develop Android applications with Eclipse”,
http://www.ibm.com/developerworks/opensource/tutorials/os-eclipse-android/,
accesat la data de 30.05.2013
[14] K. Lauszus, „A practical approach to Kalman filter”,
http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-
how-to-implement-it/, accesat la data de 02.06.2013
73
[15]Micro Metal Gearmotor, http://www.pololu.com/catalog/product/992,
accesat la data de 12.03.2013
[16] ROB-09065 Servo-Small, http://www.abra-
electronics.com/products/ROB%252d09065-Servo-%252d-Small.html, accesat la
data de 12.03.2013
[17] L293D datasheet, http://www.abra-
electronics.com/products/ROB%252d09065-Servo-%252d-Small.html, accesat la
data de 19.03.2013
[18]Zippy Flightmax LiPo,
http://www.hobbyking.com/hobbyking/store/__6474__zippy_flightmax_1000ma
h_2s1p_20c.html, accesat la data de 20.03.2013
[19] LM7806 datasheet, https://www.sparkfun.com/datasheets/
Components/LM7805.pdf, accesat la data de 19.03.2013
[20]MPU6050 Accelerometer+Gyro, http://playground.arduino.cc/Main/MPU-
6050, accesat la data de 23.03.2013
[21] Bluetooth Mate Silver, RN-42, https://www.sparkfun.com/products/10393,
accesat la data de 23.03.2013
[22]W.J. Francis, „A quick tutorial on coding Android’s accelerometer”,
http://www.techrepublic.com/blog/app-builder/a-quick-tutorial-on-coding-
androids-accelerometer/472, accesat la data de 28.04.2013
[23] A.V. Koltykov, „Data transfer between Android and Arduino via Bluetooth”,
http://www.techrepublic.com/blog/app-builder/a-quick-tutorial-on-coding-
androids-accelerometer/472, accesat la data de 29.04.2013
[24] J. Rowberg, MPU6050 code example,
https://github.com/jrowberg/i2cdevlib/ tree/master/Arduino/MPU6050, accesat
la data de 03.05.2013
[25] I2Cdevlib library,
http://www.i2cdevlib.com/docs/html/class_m_p_u6050.html, accesat la data de
03.05.2013
[26] Samsung Galaxy Gio, http://firm-guide.com/company/samsung-
electronics/samsung-mobile-phones/samsung-galaxy-gio/samsung-galaxy-gio-
picture-2/, accesat la data de 7.06.2013