Diploma Bilaniuc V Cristy 2020 [611625]

UNIVERSITATEA DIN ORADEA
FACULTATEA DE INGINERIE ELECTRICǍ ȘI
TEHNOLOGIA INFORMAȚIEI
DOMENIUL / PROGRAMUL DE STUDIU
INGINERIA SIS TEMELOR/AUTOMATICĂ ȘI
INFORMATICĂ APLICATĂ
FORMA DE ÎNVĂȚĂMÂNT ZI

PROIECT DE DIPLOMĂ

COORDONA TOR ȘTIINȚIFIC
Conf. Univ. Dr. Ing. BARABAȘ TIBERIU

ABSOLVENT: [anonimizat]
2020

UNIVERSITATEA DIN ORADEA
FACULTATEA DE INGINERIE ELECTRICǍ ȘI
TEHNOLOGIA INFORMAȚIEI
DOMENIUL / PROGRAMUL DE STUDIU
INGINERIA SIS TEMELOR/ AUTOMATICĂ ȘI
INFORMATICĂ APLICA TĂ
FORMA DE ÎNVĂȚĂMÂNT ZI

COMANDA ROBOT ULUI
MOBIL CU
AUTOECHILIBRARE

CONDUCĂTOR ȘTIINȚIFIC
Conf. Univ. Dr. Ing. BARABAȘ TIBERIU

ABSOLVENT: [anonimizat]
2020

1 Cuprins

INTRODUCERE ………………………….. ………………………….. ………………………….. ………………………….. ………… 2
CAPITOLUL 1. ROBOȚI MOBILI ………………………….. ………………………….. ………………………….. ………….. 3
1.1 CLASIFICAREA ROBOȚILOR ………………………….. ………………………….. ………………………….. …………………. 4
1.2 TIPURI DE ROBOȚI ………………………….. ………………………….. ………………………….. ………………………….. … 5
1.3 STRUCTURA ROBOȚILOR MOBILI ………………………….. ………………………….. ………………………….. ………… 6
1.4.A CȚIONAREA ROBOȚILOR MOBILI ………………………….. ………………………….. ………………………….. ……….. 7
1.4.1. Motorul de curent continuu ………………………….. ………………………….. ………………………….. …….. 7
1.4.2. Elementele motorului de curent continuu ………………………….. ………………………….. ………………. 8
1.4.3. Regimul de funcționare al motorului de curent continuu ………………………….. ……………………….. 9
1.5. PRINCIPIILE ROBOTULUI CU AUTOECHI LIBRARE ………………………….. ………………………….. …………………. 11
1.6. COMPONENTELE FUNCȚION ALE ALE ROBOTULUI CU AUTOECHILIBRARE ………………………….. ……………… 12
1.6.1 Senzorul unghiului de înclina re………………………….. ………………………….. ………………………….. ..12
1.6.2. Filtrarea datelor achiziționate ………………………….. ………………………….. ………………………….. ….13
1.7. COMANDA MOTOARELOR ………………………….. ………………………….. ………………………….. …………………. 18
1.8. DOMENII DE UTILIZARE A ROBOȚILOR ………………………….. ………………………….. ………………………….. ….18
CAPITOLUL 2. PREZENT AREA ROBOTULUI CU AUTOECH ILIBRARE ………………………….. ………. 19
2.1. STRUCTURA ROBOTULUI ………………………….. ………………………….. ………………………….. ………………….. 19
2.2. SCHEMA DE COMANDĂ A R OBOTULUI ………………………….. ………………………….. ………………………….. …..20
CAPITOLUL 2.3 DRIVERUL DE MOTOR L298N ………………………….. ………………………….. …………………………. 21
CAPITOLUL 2.4. SENZORUL DE ÎNCLINARE MPU6050 ………………………….. ………………………….. ……………….. 24
CAPITOLUL 3. SOFTWAR E-UL ARDUINO NANO ………………………….. ………………………….. …………….. 29
3.1 MICROCONTROLERE ………………………….. ………………………….. ………………………….. …………………………. 29
3.1.1. Structura unui Microcontroler ………………………….. ………………………….. ………………………….. …30
3.1.1.1. Unitatea de memorie ………………………….. ………………………….. ………………………….. ……… 30
3.1.1.2. Unitatea centrala de procesare ………………………….. ………………………….. ……………………… 30
3.1.1.3. Bus -ul ………………………….. ………………………….. ………………………….. …………………………. 31
3.1.1.4. Unitatea de intrare -ieșire (I/O) ………………………….. ………………………….. ……………………… 32
3.2 PLACA DE DEZVOLTARE ARDUINO NANO R3 ………………………….. ………………………….. ……………………… 33
3.3. ALIMENTAREA PLATFORME I ARDUINO ………………………….. ………………………….. ………………………….. …34
3.4 DESCRIEREA PINILOR PL ĂCII DE DEZVOLTAREA ARDUINO NANO R3 ………………………….. ……………………. 36
3.5 SEMNALE PWM ………………………….. ………………………….. ………………………….. ………………………….. …..36
CAPITOLUL 4. MEDIUL DE PROGRAMARE ARDUIN O ………………………….. ………………………….. ……39
4.1. PREZENTAREA MEDIULUI DE PROGRAMARE ………………………….. ………………………….. ………………………. 39
4.2 INSTALAREA MEDIULUI D E PROGRAMARE ARDUINO ȘI RULAREA UN UI PROGRAM ………………………….. ……40
4.3 INSTRUCȚIUNI UTILIZAT E PENTRU PROGRAMAREA ROBOTULUI ………………………….. ………………………….. .40
CAPITOLUL 5. IMPLEME NTAREA PROGRAMELOR P ENTRU ROBOTUL MOBIL …………………… 42
5.1. IMPLEMENTAREA PROGRAM ULUI PENTRU PLATFORM A ARDUINO NANO ………………………….. ……………… 42
CONCLUZIE ………………………….. ………………………….. ………………………….. ………………………….. ……………. 47
BIBLIOGRAFIE ………………………….. ………………………….. ………………………….. ………………………….. ……….. 48

2 Introducere

Robotica este un domeniu foarte important pentru omenire, ea ne controlează modul
de viață aproape zilnic, iar majoritatea oamenilor care o folosesc și depind de tehnologie
depind de ea.
Robotica sa dezvoltat foarte mult în ultimii ani, această dezvoltare a adus la
numeroase tipuri de roboți mobili care au dive rse variații funcționale și constructive. Roboții
fiind din ce în ce mai mulți și implementați în diferite activități din cadrul construcțiilor,
industriei, medicinei , agriculturii, transporturilor și în divertisment.
Roboții sau dezvoltat și prin crește rea componentelor echipate cu inteligență
artificială, pentru a culege informațiile un robot are implementat senzori tactili, de forță, de
moment , video , de poziție. Cu ajutorul acestor componente robotul este capabil de ași crea o
imagine a mediului în care se află .

Prin această lucrare s -a încercat elaborarea unui robot mobil cu autoechilibrare și
readactarea unui cod pentru punerea în funcțiune a robotului.

Prima componentă absolut necesară care a fost aleasă este un microcontroler 328P
care este incorporat pe placa de dezvoltare Arduino Nano R3, datorită faptului că prin
simplitatea sa programul pent ru robot poate fi redacta cu ușurință.

A doua componentă necesare este un senzor de înclinare MPU6050, cu ajutorul
acestui senzor vom putea afla poz iția robotului și să o transmitem către placa de dezvoltare.

Și ultima componentă majoră necesară pentru robot este un driver motor L298N,
necesar pentru comanda celor două motoare de curent continuu și pentru protecția
microcontrolerului, placa de dez voltare nefiind capabilă să scoată suficientă putere pe
porturile sale pentru învățarea motorului, prin conectarea motoarelor la placa de dezvoltare
există riscuri prea mari de a arde procesorul plăcii.

Robotul fiind dotat și cu două motoare și o aliment are de la baterii pentru alimentarea
tuturor componentelor.

3 Capitolul 1 . Roboți mobili

Figură 1.1 Sistemul Elmer si Elsie [2]

Robotica este o ramură a tehnologiei care se ocupă cu studiul, dezvoltarea, creare a și
operarea roboților și tot o dată cu procesarea informațiilor, controlul și studiul senzorial al
acestora.
Robotica a fost văzuta deseori ca în filme ca fiind o mimică a comportamentului uman
care ar putea îndeplini aceleași sarcini ca si omul.
Robot ul este un dispozitiv mecanic acționat cu forțe motrice cu comandă inteligentă și
care acționează conform voinței umane. [1]
Roboții sunt folosiți în producție, ambalare, transport, exploatarea Pământului și al
spațiului, în dezvoltarea armamentului, manip ularea și neutralizarea munițiilor neexplodate,
executarea unor culoare prin câmpurile de mine, cercetarea autovehiculelor, trenurilor,
aeronavelor și a clădirilor, urmată de neutralizarea dispozitivelor explozive descoperite în
aceste vehicule, în dezvol tarea măsurilor de siguranță sau în producția în masă a bunurilor
industriale.
Termenul de robot provine din limba Cehă , robota care înseamnă muncă. Tot odată
„robot ” , prescurtat bot , precum sunt numite programele de calculator care rulează seturi de
instrucțiunii ne arată că roboti sunt mecanici , virtuali și artificiali.
Institutul Francez de Standardizare definește robotul ca fiind un manipulator automat
reprogramabil și polivalent, capabil să realizeze poziționarea și reprintarea pieselor printr -o
mișcare variabilă și programabilă a brațelor terminale, prin intermediul unor dispozitive.
Institutul American de Robotică consideră robotul ca fiind un operator reprogramabil
și multifuncțional pentru deplasarea obiectelor pe traiectorii stabilite anterior în realizarea
unor sarcini concrete.
In literatura românească de specialitate robotul este definit ca fiind un echipament
automat, adaptabil prin reprogramare la condițiile de mediu în care acționează.
Dezvoltarea tehnologiilor necesare pentru obținerea un or roboți mobili care să ajute
sau să înlocuiască diferite operații realizate de om implică multe domenii ca cele ale
senzorilor, inteligenței artificiale, sistemelor de calcul, planificării traiectoriei, procesării
semnalelor, controlului motoarelor, elec tronicii și științei calculatoarelor.[3]

4 Sunt aspecte importante care trebuie luate în considerare atunci când se operează cu un
robot mobil și anume: orientarea într -un mediu necunoscut, folosind senzori pentru detectarea
obstacolelor și comunicația cu un calculator de la distanță.
Robotul mobil este un sistem complex care poate efectua diferite activități într -o
varietate de situații specifice. El este o combinație de dispozitive echipate cu servomotoare și
senzori care operează într -un spațiu real, marc at de o serie de proprietăți fizice (de exemplu
gravitația care influențează mișcarea tuturor roboților care funcționează pe pământ) și care
trebuie să planifice mișcările astfel încât robotul să poată realiza o sarcină în funcție de starea
inițială a sist emului și în funcție de informația existentă, legată de mediul de lucru.

1.1 Clasificarea Roboților

Structura generală a roboților depinde foarte mult de utilitatea și scopul pentru care
sunt produși. Roboții se pot clasifica din mai multe puncte de v edere, si anume:

Din punctul de vedere al gradului de mobilitate :

 roboți ficși: cei care sunt imobili față de anumite componente ale mediului în care
evoluează.
 roboți mobili: sunt comandați de la distanță (prin unde radio sau prin cablu), sistemele
de acționare ale brațului manipulator fiind de tip elector -hidraulic sau electric, cu
motoare hidraulice liniare sau motoare electrice rotative. Ei se clasifică la rândul lor:
o în funcție de dimensiuni: macro, micro și nano -roboți[4]
o în funcție de mediul în care acționează:
 roboți tereș tri.
 roboți subacvatici .
 roboți zburători .
 roboți spațiali .
o în funcție de sistemul care le permite deplasarea în mediul în care acționează:
 roboți pe roți sau șenile .
 roboți pășitori .
 roboți târâtori .
 roboți săritori .
 roboți de formă sferică .
o După domeniul de aplicare:
 sector primar (agricultură, minerit) .
 sector secundar producție materială.
 sector terțiar (medicină, domeniul nuclear) .
 sectoare speciale (explorări spațiale, subacvatice) .
o Din punct de vedere al sistemului de coordonare
 robotul cartezian este cel care ope rează într -un spațiu definit de
coordonat e carteziene .
 robotul sferic (polar) are spațiul de lucr u definit de coordonate sferice.
 roboți industria li în alte tipuri de coordonate .
 robotul protetic are un braț a rticulat .
o Din punct de vedere al sistemului de comandă:
 robot cu comandă punct cu punct unde nu inter esează traiectoria
propriu -zisă.
 robot cu comandă pe traiectorie continuă: imp lică toți parametrii de
mișcare .
 robot cu comandă multipunct: impl ică coordon area mișcării axelor .

5 o Din punct de vedere al sistemului de acționare:
 robot cu acționare hidraulică .
 robot cu acționare electrică: se poate asigura o funcționare auto nomă
prin alimentare cu baterii .[5]

1.2 Tipuri de Roboți

Robotul industrial este un sist em electro -hidro -pneumatic care are mai multe grade de
libertate și este capabil să execute operați și secvențe de operați de manipulare a unor scule ,
piese sau subansamble. Aceștia fiind controlați de un sistem de comandă cu o memorie
programabilă.[8]

Figura 1.2.1 Roboți Industriali

Robot Umanoid poate fi descris ca și un robot cu un aspect asemănător omului,
aceștia au următoarele caracteristici :

 Se pot de plasa pe un teren accidentat.
 Mobilitatea lor este restrânsă la cele două picioare.
 Dețin un consum sc ăzut de energie.

Figura 1.2.2 Robot Umanoid

6 Roboti Exploratori sunt folosiți în locuri greu accesibile sau periculoase teleghidați
sau parțial autonomi. Aceștia pot lucra de exemplu intr -o regiune aflată în conflict militar, pe
alte pl anete, în adâncurile oceanelor.[4]

Figura 1.2 .3 Robot Explorator [6]

1.3 Structura Roboților Mobili

Structura unui robot mobil este alcătuită din următoarele sisteme :

 Sistemul de comandă: asigură coor donarea mișcărilor întregului robot emițând
comenzi către sistemul de acționare și prelucrează informațiile preluate de la
sistemul de acționare , mecanic și de la mediu.
 Sistemul de acționare : realizează mișcarea elementelor sistemului mecanic și poate
fi de trei feluri : electrică, hidraulică și pneumatică.
 Sistemul mecanic : are rolul să asigure realizarea mișcărilor acestuia și transmiterea
energiei mecanice necesare interacțiunii cu mediul, adică are sarcina de a deplasa
un obiect .
 Sistemul senzorial (intern și extern) : transmite date de la senzori către sistemul de
comandă.[7]

Structura sistemului mecanic al roboților este alcătuită din:

 Sistemul de locomoție: prin care se asigură deplasarea robotului pe diferite
suprafețe, aceasta poate fi alcătuită din șenile sau roți.
 Sistemul de manipulare: prin care s e asigură poziționarea și orientarea organului
de lucru .
Robotul mobil în procesul de deplasare pe o anumită traiectorie este caracterizat prin
funcții:

 Funcția de locomoție.
 Funcția de percepție -decizie.
 Funcția de localizar e.

Funcția de locomoție cuprinde sistemul de acționare electric și sistemul de sprijinire
(suspensie). Modalitățile de propulsare sunt dintre cele mai diverse, cum ar fi: pe roți, cu jet
de apă, cu aer, etc.
Roboții mobili pot fi echipați cu cameră video sau alți senzori de percepere al
mediului în care activează. Memoria robotului conținută în microcontroler înmagazinează
cunoștințele necesare localizării tuturor segmentelor de traseu posibile . [9]

7 1.4.Acționarea Roboților Mobili

Acționarea roboților mobili necesita în general motoare electrice de putere mică, cu
capacitate de supra sarcină, precum si reductoare cu raport mare și moment de inerție redus de
tip armonic sau procesional.[7[

Din categoria motoarelor electrice cu ine rție redusă se pot utiliza:

 moto are de curent continuu cu indus.
 motoare sincrone cu magneți permanenți.
 motoare pas cu pas cu reluctanță variabilă.

1.4.1. Motorul de curent continuu

Motorul de curent continuu este foarte răspândit în întreaga lume, în majoritatea
sistemelor de acționare electronice, datorita caracteristicilor electrice avantajoase.

Figură 1.4. 1.1 Motorul de curent continuu

Principiul de funcționare : În momentul în care curentul trece prin rotorul unui motor
de curent continuu, se generează un câmp electro magnetic cu o forța electromagnetica care
realizează rotația rotorului. In momentul in care motorul realizează rotațiile mai rapid,
tensiunea rezultata este aproape egala cu cea indusă.
Dacă asupra motorului nu este acționată ni ci o sarcină acesta își va menține viteza iar
în cazul in care acestuia se va aplica o sarcină, motorul va absorbi un curent mai mare pentru
realizarea lucrului mecanic iar tensiunea va fi redusă.
Fiindcă viteza rotației controlează trecerea curentului p rin rotor, dacă acesta se afla in
repaus el nu deține nici o rezistență iar dacă tensiunea normală este aplicata prin acesta va
trece un curent prea mare care ar putea defecta periile colectoare sau motorul in sine, pentru
evitarea problemelor se folosesc dispozitive speciale pentru pornirea motoarelor de curent
continuu.
In majoritatea cazurilor pentru prevenirea acestor accidente sunt utilizate rezistente în
serie, la început pentru limitarea curentului până în momentul în care motorul reușește să
dezvol te un curent suficient. Rezistenta este redusă treptat automat sau manual din momentul
în care motorul își mărește viteza.
Puterea câmpului electro magnetic care acționează asupra rotorului și curentul
rotorului influențează viteza acestuia. Cu cat câmpul magnetic este mai puternic , cu atâta rata
rotației necesare pentru crearea unui curent secundar necesar devine mai mica și tot odată
curentul secundar necesar combate curentul aplicat.[10]
Motoarele de curent continuu sunt de 4 feluri:

a) Motoare de curent continuu cu excitație derivație
b)Motoare de curent continuu cu excitație paralel
c)Motoare de curent continuu cu excitație serie

8 d)Motoare de curent continuu cu excitație mixta

a. Motoarele cu excitație derivație : acestea sunt folosite pentru acționarea mașinilor
unelte grele , alimentarea acestora realizată printr -o sursă de tensiune constantă și având doar
înfășurarea de excitație la tensiune constanta , iar rotorul alimentat de la o sursa de tensiune
alternativă.
b. Motoarele cu excitație s erie: acestea fiind utilizate in tracțiunea electrica, in
domeniul rutier pentru demararea motoarelor autovehiculelor, tramvaielor și la diferite tipuri
de macarale. Acestea au cuplu mare la pornire, suporta mai bine suprasarcinile si nu sunt atât
de sensi bile asupra căderilor de tensiune.
c. Motoarele cu excitație mixta : sunt utilizate în acționările mecanismelor cu regim
variabil. Caracteristicile acestora fiind intermediare cu cele a motoarelor derivație si serie.

Motoarele de curent continuu sunt re versibile, adică ele pot funcționa ca si generatoare
sau ca si motoare după felul în care sunt folosite sau folosesc mecanisme.

Figură 1.4.1.2. Mod de funcționare a motorului de curent continuu

1.4.2. Elementele motorului de curent continuu

Motorul de curent continuu este alcătuit din 2 părți constructive de baza:

 Statorul – este partea fixa a motorului cu elementele:
o Carcasă
o Polii de excitație si înfășurarea de curent continuu de excitație
o Polii de comutație cu înfășurarea de curent continuu
o Sistem ul de perii
o Cutia de borne
 Rotorul – este partea mobila a motorului cu elementele:
o Miezul rotoric
o înfășurarea rotorică
o Colectorul
o Ventilatorul

Figură 1.4.2.1 Elementele motorului de curent continuu

9 Carcasa este partea fixă care fixează polii de exc itație si este alcătuită din fonta la
mașinile de putere mare .
Polii de excitație sunt alcătuită din hotel electrotehnic , aceștia poarta bobinele de
excitație care sunt realizate dintr -un conductor de cupru izolat. Bobinele polilor sunt legate in
serie sau in paralel si sunt alimentate prin bornele din cutia de borne.
Polii de comutație sunt alcătuiți dintr -un miez din tole de OL ETH si din bobina
înfășurată pe miez alcătuita din cupru si străbătuta de curent continuu.
Miezul rotoric este alcătuit din tole de OL electrotehnic în forma circulara, cu
crestături și dinți, izolate intre ele cu un strat de lac.[11]
înfășurarea rotorică este alcătuita din spire de forma speciala așezate in crestăturile
miezului rotoric, înfășurarea fiind izolata de miez, iar spirele înfășurării rotorice se leagă la
colector
Colectorul are o forma cilindrica , fiind alcătuit din plăcuțe de cupru izolate una fata
de cealaltă , acesta se învârte solidar cu rotorul mașinii

1.4.3. Regimul de funcționare al motorului de curent co ntinuu

Motorul de curent continuu poate funcționa in regim de motor electric. Motorul
electric transforma puterea electrică primita de la o sursa în putere mecanică prin intermediul
câmpului electromagnetic.

Figură 1.4.3.1 Motorul de curent continuu

In regimul de motor cuplul electromagnetic schimbă se mnul și acționează asupra
rotorului în sensul de rotație. În regimul de motor sensurile de referință ale tensiunii la borne
și ale curentului se asociază după regula de la receptoare.
După modul de con ectare sa înfășurării de excitație sunt:

 Motoare cu excitație separata
 Motoare cu excitație derivație
 Motoare cu excitație serie
 Motoare cu excitație mixtă

În momentul in care alimentarea motorului se face de la o sursa de tensiune costata nu
exista diferențe între motorul cu excitație separată si motorul cu excitație derivație.
Polaritatea polilor auxiliari se realizează cu aceleași reguli ca si la generator,
rezultând ca parcurgând periferia rotorului in sensul de rotație , se întâlnește un pol au xiliar de
polaritate opusa fata de polul de excitație care urmează.
Motoarele de curent continuu sunt foarte importante în acționările cu reglaj de viteză,
cunoscând în prezent o largă dezvoltare, ca fabricație si utilizare. [12]

10 1.4.4. Comanda motoarelor de curent continuu

Pentru comandarea unui motor de curent continuu în majoritatea cazurilor suntem
obligați să inversăm polaritatea motorului. De preferat ar fi să se poată varia viteza motorului,
una dintre soluții pentru aceste două probleme este punt ea H ( H bridge).
O punte H este un circuit electronic care permite aplicarea unei tensiuni pe o sarcina in
orice sens. Aceste circuite sunt folosite în robotică si în alte aplicații pentru a permite
motoarelor de curent continuu sa funcționeze în ambele sensuri(înainte,înapoi). Punțile H sunt
circuite integrate ori pot fi construite din componente discrete, tranzistoare bipolare sau MOS.
Puntea H are numele derivat de la modul de desenare a circuitului.

Figură 1.4.4.1 Punte H

Modul de funcționare : În momentul in care întrerupătoarele S1 și S4 sunt închise iar
întrerupătoarele S2 și S3 sunt deschise o tensiune pozitivă va fi aplicată la nivelul motorului.
Prin deschiderea întrerupătoarelor S1 și S4 și închiderea întrerupătoarelor S2 și S3
tensiunea este inversată, iar acest lucru permite funcționarea inversă a motorului.
În momentul în care întrerupătoarele S1 și S2 sunt închise este provocat un scurt –
ciuruit la sursa de tensiune, același lucru se întâmpla și în cazul închiderii întrerupătoarelor S3
și S4.

Figură 1.4.4.2. Funcționarea punții H
a) Circuit închis pentru rotația în sensul acelor de ceasornic
b) Circuit închis pentru rotația în sensul opus acelor de ceasornic

11

Figură 1.4.4.3 Circuitul activ al punții H în funcție de sensul de rotaț ie [13]
a) Circuit închis pentru rotația în sensul acelor de ceasornic
b) Circuit închis pentru rotația în sensul opus acelor de ceasornic

1.5. Principiile Robotului cu autoechilibrare

Robotul cu autoechilibrare pe două roți este alcătuit dintr -un șasiu al robotului care
susține restul componentelor, două roti pentru deplasare. Acesta are capacitatea de a menține
o poziție verticală de echilibru. Aceste este unic în comparație cu alți roboti cu senile sau mai
multe roti datorită acestei caracteristici de a menține echilibrul. Are și capacitatea de rotire
intr-un punct fix care îi mărește manevrabilitatea și este ideal pentru a fi folosit în spații
reduse precum si în aplicații pentru transport. Stabilitatea este realizată prin menținerea celor
doua roți s ub centrul de greutate al robotului.
Acest robot are ca principiu sistemul pendulului invers care este instabil în mod
natural și reprezintă o problema de control. Dezvoltarea si punerea în funcțiune a unui sistem
de echilibrare adecvat care răspunde repede și este robust poate fi creat prin abordări liniare
ori prin abordări non-liniare.
Pentru dezvoltarea unui sistem de control capabil pentru echilibrarea unui robot pe
două roți este necesară o înțelegere a parametrilor din cadrul sistemului. Repreze ntarea
acesteia poate fi realizată printr -un model matematic.

Figură 1.5.1 Principiile robotului cu autoechilibrare [15]

Scopul principiului pendulului invers este de a menține roțile sub centrul de greutate al
robotului de aceea ambele roti vor fi i ncluse împreuna și combinate sub termenul roți, pentru
a simplifica calculele.

12 Ambele roți vor lucra la unison pentru menținerea stabilității , pentru a determina
cerințele specifice de cuplu pentru fiecare roa ta individual, valoarea roților poate fi re dusă la
jumătate pentru o valoare aproximativa pe o singură roata. Abordarea aceasta este considerata
acceptabila datorita faptului că terenul și suprafața pot varia.
În momentul în care robotul începe să se încline înainte , pentru menținerea stabilități i
roțile va trebuii să se deplaseze înainte pentru revenirea acestora sub masa șasiului. Dacă
condiția nu este menținută robotul va cădea.
Stabilitatea robotului cu autoechilibrarea este alcătuită din abilitatea de a menține
șasiul robotului intr -o poziți e dreapta si de echilibru. Aceasta echilibrare tre buie realizata în
mod automat fără intervenția umană și este cunoscută ca si stabilitate autonomă deoarece
robotul ia propriile decizii legate de acțiunile sale.
Este necesara implementarea unui sistem de control eficient și capabil să răspundă la
intrările senzorilor intr -un timp foarte scurt astfel încât stabilitatea să fie atinsă și
menținută.[14]

1.6. Componentele funcționale ale robotului cu autoechilibrare

In mare parte componentele unui robot cu autoechilibrare ar fi următoarele:

 Șasiul robotului
 Motoare electrice cu cuplu mare
 Senzor de înclinație
 Accelerometru
 O placa de dezvoltare
 Sursă de alimentare [16]

1.6.1 Senzorul unghiului de înclinare

Senzorul unghiului de înclinare este cea mai importantă componentă a robotului cu
autoechilibrare deoarece el transmite măsurători ale unghiului de înclinare aceste măsurători
trebuie sa fie transmise rapid și să fie precise pentru o stabilitate ridicată a robotului.
În caz contrar robotul nu va fi capabil să se mențină în echilibru în momentul în care
apar schimbări bruște ale unghiului de înclinare.[17]
Două componente esențiale ar fi:

 Accelerometrul
 Giroscopul

Accelerometrul este un senzor care măsoară accelerația, pe baza inerției corpurilor și
poate efectua măsurători în oricare dintre cele 3 planuri(x,y,z).
Ele mai sunt numite si gradiometre de gravitație, pentru că măsoară pante în câmpul
gravitațional. Accelerometrele pot fi utilizate și pentru detectarea unor posibile valuri
gravitaționa le.
În momentul în care un accelerometru sesizează o accelerație masa este deplasată pana
la punctul în care resortul este capabil de a accelera masa în același ritm ca și carcasa.
Deplasarea de apoi măsurată pentru a genera valoarea accelerației .[18]

13

Figură 1.6.1.1 Axele Accelerometrului [19]

Giroscopul este un dispozitiv folosit pentru măsurarea sau menținerea orientării și a
vitezei unghiulare. Acesta fiind o roată sau un disc în care axa de rotație este liberă de a se
orienta în orice parte.
Din c auza conservării momentului unghiular la rotire orientarea acestei axe nu este
afectată deloc de înclinarea sau rotirea montării în care este amplasat.
Există , giroscoape bazate pe alte principii de funcționare , precum dispozitivele
electronice giroscop MEMS care utilizează , inele cu laser, giroscoape cu fibră optică și
giroscoape cuantice care sunt extrem de sensibile.
Giroscopul este utilizat în sistemele de navigație inerțiale, cum ar fi în telescopul
Hubble, sau în interiorul carneiei de oțel a unui submarin subacvatic. De asemenea datorită
preciziei acestora ele sunt folosite si la minerit pentru a menține direcția tunelului.
Pot înlocui busolele magnetice (la aeronave, nave spațiale), sau pentru a ajuta la
stabilitatea bicicletelor, motocicletelor , ori utilizate ca parte a unui sistem de orientare.

Figură 1,6,1,2 Giroscopul [20]
1.6.2. Filtrarea datelor achiziționate

Filtrarea datelor achiziționare în cazul robotului cu autoechilibrare poate fi realizată cu
ajutorul a două filtre: [21]

1) Filtru l complementar
2) Filtrul Kalman

14

1) Fi ltrul C omplementar

Acest filtru acționează prin efectuarea unui filtraj cu o pondere mică pentru datele
măsurate de către accelerometru, un filtraj cu pondere m ai mare pentru datele achizițio nate de
giroscop care cu prind toate calculele de mediere a valorilor de la cei doi senzori.
Metoda această are rezultate bune și oferă un reglaj bun, dar durează mult timp din
cauza medierii, fiind lent acesta produce întârzieri .

Figură 1.6.2.1 Filtru Complementar [22]

Datorită neglijări giroscopului se pot provoca mari compensații ale referinței în
situația în care tendința giroscopului se modifică.[23]

2) Filtrul Kalman

Filtrul Kalman este un algoritm care folosește o serie de măsurători observate în timp,
aceste măsu rători conțin zgomot sau alte inexactități și produce estimări a variabilelor
necunoscute, care sunt mai precise decât cele bazate pe o singură măsurare.
Acest filtru funcționează recursiv pe fluxuri de date de intrare care conțin zgomot
pentru a putea pr oduce o estimare optimă a statisticii stării de bază a sistemului.
Filtrul Kalman este numit după Rudolf E. Kalman, el fiind unul dintre dezvoltatorii
principali ai teoriei sale.
Filtrul are numeroase aplicații în tehnologie, cele mai folosite ar fi în o rientarea
navigației si a controlului vehiculelor, în mode special la aeronave, nave spațiale .
Fiind un concept aplicat pe scară largă în analiza seriilor de timp utilizate precum și
pentru procesarea semnalului. Acest filtru este principalul subiect din domeniul planificării si
a controlului mișcării roboților, acestea fiind uneori incluse în optimizarea traiectoriei
roboților . El funcționând și pentru modelarea sistemului de control al mișcării .
Datorită unor întârzieri dintre emiterea comenzii motorulu i si primirea feedback -ului
senzorial , utilizarea filtrului acceptă un model pentru estimarea stării actuale a sistemului
motor și emite comenzi actualizate.
Algoritmul funcționează într-un proces alcătuit din două etape. Prima etapă fiind
predicția, în această etapă filtrul produce estimări ale variabilelor de stare actuale. După
aflarea rezultatului următoarei măsurători care vine cu o anumită cantitate de eroare și un
zgomot aleatoriu, aceste estimări sunt actualizate folosind o medie ponderată, cu o p ondere
mai mare acordată estimărilor .
Algoritmul este recursiv, aceste poate ru la și în timp real , folosind doar măsurătorile
de intrare și starea calculată anterior și matricea sa de incertitudine, nefiind necesare alte
informații suplimentare.

15

Figu ră 1.6.2.2 Filtrul Kalman [24]

Ipoteza principală a filtrului Kalman este aceea că sistemul de bază este dinamic liniar
și că toate condițiile de eroare au o distribuție Gaussiană.
Filtrul si expresia sa generală au fost dezvoltate de -a lungul timpului în filtre Kalman
extinse și filtre Kalman modificate să funcționeze pe sisteme nelineare. Toate calculele și
măsurătorile bazate pe niș te estimări cu un anumit grad.
Aproximările în ecuații care descriu modul în care se schimbă sistemul, datele
zgomotoas e precum și factorii externi nu sunt luați în considerare pentru a induce o anumită
incertitudine pentru valorile deduse despre starea unui sistem .
Filtrul Kalman oferă o predicție a stării unui sistem cu o nouă măsurare folosind o
medie ponderată. Scopul ponderilor este de a fi considerate mai sigure valorile cu
incertitudine mai mică. O nouă estimare a stării este rezultatul ponderat, care se află starea
precisă si măsurată, aceasta având o incertitudine mai bună decât cele două separat.
Deoarece certit udinea măsurătorilor este de multe ori dificilă de estimat, se tratează
comportamentul filtrului din punct de vedere al câștigului . Câștigul filtrului este o funcție de
siguranță relativă a măsurătorilor și de estimare a stadiului actual, poate fi reglat p entru a
obține performanță cât mai bună. Cu un câștig ridicat, filtrul pune accent pe măsurători și le
urmărește mai îndeaproape . Cu un casting redus, filtrul pune accept pe predicțiile modelului și
netezește zgomotul dar acesta îi scade din sensibilitate.
La punctele extreme un câștig unitar determină filtrul să ignore complet estimarea
stării, iar un câștig nul determină ignorarea măsurătorilor efectuate. La efectuarea calculelor
pentru filtru starea estimată și covarianțele sunt codificate în matrici pe ntru ocuparea
diferitelor dimensiuni implicate intr -un singur set de calcule. Acest lucru permite
reprezentarea relațiilor liniare între diferite variabile de stare în oricare dintre modelele de
tranziție sau covarianțe
Dezavantajul alegerii filtrului Kal man este acela ca trebuie sa fie reglat corespunzător
înainte de a putea fi folosit intr -un sistem, nu exista nici o metodă de reglaj fixă din cauză că
este aplicat pe o scară largă in numeroase domenii. Reglajul acestuia necesită nenumeroase
încercări si teste pană la ajungerea valorilor optime ale parametrilor de reglare.

1.6.3. Algoritmul de control

Acest algoritm are un rol foarte important pentru controlul unui robot autonom cu
autoechilibrare, deoarece acesta stă la baza principiului de auto balansa re. Pentru acest sunt
două algoritme:

a) Regulatorul PID
b) Controlul cu logica Fuzzy

16 a) Regulatorul PID

Regulatorul PID este alcătuit din 3 păți ( partea proporțională , integratoare , si
derivatoare ), este un mecanism generic de control al buclei feedback , utilizat pe scară largă
în sistemele de control industriale. Controlerul PID calculează o valoare a erorii ca fiind
diferență între variabila de proces măsurată și valoarea de referință dorită. Regulatorul
încercând să minimizeze eroarea prin ajustarea intrărilor de control ale procesului
Regulatorul utilizează trei termeni constanți diferiți, aceștia fiind termenul
proporțional , integral și derivat, notate cu P, I și D. Aceste valori pot fi interpretate în termeni
de timp: P depinde de eroarea prezentă , I de erorile acumulate din trecut , D este predicția a
erorilor viitoare, în funcție de rata actuală de schimbare.

Figură 1.6.3.1 Schema bloc a Regulatorului PID

Suma ponderată a acestor trei acțiuni este utilizată pentru reglarea unui proces pri n
intermediul controlului unui element. Prin ajustarea celor trei parametrii în algoritmul PID ,
acesta poate oferi control asupra acțiunii proiectate pentru cerințele specifice unui proces .
Răspunsul primit de regulator poate fi descris ca fiind un răspu ns primit la o eroare al
controlerului , gradul în care regulatorul se abate de la valoarea de referință precum ș i gradul
de oscilație al sistemului. Utilizarea algoritmului PID pentru control nu poate să garanteze un
control optim al sistemului sau stabil itatea acestuia.[25]

Termenul proporțional produce o valoare de ieșire care este proporționala cu
valoarea de eroare actuală. Prin înmulțirea erorilor cu o constantă Kp putem ajusta răspunsul
proporțional , constanta Kp fiind numită de câștigul proporți onal
Un câștig proporțional mare duce la o schimbare majoră a ieșirii la o anumită
schimbare a erorii. Î n cazul în care câștigul proporțional este prea mare , sistemul devine
instabil. Iar un câștig proporțional mic duce la un răspuns de ieșire mic la o eroare de intrare
mare și un regulator mai puțin receptiv sau reduce sensibilitatea acestuia.

Termenul integral contribuția acestui termen este proporțională în magnitudinea erori
cât și durata acesteia . Termenul integral intr -un regulator PID este sum a erorii instantanee în
timp și rezultă diferența acumulată care ar fi fost remediată anterior. Eroarea acumulată este
multiplicată cu un câștig integral Ki , și se adaugă la ieșirea regulatorului. Dacă forța aplicată
nu este suficientă pentru aducerea ero ri la zero , această forță va fi crescută în timp. Un
controler Integrator pur ar putea duce eroarea la zero, dar reacția va fi foarte lentă la început și
brutală

Termenul derivativ este calculat prin determinarea pantei de eroare în timp și se
înmulțeș te această rată de variație cu derivata câștigului Kd. Contribuția termenului derivat la
acțiunea de control general este determinată de castingul derivatei Kd.

17 Aceste termen încearcă să aplaneze traiectoria erorii intr -o linie orizontală, amortizând
forța aplicată și astfel reduce depășirea . Aplicarea unui impuls prea mare în momentul în care
eroarea este mică și scăderea va duce la depășire excesivă. După depășire , regulatorul dacă ar
aplica o corecție mare în direcția opusă și ar depăși în mod repetat poziția dorită, ieșirea ar
oscila în jurul valorii de referință, fie intr -o sinusoidală perfectă.

b)Controlul cu logica Fuzzy

Se utilizează un controler cu logica fuzzy pentru determinarea cursului unei acțiuni .
Calcului acestei acțiuni este alcătuit din patru etape :

 Scalarea intrării si reprofilarea
 Fuzzificare
 Inferența fuzzy
 Defuzificare

În prima etapă (scalarea intrării și reprofilarea) se prelucrează intrările sistemului
scalare corespunzător . În a doua etapă (fuzificarea) după actualizarea intrărilor ele sunt
convertite în valori fuzzy. A treia etapă (inferența fuzzy) este o acțiune adecvată de control
care este determinată de căutarea în cadrul unui tabel a regulilor stabilite anterior. Iar în
ultima etapă (defuzificare) este definită ieșirea deterministă.
Pentru implementarea acestui sistem de algoritm, trebuie să definim o matrice cu stări
multiple care să ia în considerare cel puțin două stări de valori furnizate de senzori, datorită
complexității si în stabilității crescute a sistemul ui.[25]

Șansa de eroare

Eroare FI I Z S FS
FI FI FI FI I Z
I FI FI I Z S
Z FI I Z S FS
S I Z S FS FS
FS Z S FS FS FS
Tabel 1.6.3 Tabel control logică fuzzy

Alegerea algoritmului

Pentru realizarea controlului c omportamentului robotului cu autoechilibrare am ales
utilizarea unui regulator PID și ajustarea parametrilor de calcul (parametru proporțional ,
parametru integral , parametru derivativ) după efectuarea mai multor teste.
Structura acestui robot este destu l de simpla, dar un regulator PID nu este deloc ușor
de reglat mai ales în cazul în avem nevoie de un nivel ridicat de stabilitate. Ieșirea sistemului
poate fi una instabilă sau poate intra în oscilații datorită valorilor greșit e ale parametrilor.
De asem enea se poate realiza un control al acestui robot și cu un algoritm de control
hibrid PD -fuzzy.

18 1.7. Comanda Motoarelor

Pentru comandarea motoarelor este necesară o comandă variabilă a vitezei, aceasta
trebuie sa fie cât mai liniară. Viteza motorului e ste direct proporțională cu tensiunea
motorului, de asemenea prin reducerea tensiunii motorului se va reduce si cuplul motorului nu
doar turația acestuia.

Figura 1.7 Comanda PWM a motoarelor [26]

Pentru a combate această problemă se poate folosi modu larea de tip PWM, care
funcționează prin trimiterea unui semnal dreptunghiular pentru pornirea și oprirea motorului
totul fiind la o frecventă foarte mare în așa fel încât motorul să nu se oprească
Pentru o perioadă de timp motorul primește maximul de ten siune pentru dezvoltarea
unui cuplu mai mare decât reducerea directă a tensiunii motorului pentru a controla viteza.
Tensiunea medie a motorului este proporțională cu factorul de umplere PWM, astfel factorul
de umplere PWM este proporțional cu viteza motor ului.
Pentru schimbarea direcției de mers și a sensului de rotație o comandă PWM modulată
în frecventă nu este suficientă pentru a face acest lucru, pentru schimbarea direcției de mers și
a sensului de rotație se va utiliza Puntea H.

1.8. Domenii de u tilizare a roboților

Robo ții cu auto echilibrare sunt folosiți în marea majoritate a timpul pe suprafețe
accidentate sau în scopuri militare în diferite misiuni precum și în explorarea planetelor.
Deseori sunt folosiți si în zone periculoase pentru om cum ar fi pentru recunoașterea
daunelor făcute de o centrală nucleară și pentru dezamorsarea bombelor pentru reducerea
riscului, sau pentru căutarea oamenilor în zone montane.
Sunt folosiți si pentru întreținerea cablurilor electrice sau efectuarea anumi tor lucrări
la acestea. Datorită mobilității ridicate a acestora ei pot fi folosiți în foarte multe domenii în
afara de domeniile navale unde aceștia nu pot face nimic.[4]

19 Capitolul 2. Prezentarea robotului cu autoechilibrare

2.1. Structura Robotulu i

Scopul acestei lucrări este de a realiza un robot cu autoechilibrare și acesta să fie
autonom , acesta să se deplaseze pe sol, cu ajutorul sistemului de mișcare pe roți, având
capacitatea de aș -i menține echilibrul tot timpul pe diferite suprafețe și având o mobilitate
ridicată.
Robotul este construit dintr -un cadru pe care este montată placa de dezvoltare Arduino
Nano alături de un driver motor L298N pentru comandarea celor două motoare de curent
continuu, MPU6050 senzorul care are în componentă giroscop ul si accelerometrul pentru
aflarea poziției robotului, servomotorul SG90 pentru schimbarea direcției și bateriile pentru
alimentarea tuturor componentelor.

Figura 2.1.1 Robotul autonom în autoechilibrare

20 2.2. Schema de comandă a robotului

Figura 2.2.1 Schema de comandă a robotului cu auto echilibre

21

Realizarea conexiunilor:

Arduino PIN GND – Alimentare GND
Arduino PIN +5V –Alimentare

Arduino PIN 6 – INPUT 1 Motor A+ (pentru comanda înainte)
Arduino PIN7 – INPUT 2 Motor A-(pentru comanda înapoi)
Arduino PIN8 –INPUT 3 Motor B -(pentru comanda înainte)
Arduino PIN9 – INPUT4 Motor B -(pentru comanda înapoi)

Arduino PIN 11 – Semnal PWM Servo Motor SG90
Servo Motor SG90 PIN GND – Alimentare GND
Servo Motor SG90 PIN VCC – Alime ntare

Arduino PIN GND –Senzor Înclinare MPU 6050
Arduino P IN 3V3 – Alimentare Senzor Înclinare MPU6050
Arduino PIN A4 – Pin serial Clock MPU 6050
Arduino PIN A5 – Pin serial Data MPU6050

Drive Motor L298N OUT1 – Motor A+
Drive Motor L298N OUT2 – Motor A –
Drive Motor L298N OUT3 – Motor B+
Drive Motor L298N OUT4 – Motor B –
Drive Motor PIN GND – Alimentare GND
Drive Motor PIN 5V – Alimentare

Capitolul 2.3 Driverul de motor L298N

Acest driver de motoare L298N este utilizat pentru controlul motoarelor de c urent
continuu folosind o placă de dezvoltare Arduino, aceasta nefiind capabilă să scoată pe
porturile sale o putere suficientă pentru invertirea unui motor, nu este posibilă conectarea unui
motor electric direct la un port al plăcii de dezvoltare Arduino pentru că sunt șanse mari ca să
obținem o ardere a procesorului plăcii.
Pentru evitarea acestui lucru, este necesar un amplificator de putere, acesta își va lua
putere din sursa de alimentare și o va transmite motoarelor așa cum este comandat de placa de
dezvoltare Arduino.
Amplificatorul are numele uzual de Driver de Motoare , acesta se bazează pe o punte
H, în momentul în care comandăm două tranzistoare să acționeze motorul într -o anumită
direcție acesta să poată învârti motorul în direcția cerută că să în direcția opusă la comandă.
Driverul este de nivel mediu din punct de vedere al puterii conduse , acesta poate
controla motoare care necesită cel mult 2 amperi , fiecare în ambele sensuri
Acest drive rezistă la o putere de la sursa de alimentare de până în 35 de V, acesta
putând să controleze si motoare de tip Stepper.[27]

22
Specificații tehnice:

 Tensiune de intrare: 3.2V ~ 40Vdc.
 Driver: L298N Dual H Bridge Dc driver motor
 Sursa de alimentare: DC 5V -35V
 Curent maxim: 2 Amperi
 Domeniul de operare: 0 ~ 36mA
 Interval de tensiune de intrare a semnalului de control:
o Scăzut : -0.3V<Vin<1,5V
o Mare: 2.3V<Vin1<Vss
 Activarea intervalului de tensiune de intrare a semnalului:
o Scăzut : -0.3<Vin<1.5V(Semnalul de control nu este activ)
o Mare: 2.3V<Vin<Vss(Semnal de cont rol este activ)
 Consum maxim de energie: 20W
 Temperatura de depozitare: -25*C ~ +130*C
 Alimentare la ieșire reglată +5V

Figura 2.3.1 Driverul de motor L298N

Exemplu program pentru controlul motoarelor cu ajutorul algoritmului PID:

#include <L298N X2.h>

// Definirea pinilor
const unsigned int EN_A = 10;

23 const unsigned int IN1_A = 6;
const unsigned int IN2_A = 7;

const unsigned int IN1_B = 8;
const unsigned int IN2_B = 9;
const unsigned int EN_B = 5;

// Inițializarea ambelor motoare
L298NX2 motor s(EN_A, IN1_A, IN2_A, EN_B, IN1_B, IN2_B);

float curent_position,error,last_error,pwm,kp,ki,kd,integral,derivative,pwm2;

void setup() {

}

void loop() {

// PID

kp=32; //constanta proporționala
ki=0.3; // constanta integratoare
kd=80; //con stanta derivatoare

last_error=error;
curent_position = kalAngleX;
error = 37.67 – curent_position;
integral=integral+error;
derivative= error – last_error;
pwm=(kp*error)+(ki*integral)+(kd*derivative);

if(pwm>255) pwm=255;
else if(pwm< -255) pwm= -255;

if (pwm > 0)
{
motors.setSpeed(pwm);
motors.backward();
}
else if (pwm <0)
{
pwm2 = (pwm)*( -1);
motors.setSpeed(pwm2);
motors.forward();
}
else motors.stop();
}

24 Capitolul 2.4. Senzorul de înclinare MPU6050

Aceste dispozitive sunt printre p rimele concepute pentru cerințele de putere redusă,
costuri reduse și performanțe ale tabletelor și senzorilor purtabili.
Aceste au incorporate firmware -ul de calibrare care permite producătorilor să elimine
selecția, calificare și integrarea la nivel de s istem costisitoare și complexe a dispozitivelor
discrete în produse cu mișcare, asigurând că algoritmii de fuziune .
Aceste dispozitive au un giroscop cu 3 axe și un accelerometru cu 3 axe pe același
matriță de siliciu, împreună cu un procesor digital de mișcare , care procesează algoritmi
complecși cu 6 axe. Dispozitivul poate accesa magnetometre extreme sau alți senzori printr –
un bus I2C, care permite dispozitivelor să adune un set complet de date ale senzorului fără
intervenția procesului de sistem.[28]

Specificați tehnice:
 Tensiunea de intrare: 3.3V -5V
 Tensiune magistrală I2C : 3.3V
 Interfața serială suportată : I2C
 Curent: 5mA
 Raza programabilă giroscop : +250, +500, +1000,+2000 0/s
 Raza programabilă accelerometru: +2g , +4g , +8g , +16g.
 Interval măsu rare temperatură : -40 0C – +85 0C
 Frecvența maximă I2C – 400kHz

Figura 2.4.1 Senzorul de înclinare MPU 6050

Exemplu program pentru senzorul de înclinare MPU 6050: și filtrarea datelor cu
ajutorul filtrului Kalman :

#include <Wire.h>
#include <Kalman .h>
#define RESTRICT_PITCH

Kalman kalmanX; // Crearea instanțelor Kalman

25 Kalman kalmanY;

/* IMU Data */
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
int16_t tempRaw;

double gyroXangle, gyroYangle; // Calcularea unghiului utilizând doar girosco pul
double compAngleX, compAngleY; // Calcularea unghiului utilizând filtrul complementar
double kalAngleX, kalAngleY; // Calcularea unghiului utilizând filtrului Kalman

uint32_t timer;
uint8_t i2cData[14]; // Buffer pentru I2C data

// TODO: Crearea ru tinei de calibrare

void setup() {
Serial.begin(115200);
Wire.begin();
#if ARDUINO >= 157
Wire.setClock(400000UL); // Setarea I2C la o frecventă de 400kHz
#else
TWBR = ((F_CPU / 400000UL) – 16) / 2; // Setarea I2C la o frecventă de 400kHz
#endif

i2cData[0] = 7 ;
i2cData[1] = 0x00;
i2cData[2] = 0x00;
i2cData[3] = 0x00; //
while (i2cWrit e(0x19, i2cData, 4, false));
while (i2cWrite(0x6B, 0x01, true));
sleep mode

while (i2cRead(0x75, i2cData, 1));
if (i2cData[0] != 0x68) { // Citire registru "WHO_AM_I"
Serial.print(F("Error reading sensor"));
while (1);
}

delay(100); // Delay pentru stabilizarea senzorului

/* pornire kalman ș i începerea unghiului */
while (i2cRead(0x3B, i2cData, 6));
accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);

// C
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan( -accX / sqrt( accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29

26 double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2( -accX, accZ) * RAD_TO_DEG;
#endif

kalmanX.setAngle(roll); // Setarea unghiului de început
kalmanY.setAngle(pitch);
gyroXangle = roll;
gyroYangle = pitch;
compAngleX = roll;
compAngleY = pitch;

timer = micros();
}

void loop() {

/* Actualizarea tuturor valorilor */
while (i2cRead(0x3B, i2cData, 14));
accX = (int16_t)((i2cData[ 0] << 8) | i2cData[1]);
accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
tempRaw = (int16_t)((i2cData[6] << 8) | i2cData[7]);
gyroX = (int16_t)((i2cData[8] << 8) | i2cData[9]);
gyroY = (int16_t)( (i2cData[10] << 8) | i2cData[11]);
gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]);;

double dt = (double)(micros() – timer) / 1000000; // Calcularea timpului
timer = micros();

// Convertire din rază în grade
#ifdef RESTRICT_PITCH // Eq. 25 an d 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan( -accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2( -accX, accZ) * RAD_TO_DEG;
#endif

double gyroXrate = gyroX / 131.0; // Convertire în grade
double gyroYrate = gyroY / 131.0; // Convertire în grade

#ifdef RESTRICT_PITCH

// Remedierea problemei de translație cană accelerometrul sare de la +180 la -180
if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
kalmanX.setAngle(roll);
compAngleX = roll;
kalAngleX = roll;
gyroXangle = roll;
} else
kalAngleX = kalmanX.get Angle(roll, gyroXrate, dt); // calcularea unghiu lui cu filtrul
kalman

27
if (abs(kalAngleX) > 90)
gyroYrate = -gyroYrate;
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else

// Remedierea problemei de translație cană accelerometrul sare de la +180 la -180

if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
kalmanY.setAngle(pitch);
compAngleY = pitch;
kalAngleY = pitch;
gyroYangle = pitch;
} else
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // calcularea unghiului cu filtrul
kalman

if (abs(kalAngleY) > 90)
gyroXrate = -gyroXrate;
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // calcularea unghiului cu filtrul
kalman

#endif

gyroXangle += gyroXrate * dt; // Calcularea unghiului giroscopului fără nici un filtru
gyroYangl e += gyroYrate * dt;
//gyroXangle += kalmanX.getRate() * dt; // calcularea unghiului giroscopului
//gyroYangle += kalmanY.getRate() * dt;

compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // calcularea unghiului
utilizând filtrul comp lementar
compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;

// resetare giroscop când trece de un unghi
if (gyroXangle < -180 || gyroXangle > 180)
gyroXangle = kalAngleX;
if (gyroYangle < -180 || gyroYangle > 180)
gyroYangle = kalAngleY;

/* Afișare date */

// Serial.print(accX); Serial.print(" \t");
// Serial.print(accY); Serial.print(" \t");
//Serial.print(accZ); Serial.print(" \t");

// Serial.print(gyroX); Serial.print(" \t");
// Serial.print(gyroY); Serial.print(" \t");
// Serial.print(gyroZ); Serial.print(" \t");

// Serial.print(" \t");
// Serial.print(roll); Serial.print(" \t");
// Serial.print(gyroXangle); Serial.print(" \t");

28 // Serial.print(compAngleX); Serial.print(" \t");
// Serial.print(kalAngleX); Serial.pr int("\t");

// Serial.print(" \t");

// Serial.print(pitch); Serial.print(" \t");
// Serial.print(gyroYangle); Serial.print(" \t");
// Serial.print(compAngleY); Serial.print(" \t");
// Serial.print(kalAngleY); Serial.print(" \t");

#if 0 // 1 pentru afișar ea temperaturi
// Serial.print(" \t");

double temperature = (double)tempRaw / 340.0 + 36.53;
// Serial.print(temperature); Serial.print(" \t");
#endif

// Serial.print("Xș: ");
//Serial.print(kalAngleX); Serial.print(" \t");

//Serial.print("Yș: ");
// Serial.print(kalAngleY); Serial.print(" \t");

}

29 Capitolul 3. Software -ul Arduino Nano

3.1 Microcontrolere

Un microcontroler este o structură electronică destinată, pentru a controla un proces
ori în mod general este un micro circuit care încorporează o unitate adică un CPU și o
memorie împreună cu resurse care -i permit interacțiunea cu mediul exterior.

Microcontrolerul este la fel ca un calculator, specializat în operații de intrare/ieșire pe
un singur chip și este alcătuit din cinci elemente de bază:

 Unitatea de intrare
 Unitatea aritmetică și logică
 Unitatea de memorie
 Unitatea de ieșire
 Unitatea de control

Un microcontroler diferă în mai multe feluri de un microprocesor în multe feluri.
Prima diferență dintre aceștia ar fi importanța și funcționalitatea sa. Pentru a putea fi folosit
un microprocesor este necesar să i se incorporeze și alte componente precum memoria și
componente pentru primirea și transmiterea de date.
În schimb microcontrolerul este creat să le dețină p e toate acestea, nefiind necesare
alte componente externe pentru utilizarea sa , fiindcă acesta are incluse deja toate perifericele
necesare.[35]
Câteva dintre multele domenii unde se utilizează microcontrolere ar fi:

 În industria de automobile( controlul aprinderii motorului, climatizarea,
diagnoza).
 În electronica de consum(sisteme audio, televizoare, camere video).
 În aparatura electrocasnică(mașini de spălat, frigidere, cuptoare cu microunde).
 În controlul mediului și climatizare (sere, locuințe,hale in dustriale).
 În mijloace moderne de măsurare(aparate de măsură, senzori )
 În medicină.
 La realizarea de periferice pentru calculatoare.

Figura 3.1 .1 Schema bloc a unui Microcontroler [36]

30 3.1.1. Structura unui Microcontroler

3.1.1.1. Unitat ea de memorie

Memoria este o componentă a Microcontrolerului , acesta folosește diferite tipuri de
informații care sunt stocate în diferite tipuri de memorii. Toate comenzile date adică
instrucțiunile trebuie stocate intr -o memorie nevolatilă, în care to ată informația să fie stocată
și după oprirea sau repornirea microcontrolerului.
Memoria trebuie să permită citirea si scrierea în ea , aceasta poate fi accesată prin
adresarea adresei memoriei selectare, toate aceste se pot realiza prin asigurarea unei l inii de
control (R/W).

Figură 3.1 .1.1.1 Model simplificat de unitate de memorie [37]

Memoria este de mai multe tipuri:
 Memoria RAM – aici vor fi stocate variabilele, sunt stocate doar în timpul
funcționării după repornire acestea sunt șterse .
 Memoria ROM – aici vor fi stocate programele în faza de fabricație, CPU -ul
poate citi informațiile dar nu le poate modifica.
 Memoria PROM – este similară cu memoria ROM, aceasta poate fi programată
de utilizator.
 Memoria DMA – această memorie este întâlnită doar la controlere mai mari , ea
gestionează transferul de informație între componentele periferice și memorie.
3.1.1.2. Unitatea centrala de procesare

Unitatea centrală de procesare sau numită și CPU este alcătuită din mai multe locații
de memorie având imple mentată capabilitatea de a face înmulțiri, împărțiri, scăderi și adunări.
Permițând mutarea informației dintr -o locație a memoriei în alta. Locațiile acesteia sunt
numite regiștri.

Acești regiștrii au rolul de a ajuta la executarea a variate operații mate matice sau a
altor operații cu date.

31

Figură 3.1 .1.2.1 Unitate simplificată de procesare centrală cu trei Regiștrii [37]

Unitatea centrală de procesare conține următoarele:

 Unitatea aritmetico -logică
 Unitatea de control
 Registre (stack pointer, progra m counter , acumulator registru)
 Oscilator intern pentru ceasul de sistem.
3.1.1.3. Bus -ul

Așa zisul Bus sau magistrala este alcătuită dintr -un grup de 8,16, 32,64 sau mai multe
fire. Există două tipuri de bus -uri :

 Bus de adrese – el fiind alcătuit din atâtea linii cât este numărul de cifre binare
adică dacă este de 8 biți o să fie alcătuit din 8 fire + 1 fir pentru masa.
 Busul de date – este atât de lat cât sunt datele.

Bus-ul de adrese este folosit la transmiterea adreselor de la CPU la memorie, iar Bus-
ul de date este folosit pentru conectarea tuturor blocurilor din interiorul microcontrolerului.

Figura 3.4.1.3.1. Bu s-ul ( conectarea dintre CPU si Memorie)[37]

32 3.1.1.4. Unitatea de intrare -ieșire (I/O)

Unitatea de intrare -ieșire este alcătuită din 2 sisteme :

 Sistemul d e intrare -ieșire digitale, conț ine porturile digitale paralele
 Sistemul de intrare -ieșire analogic, conține porturile analogice care
funcționează pe 8 -12 biți și sunt 2 -16 porturi.

Unitatea de intrare -ieșire este folosită pentru co municarea între microcontroler și
”lumea exterioară”.

Figura 3.1 .1.4.1 Unitate intrare -ieșire schemă [37]

Dispozitivele de intrare/ieșire conduc operații generale de comunicație, funcții
generale de timp, operații de conversie analog/numerică, funcții speciale de comandă.
O mare parte din resurse este folosită pentru funcțiile de control propriu -zise, iar
cealaltă parte asigură funcțiile aplicațiilor necesare în timp real.
Microcontrolerul utilizat de placa de dezvoltare Arduino Nano R3 este ATmega 3 28,
este un microcontroler de înaltă performanță, cu un consum redus și un microcip bazat pe 8
biți, care combină memoria FLASH isp de 32kb , SRAM de 2kb, EEPROM de 1kb. 23 de lini
de intrare/ieșire de uz general și 32 de registre, 3 cronometre flexibile/c ontoare. O orientare
interfață serială byte de 2 cabluri, SPI port serial, 6 canale de 10 -biți 5 moduri de software
intern de economisire a energiei.

Figura 3.1 .1.4.2. Microcontrolerul ATmega 328 [38]

Prin executarea instrucțiunilor puternice într -un singur ciclu de ceas și având un timp
de răspuns de 1MIPS.

33 3.2 Placa de dezvoltare Arduino Nano R3

Arduino nano este o placă de dezvoltare de dimensiuni mici care are în componentă
micro -controlerul (atmega328p) de pe placa de dezvoltare Arduino uno și u n convertor USB
serial CH 340.
Unul dintre avantajele acestei plăci este dimensiunea reduse și integrarea acesteia în
diferite proiecte unde dimensiunea componentelor este importantă. Programarea acesteia se
realizează prin intermediul unui cablu cu mufă m ini-B USB , această placă vine și cu un
bootloader. Această versiune are si următoarele ajustări:

 Circuit -ul de reset este unul mai puternic față de versiunea R2 a plăcii
 S-au adăugat pinii SDA și SCL la pinul AREF și 2 pini noi la pinul RESET,
respectiv pinul IOREF, permițând scuturilor să se ajusteze la tensiunea de pe
placă. Pe viitor, scuturile vor fi compatibile atât cu versiunile de 5v cât și de
3,3 v. Al doilea pin nu este conectat și este rezervat pentru actualizări viitoare.

Platforma de dezvolt area Arduino poate fi utilizată pentru a dezvolta obiecte
interactive și interceptează semnale de intrare de la o varietate de switch -uri, ori senzori după
care poate controla o multitudine de led -uri, sau motoare și alte ieșiri fizice.[29]

Figura 3.2 .1 Placa de dezvoltare Arduino nano R3

Specificați tehnice :

 Tensiune de alimentare suportată de limitator: 7 V – 12 V;
 Tensiune de alimentare: 5 V;
 Pini Input/Output: 14;
 Pini ADC: 8 (din cei 14 de Input/Output);
 Pini PWM: 6 (din cei 14 de Input/Output) ;
 Memorie flash: 32 kB / 16 kB (din care 2 kB sunt folosiți de bootloader);
 Comunicație TWI, SPI și UART;
 Curent pentru pini Input/Output: 40 mA/pin;
 Frecvență de funcționare: 16 MHz;
 Dimensiuni: 45 x 18 mm

34 3.3. Alimentarea platformei Arduino

Placa d e dezvoltare Arduino Nano R3 poate fi alimentată prin conexiunea USB (USB
mini B), ori cu o sursă externă de alimentare. Alimentarea externă poate proveni de la un
adaptor, fie de la o baterie, sursa de alimentare se selectează într -un mod automat.
Pini de la o baterie pot fi inserați la pinii GND și Vin ale conectorului Power, placa
poate funcționa pe o sursă externă de 5.5 până la 16 volți, dacă acesta este furnizat cu mai
puțin de 7V, pinul 5V poate furniza mai puțin de 5 volți și placa poate deveni inst abilă.
Dacă se utilizează o tensiune mai mare de 12V, regulatorul de tensiune se va încălzi și
va deteriora placa. Un interval recomandat ar fi de la 7 la 12 volți.

Pinii de alimentare a plăcii de alimentare Arduino sunt următorii:

 VIN – este tensiune d e intrare a plăcii de dezvoltare Arduino atunci când
utilizează o sursă externă de alimentare. Se poate furniza o tensiune prin acest
pin sau, dacă tensiunea este furnizată prin mufa de alimentare, ea poate fi
accesată prin acest pin

Figura 3.3 .1. C ablu USB tip B [31] Figura 3.3 .2 Alimentator extern AC -DC
pentru alimentarea platformei Arduino

 5V – Acest pin scoate 5V și este reglat de la regulatorul de pe placa de
dezvoltare. Placa poate fi alimentată de la mufa de alimentare DC (7 -12v),
tensiunea de alimentare prin pinii 5V sau 3,3 V ocolește regulatorul și poate
deteriora placa.
 3V3. – Este un pin , care generează o sursă de 3.3 volți de la regulatorul plăcii
de dezvoltare. Acesta rezistă la un curent de maxim 50 mA.
 GND – este pinul care are masa.
 IOREF – Este un pin de pe placa de dezvoltare Arduino care furnizează o
tensiune de referință cu care microcontrolerul funcționează.

35
Figura 3.3 .3. Schema electrică a platformei de dezvoltare Arduino Nano R3 [32]

36 3.4 Descrierea pinilor plăcii de dezvoltarea Arduino Nano R3

Oricare dintre pinii digitali de pa placa de dezvoltare Arduino nano r3 poate fi folosit
ca intrare sau ieșire , folosindu -se funcțiile pinMode(), digitalRead() și digitalWri te(), acești
pini funcționează la o tensiune de 5 volți.[33]

Oricare dintre pini poate genera sau primi un maxim de 40 mA și deține un rezistor
intern de tracțiune de 20 -50 kOhm. Unii pini au funcții specializate:

 Serial RX / TX – sunt folosiți pentru a primi si a transmite datele TTL
 Întreruperi externe (2,3) – sunt folosiți pentru a genera o întrerupere la o valoare
scăzută , o margine în creștere sau în scădere sau o schimbare a valorii.
 PWM – pini 3,5,6,9,11. Aceștia asigură o ieșire PWM pe 8 biți cu funcția
analogWrite().
 Pini analogici (A0 -A7) – Sunt folosiți pentru a măsura analog voltajul între 0 -5V.
 Pini digitali (D0 -D13) – Sunt folosiți pentru intrare sau ieșire. (0V – LOW , 5V –
HIGH).
 SPI- pini 10(SS),11(MOS),12 ()MISO), 13(SCK). Acești pini p ot suporta comunicarea
SPI utilizând biblioteca SPI.
 LED – pin 13. Placa deține un LED incorporat conectat la pinul digital 13. În momentul
în care pinul este setat pe HIGH, led -ul este pornit, iar în momentul în care este LOW
acesta este oprit.
 AREF – este un pin care transmite referința pentru intrările analogice, folosite cu
analogReference().
 RESET – Această linie se aduce pe LOW pentru a reseta microcontrolerul.

3.5 Semnale PWM

Semnalele PWM sunt module de lățime a pulsului, este o tehnică pentru obț inerea
rezultatelor analogice cu mijloace digitale. Manipularea digitală este utilizată pentru generarea
unei unde pătrate a unui semnal pornit sau oprit. Acest model Pornit -Oprit poate simula
tensiunile între 5V și 0V, prin schimbarea porțiunilor de timp pe care semnalul o petrece față
de timpul petrecut de semnal.
Durata se numește lățimea pulsului, pentru obținerea valorii analogice diferite , se
schimbă sau modulează acea lățime a pulsului. Dacă se repetă acest model de pornire suficient
de rapid cu un LED de exemplu , rezultatul este ca și cum tensiunea ar fi una constantă
undeva intre 0 -5V și controlând luminozitatea ledului respectiv.
În cazul unui motor, căruia ăi este aplicat un semnal PWM cu factor de umplere de
0%, viteza de rotație a acestui va f i egală cu 0 rotații pe minut, iar un factor de umplere de 100
% va duce la o turație maximă a acestuia.
Factorul de umplere se exprimă prin procente și este reprezentat cât la sută din
perioada unui semnal va fi pe nivelul ON, se poate observa semnalul P WM cu factori de
umplere diferiți.
Modularea folosește variația factorului de umplere a unei forme de undă
dreptunghiulară pentru a genera la ieșire o tensiune analogică.
O mulțime de circuite digitale pot genera semnale PWM, multe microcontrolere fac
acest lucru folosind un numărător care este incrementat periodic și care este resetat la sfârșitul
fiecărei perioade a PWM -ului.

37

Figura 3.5 .1 Semnal PWM cu diferiți factori de umplere

Elementele de bază alei unui modulator PWM ar fi următoarele:

 Com parator
 Generator de semnale (dinte de ferăstrău
 Amplificatorul de eroare (compară tensiune de referință și semnalul de reacție)

De obicei semnalul de reacție este generat printr -o divizare a tensiunii de ieșire.

Figura 3.5 .2 Circuit simplificat pentru realizarea semnalului PWM [34]

38

Modulatorul PWM ,constă dintr -un generator în dintre ferăstrău , un amplificator și
un comparator.

Mod de fun cționare:

Tensiune de ieșire a amplificatorului de eroare este comparată cu calitatea semnalului
în dinte de ferăstrău. Dacă valoarea dintelui de ferăstrău este mai mică decât ieșirea atunci la
ieșirea comparatorului vom avea ”1” logică. Dacă ieșirea amplificatorului este mai mică decât
valoarea dintelui de ferăstrău atunci la ieșirea comparatorului vom avea ”0” logic.
Dacă tensiune de ieșire tinde să crească, atunci tensiunea de reacție va crește și ea
peste pensiunea de referință. Tensiunea de ieșire a amplificatorului de eroare va avea o
valoare mică rezultând astfel o durată mai mică pentru care la ieșirea c omparatorului vom
avea ”1” logic. Tensiunea de ieșire scade atunci când la ieșirea comparatorului vom avea o
durată mai mare de ”1”! logic. Această modificare a lățimii impulsurilor în funcție de
tensiunea de ieșire este datorată factorului de umplere.
În unele cazuri în care tensiunea de ieșire este constantă această se va menține de către
reacția negativă la valoarea dorită

39 Capitolul 4. Mediul de programare Arduino

4.1. Prezentarea mediului de programare

Arduino este o p latformă foarte simplă de utilizat și uș or de învățat aceasta suportă
două limbaje de programare C și C++ , ea poate fi folosită pentru crearea unor aplicații
interactive. Toată informația fiind preluată de la elementele de intrare cum ar fi senzori și
comutatoare, toată această informație fiind procesată în interiorul microcontrolerului și este
trimisă la elementele de ieșire cum ar fi motoare,leduri,etc.

Câteva avantaje ale plăcilor Arduino ar fi:

 Preț scăzut la achiziționare
 Crearea aplicațiilor cu o ușurință foarte mare
 Comptabilitate pe mai multe sisteme de operare (Windows,Linux,MacOS) ,
marea majoritate a plăcilor de dezvoltare sunt folosite pentru sistem de operare
Windows.
 Nu necesită multe cunoștințe pentru crearea codurilor.

Programele Arduino se pot scrie în orice limbaj de programare cu un compilator care
poate să producă un cod mașină binar.
Arduino utilizează un mediu de dezvoltare (IDE), acesta fiind o aplicație scrisă în Java
este proiectat pentru a introduce ”programarea” în lumea artiș tilor și a persoanelor nu prea
cunoscătoare cu dezvoltarea software.
Include și un editor de cod cu funcții de bază cum ar fi evidențierea sintaxelor,
potrivirea acoladelor și spațierea automată, oferă de asemenea și mecanisme simple pentru a
compila și a încărca programele în plăcuța Arduino .
Un program scris în IDE pentru Arduino deține denumirea ”sketch”. [39]
Orice program arduino trebuie să conțină două funcții care sunt compilate și legate,
acestea fiind următoarele :

 Setup() – este o funcție care este rulată o singură dată la începutul programului.
 Loop() – este o funcție care este rulată în mod repetat până la oprirea plăcuței.

40

Figura 4.1 Interfața mediul Arduino

4.2 Instalarea mediului de programare Arduino și rularea unui program

În pri mul rând trebuie descărcat mediu l de dezvoltare care este disponibil în mod
gratuit pe site-ul producătorului pentru mai multe sisteme de operare, următorul pas este
descărcarea driverelor necesare. În continuare o să enumer pași necesari pentru instalarea
programului pe calculator și realizarea conexiunii cu placa Arduino nano r3, aceștia sunt:

 Achiziționarea unui plăci de dezvoltare Arduino Nano R3 și a unui cablu USB
 Descărcarea mediului de programare Arduino IDE
 Conectarea plăcii la calculator cu ajuto rul cablului USB (dacă este conectat
corespunzător un LED va fi aprind pe placa de dezvoltare)
 Instalarea driverelor necesare
 Rularea mediului de dezvoltare
 Setarea modelului plăcii. Tools -> Board
 Setarea portului la care sa realizat conexiunea cu placa A rduino.
Tools -> Serial Port
 Deschiderea unui exemplu gol (Sketch).
File -> Exemples -> 1.Basics -> Blink
 Compilarea programului și încărcarea acestuia în placa de dezvoltare.
File -> Upload and Compile

4.3 Instrucțiuni utilizate pentru programarea robo tului

Majoritatea programelor arduino sunt împărțite după tipul instrucțiunilor în 3 grupe:

 Instrucțiuni de structură
 Instrucțiuni de valori (Variabile sau constante)
 Instrucțiuni pentru funcții

41 Datorită numeroaselor instrucțiuni destinate mediului de programare arduino , o să
prezint doar cele mai des folosite pentru funcționarea robotului mobil.

Instrucțiunea ”void” este o instrucțiune care aparține grupului de valori și este utilizată
numai în declararea funcțiilor. Această funcție nu așteaptă nici o valoare returnată de la
funcția care s -a apelat.
Instrucțiunea ”Void setup()”, această instrucțiune este rulată de fiecare dată când este
alimentată , la încărcarea unui program nou, sau la resetarea plăcii. De obicei în această
instrucțiune sunt puse v ariabilele și orice valori stabilite inițial.
După crearea configurației cu valorile inițiale prin funcția ”Void loop()”, această
funcție este ca o buclă, permite programului să facă schimbări sau să răspundă dacă este
cazul.
Instrucțiunea ”Serial.begin( )”, este utilizat pentru setarea ratei de date în biți pe
secundă pentru transmisia de date seriale. Și pentru comunicarea cu Serial Monitor.
Instrucțiunea ”PinMode(Nr pin,OUTPUT)”, această funcție transmite plăcii arduino să
utilizeze un pin ca ieșire, f ără această instrucțiune arduino nu ar ști ce să facă cu fiecare pin.
”DigitalWrite(Nr pin,HIGH)”, este utilizat pentru a seta un pin pe HIGH sau LOW
adică pornit -oprit.
”analogWrite”, este o instrucțiune care atribuie o valoare analogică unui pin și po ate fi
folosită pentru controlarea vitezei unui motor.
”Delay(Nr milisecunde)”, această instrucțiune transmite plăcii să aștepte o anumită
perioada de timp (în milisecunde).
”Serial.printIN()”, afișează un text pe interfața serială.
”if(condiție)”, este utilizat împreună cu un operator de comparație, verifică daca este
îndeplinită o condiție.
”#include <nume bibliotecă>”, este folosită pentru inserarea unei biblioteci. [40]
”#define ”, este folosit pentru a da un nume unei valori constant înainte de compilarea
programului .

42

Capitolul 5. Implementarea programelor pentru robotul mobil

5.1. Implementarea programului pentru platforma Arduino Nano

Acest subcapitol are ca conținut programul de comandă realizat pentru robotul mobil
în autoechilibrare, pentru mediul de programare Arduino care a fost încărcat in memoria
platformei Arduino Nano R3.
Programul are ca scop de a menține echilibrul robotului în orice situație și în cazul în
care intervine omul pentru destabilizarea lui.

Codul sursă al programului:

#include <Wire.h>
#include <Kalman.h>
#define RESTRICT_PITCH
#include <L298NX2.h>
#include <I2C.h>

Kalman kalmanX; // Crearea instanțelor Kalman
Kalman kalmanY;

/* IMU Data */
double accX, accY, accZ;
double gyroX, gyroY, gyro Z;
int16_t tempRaw;

double gyroXangle, gyroYangle; // Calcularea unghiului utilizând doar giroscopul
double compAngleX, compAngleY; // Calcularea unghiului utilizând filtrul complementar
double kalAngleX, kalAngleY; // Calcularea unghiului utilizând filtr ului Kalman

uint32_t timer;
uint8_t i2cData[14]; // Buffer pentru I2C data

// Definirea pinilor
const unsigned int EN_A = 10;
const unsigned int IN1_A = 6;
const unsigned int IN2_A = 7;

const unsigned int IN1_B = 8;
const unsigned int IN2_B = 9;
const unsigned int EN_B = 5;

// Inițializarea ambelor motoare
L298NX2 motors(EN_A, IN1_A, IN2_A, EN_B, IN1_B, IN2_B);

//Definirea valorilor pentru controlul PID al motoarelor

float curent_position,error,last_error,pwm,kp,ki,kd,integral,derivative,pwm2;

43
// TODO: Crearea rutinei de calibrare

void setup() {
Serial.begin(115200);
Wire.begin();
#if ARDUINO >= 157
Wire.setClock(400000UL); // Setarea I2C la o frecventă de 400kHz
#else
TWBR = ((F_CPU / 400000UL) – 16) / 2; // Setarea I2C la o frecventă de 400kHz
#endif

i2cData[0] = 7;
i2cData[1] = 0x00;
i2cData[2] = 0x00;
i2cData[3] = 0x00; //
while (i2cWrite(0x19, i2cData, 4, false));
while (i2cWrite(0x6B, 0x01, true));
sleep mode

while (i2cRead(0x75, i2cData, 1));
if (i2cData[0] != 0x 68) { // Citire registru "WHO_AM_I"
Serial.print(F("Error reading sensor"));
while (1);
}

delay(100); // Delay pentru stabilizarea senzorului

/* pornire kalman și începerea unghiului */
while (i2cRead(0x3B, i2cData, 6));
accX = (int16 _t)((i2cData[0] << 8) | i2cData[1]);
accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);

// C
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = at an(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2( -accX, accZ) * RAD_TO_DEG;
#endif

kalmanX.setAngle(roll); // Setarea ungh iului de început
kalmanY.setAngle(pitch);
gyroXangle = roll;
gyroYangle = pitch;
compAngleX = roll;
compAngleY = pitch;

timer = micros();
}

44
void loop() {

/* Actualizarea tuturor valorilor */
while (i2cRead(0x3B, i2cData, 14));
accX = (in t16_t)((i2cData[0] << 8) | i2cData[1]);
accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
tempRaw = (int16_t)((i2cData[6] << 8) | i2cData[7]);
gyroX = (int16_t)((i2cData[8] << 8) | i2cData[9]);
gyroY = (int16_t)((i2cData[10] << 8) | i2cData[11]);
gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]);;

double dt = (double)(micros() – timer) / 1000000; // Calcularea timpului
timer = micros();

// Convertire din rază în grade
#ifdef RESTRICT_PI TCH // Eq. 25 and 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan( -accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double p itch = atan2( -accX, accZ) * RAD_TO_DEG;
#endif

double gyroXrate = gyroX / 131.0; // Convertire în grade
double gyroYrate = gyroY / 131.0; // Convertire în grade

#ifdef RESTRICT_PITCH

// Remedierea problemei de translație cană accelerometrul sare d e la +180 la -180
if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
kalmanX.setAngle(roll);
compAngleX = roll;
kalAngleX = roll;
gyroXangle = roll;
} else
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // c alcularea unghiului cu filtrul
kalman

if (abs(kalAngleX) > 90)
gyroYrate = -gyroYrate;
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else

// Remedierea problemei de translație cană accelerometrul sare de la +180 la -180

if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
kalmanY.setAngle(pitch);
compAngleY = pitch;
kalAngleY = pitch;
gyroYangle = pitch;

45 } else
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // calcularea unghiului cu filt rul
kalman

if (abs(kalAngleY) > 90)
gyroXrate = -gyroXrate;
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // calcularea unghiului cu filtrul
kalman

#endif

gyroXangle += gyroXrate * dt; // Calcularea unghiului giroscopului fără nici un fi ltru
gyroYangle += gyroYrate * dt;
//gyroXangle += kalmanX.getRate() * dt; // calcularea unghiului giroscopului
//gyroYangle += kalmanY.getRate() * dt;

compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // calcularea unghiului
utiliz ând filtrul complementar
compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;

// resetare giroscop când trece de un unghi
if (gyroXangle < -180 || gyroXangle > 180)
gyroXangle = kalAngleX;
if (gyroYangle < -180 || gyroYangle > 180 )
gyroYangle = kalAngleY;

// PID

kp=32; //constanta proporționala
ki=0.3; // constanta integratoare
kd=80; //constanta derivatoare

last_error=error;
curent_position = kalAngleX;
error = 37.67 – curent_position;
integral=integral+error;
derivative= error – last_error;
pwm=(kp*error)+(ki*integral)+(kd*derivative);

if(pwm>255) pwm=255;
else if(pwm< -255) pwm= -255;

if (pwm > 0)
{
motors.setSpeed(pwm);
motors.backward();
}
else if (pwm <0)
{
pwm2 = (pwm)*( -1);
motors.setSpeed(pwm2);

46 motors.forward();
}
else motors.stop();

/* Afișare date */

// Serial.print(accX); Serial.print(" \t");
// Serial.print(accY); Serial.print(" \t");
//Serial.print(accZ); Serial.print(" \t");

// Serial.print(gyroX); Serial.print(" \t");
// Serial .print(gyroY); Serial.print(" \t");
// Serial.print(gyroZ); Serial.print(" \t");

// Serial.print(" \t");
// Serial.print(roll); Serial.print(" \t");
// Serial.print(gyroXangle); Serial.print(" \t");
// Serial.print(compAngleX); Serial.print(" \t");
// Ser ial.print(kalAngleX); Serial.print(" \t");

// Serial.print(" \t");

// Serial.print(pitch); Serial.print(" \t");
// Serial.print(gyroYangle); Serial.print(" \t");
// Serial.print(compAngleY); Serial.print(" \t");
// Serial.print(kalAngleY); Serial.print(" \t");

// Serial.print("Xș: ");
//Serial.print(kalAngleX); Serial.print(" \t");

//Serial.print("Yș: ");
// Serial.print(kalAngleY); Serial.print(" \t");

}

}

47 Concluzie

Scopul acestei lucrări a fost de a realiza un robot mobil cu autoechil ibrare care să fie
autonom cu roți mici și punerea în evidență și îmbinarea a trei componente de bază a
roboticii:

 Mecanica
 Electronica
 Informatica(utilizarea microcontrolerelor, a microprocesoarelor și
calculatoarelor de proces)

Robotica are un domeniu foarte vast, existând o grămadă de modele de roboți cu
construcții diferite care sunt folosiți în scopuri educaționale și amuzament sau chiar pentru
situații critice.
Partea cea mai importantă în timpul proiectării robotului cu autoechilibrare este ca el
să fie echipat cu un senzor de înclinație, acesta trebuie să aibă capacitatea de a lua toată
informația necesară pentru a o lua o decizie și a planifica acțiunea următoare. Robotul trebuie
să fie capabil să extragă informații de la senzorul său și să le int erpreteze, pentru a calcula
următoarea sa mișcare. Un robot cu autoechilibrare autonom, trebuie să fie capabil să poată
să-și mențină echilibrul pe toată durata traseului său sau în caz de intervenire a altor forțe.

Lucrarea tratează COMANDA ROBOTULUI MOB IL CU AUTOECHILIBRARE și
este structurată astfel:

Capitolul 1 – conține generalități despre roboții mobili, tipuri de roboți mobili, structura
roboților mobili, acționarea roboților mobili.

Capitolul 2 -prezintă robotul cu autoechilibrare, modul de comandă a robotului precum
și componentele principale a acestuia.

Capitolul 3 -prezintă placa de dezvoltare Arduino Nano R3 precum și procesorul 328p

Capitolul 4 – prezintă tipurile de limbaje utilizate pentru realizarea robotului precum și
librăriile predefinite care au fost utilizate .

Capitolul 5 – prezintă codul folosit pentru robotul cu autoechilibru, implementarea
anumitor algoritmi pentru citirea datelor de la senzorul de înclinare precum și filtrarea acestor
date pentru diminuarea erorii, controlul motoare lor folosind algoritmul PID pentru realizarea
mai ușoară a echilibrului.

48 Bibliografie

[1]- https://en.wikipedia.org/wiki/Elmer_and_Elsie_(robots)
[2]- http://cyberneticzoo.com/cyberneticanimals/elsie -cyberneticanimals/elsie/
[3]- https://ro.wikipedia.org/wiki/Robotic%C4%83
[4]- https://ro.wikipedia.org/wiki/Robot
[5]- https://www.scribd.com/doc/253119456/Robo%C8%9Bi -mobili -autonomi
[6]-https://science.hotnews.ro/stiri -spatiul -8502082 -video -nasa-detaliaza -macaraua -spatiala –
care-usura -ateriza rea-urmatorului -robot -trimis -marte.htm
[7]- http://www.sinuc.utilajutcb.ro/SINUC -2010/SECTIA –IV/10.IV.8.pdf
[8]- https://ro.wikipedia.org/wiki/Robot_industrial
[9]- https://www.scribd.com/document/149343312/Structura -robotului
[10]- http://www.emie.ugal.ro/doc/me/Capitolul%206.pdf
[11]-https://motoareatestat.weebly.com/motoare -cu-curent -continuu.html
[12]-
http://www.mobilindustrial.ro/current_version/online_docs/COMPENDIU/motoare_de_curen
t_continuu.htm
[13]-http://www.mikroelectronica.ro/2012/01/ce -este-puntea -h/
[14]-https://core.ac.uk/download/pdf/11039073.pdf
[15]-https://www.semanticscholar.org/paper/Mathematical -model -and-control -strategy -of-a-
robot -Mahler -Haase/ba219433ed37c2498bce1e3b43298d7e4dd561a3
[16]-https://hackaday.io/project/7567/components
[17]-
http://www.rovislab.com/courses/br/Curs_07_Sistemul%20senzorial%20al%20unui%20r obot
.pdf
[18]-http://www.roboticmagazine.com/popular/what -is-an-accelerometer
[19]-http://miguelnava s.byethost5.com/2014/09/01/hola -mundo/?i=1
[20]-https://en.wikipedia.org/wiki/Gyroscope
[21]-http://alexandrusalceanu.ro/pdf/curs/capitolul -7-principii -si-tehnici -de-filtrare.pdf
[22]-https://www.chegg.com/homework -help/questions -and-answers/task -5-complementary –
filter -previous -task-students -observed -sensor -output -steady -due-noise -q30630657
[23]-
https://www.academia.edu/6261055/Complementary_Filter_Design_for_Angle_Estimation_u
sing_MEMS_Accelerometer_and_Gyroscope
[24]-https://en.wikipedia .org/wiki/Kalman_filter
[25]-https://en.wikipedia.org/wiki/PID_controller
[26]-https://electrod b.ro/control -turatie -motor -prin-varierea -factorului -de-umplere -pwm/
[27]-http://www.handsontec.com/dataspecs/L298N%20Motor%20Driver.pdf
[28]- https://incdmtm.ro/simoha/wp -content/uploads/2018/12/3. -Senzor -de-
%C3%AEnclinare.pdf
[29]- https://cleste.ro/arduino -nano -v3.html
[30]- https://github.com/TKJElectronics/KalmanFilter
[31]- https://www.autorc.ro/ac cesorii -radiocomenzi/4693 -suport -receptie -4-baterii -aa-cu-
mufa -futaba.html
[32]- https://forum.arduino.cc/index.php?topic=654630.0
[33]- https://components101.com/microcontrollers/arduino -nano
[34]- https://www.arduino.cc/en/tutorial/PWM
[35]- https://ro.wik ipedia.org/wiki/Microcontroler
[36]- https://www.scritub.com/stiinta/informatica/Notiunea -de-microcontroller -De95575.php
[37]- https://www.creeaza.com/tehnologie/electronica -electricitate/Microcontrolere429.php
[38]- https://ro.wikipedia.org/wiki/Atmega328

49 [39]- https://ro.wikipedia.org/wiki/Arduino
[40]-https://www.arduino.cc/reference/en