Documente Academic
Documente Profesional
Documente Cultură
LUCRARE DE LICENŢĂ
Coordonator științific:
Prof. dr. ing. Sergiu Stelian Iliescu
Consultant:
Sl. dr. ing. Iulia Dumitru
Ing. Cătălin Vasile (ITA)
Absolvent:
Vidraşcu Irina-Diana
București
2013
Irina Diana Vidrașcu CUPRINS
CUPRINS
LISTĂ FIGURI
INTRODUCERE 1
1.3.1. Accelerometru 11
1.3.2. Giroscop 13
1.4. Microcontrolere ( ) 16
2.2. Senzori 22
2.3. Microcontroler 27
2.4. Motoare 30
CAPITOLUL IV - CONCLUZII 57
4.1. Concluzii 57
BIBLIOGRAFIE 60
LISTA FIGURILOR
2.17. Cuplaj montare servo pentru direcția “pitch”(a) ; direcția “roll” (b) 32
(sony.ro)
2.18. Cameră video FCB-EX480CP 33
INTRODUCERE
Cea mai comună referinţă folosită de aceste sisteme este Pământul, considerându-se
un sistem de coordonate pe trei axe la care se va raporta sistemul UAV-ului în cauză.
1
Irina Diana Vidrașcu INTRODUCERE
Sistemele de navigaţie inerţială sunt sisteme tridimensionale care indică în timp real
poziţia şi viteza unui vehicul utilizând seturi de date provenite de la senzori (IMU – unităţi
inerţiale de măsurare). Există mai multe categorii de sisteme INS, ele diferenţiindu-se prin
sistemul de referinţă adoptat. Astfel, în cadrul lucrării se va utiliza ca sistem de referinţă local,
vehiculul şi ca sistem de referinţă global, sistemul de navigaţie. Menţionarea sistemului de
referinţă precizează practic faţă de ce sistem se vor raporta măsuratorile făcute cu ajutorul
senzorilor [1].
Principalii senzori folosiţi de aceste sisteme sunt reprezentaţi de accelerometre şi
giroscoape, dar şi magnetometre. Ieşirile senzorilor vor determina poziţia, viteza şi altitudinea
vehiculului. Giroscoapele măsoară viteza unghiulară, iar accelerometrele oferă acceleraţia
datorată tuturor forţelor, cu excepţia forţei gravitaţionale a Pământului.
Condiţii
IMU
Giroscop Poziţie
Procesor
Viteză unghiulară
Accelerometru
Model gravitaţional
Avantajele cele mai importante ale utilizării acestor sisteme INS sunt achiziţia
permanentă a parametriilor, folosirea unei soluţii de navigaţie cu bandă de frecvenţă înaltă
(minim 50Hz), introducerea unor zgomote de scurtă durată.
Însă, dezavantajul în cazul acestor sisteme îl reprezintă degradarea acurateţei de
navigaţie în timp şi dependenţa preciziei de costul senzorilor folosiţi. Apar cumulări ale
erorilor provenite de la măsurătorile accelerometrelor şi giroscoapelor, erori ce se propagă în
special prin intermediul feedback-ului dat de modelul gravitaţional.
Performanţele obţinute pot varia cu câteva ordine de mărime importante, depinzând
foarte mult de calitatea senzorilor inerţali folosiţi. Spre exemplu, sistemele INS folosite de
liniile aeriene comerciale ajung să coste 100,000 €, în timp ce sistemele folosite de avioane
uşoare, arme ghidate, etc. sunt mult mai ieftine, dar oferă performanţe mai slabe cu
aproximativ două ordine de mărime. Industria automobilă este cea pentru care sunt folosiţi
senzorii inerţiali cu cele mai slabe performanţe (de şase ori mai mici decît în industria
marină), aceştia fiind utilizaţi în mod individual, şi nu integraţi ca unităţi IMU. Pentru a
suplini această deficienţă, sunt utilizate laolaltă mai multe tipuri de sisteme de navigaţie (ex:
GNSS – global navigation satellite system) [4].
Navigaţia inerţială este folosită de o serie variată de aplicaţii din industria
aeronautică, în misiuni spaţiale, în navigaţie marină şi submarină. Progresul rapid al
echipamentelor utilizate în sistemele de navigaţie inerţiale permite, mai nou, folosirea
acestora în aplicaţii ce ţin de domeniul mişcării umane şi animale. Senzorii inerţiali (tip
MEMS - micro-machined electromechanical systems) sunt mult mai mici în dimensiuni şi
greutate, permiţând aplicaţii mai minuţioase.
2
Irina Diana Vidrașcu INTRODUCERE
3
Irina Diana Vidrașcu CAPITOLUL I – CONSIDERAȚII TEORETICE
CAPITOLUL I
CONSIDERAŢII TEORETICE
Pentru a obține informații cât mai exacte este foarte importantă alegerea sistemul de
referință cu care se va lucra. În general, se optează pentru un sistem cartenzian de coordonate
cum ar fi:
sistem de referință inerțial – are originea în centrul Pământului, respectă
legile mișcării definite de Newton și nu accelerează, nici nu se poate roti;
sistem de referință al Pământului – are originea în centrul Pământului, iar
axele sunt fixate în raport cu Pământul așa cum este prezentat în figura 1.1
după [4];
Zb Zg
INS
yb
xg
yg
INS
xb
În continuare voi face referire la aceste două tipuri de sisteme, cu accent pe cel ce
încadrează tehnologia folosită de platforma dezvoltată în cadrul licenței, „strapdown” system.
5
Irina Diana Vidrașcu CAPITOLUL I – CONSIDERAȚII TEORETICE
Acest tip de sistem este cel mai vechi dintre cele două, fiind bazat pe o platformă cu
dublă articulație cardanică. Principalele probleme care se ridică sunt izolarea platformei de
mișcăriile de rotație ale vehiculului (dronei), schimbarea permanentă a vectorului gravitație
odată cu poziția și, în timp, din cauza suprafeței sferice a Pământului și a rotației acestuia, în
jurul propriei axe.
Cele trei axe ale dronei pe care au loc mișcările sunt enumerate mai jos, având și o
reprezentare prin figura 1.4 după [4]:
roll (tracțiune) – axa ce unește punctele extreme de pe lungimea aeronavei;
yaw (greutate) – axa verticală;
pitch (portanță) – axa ce unește linia aripilor aeronavei.
6
Irina Diana Vidrașcu CAPITOLUL I – CONSIDERAȚII TEORETICE
Așa cum se poate vedea în reprezentarea din figura 1.7 după [1], pentru a afla
orientarea vehiculului se folosesc unghiurile dintre axurile cardanice. Pentru a calcula poziția
vehiculului, semnalul accelerometrului este integrat de două ori, cu mențiunea că este
necesară eliminarea componentei accelerației datorată gravitației.
Acest tip de sistem inerțial este încă în uz și este folosit pentru aplicații ce necesită un
grad foarte ridicat de acuratețe a datelor de navigație estimate cum ar fi industria navelor și
submarinelor, deoarece minimizează erorile provenite de la mișcările vehiculului gazdă (în
cazul ales, drona este vehiculul gazdă pentru o astfel de platfomă). Mai mult, reduce
încărcarea procesorului (microcontroler-ului folosit), putând fi implementat chiar și calcul
analogic.
7
Irina Diana Vidrașcu CAPITOLUL I – CONSIDERAȚII TEORETICE
Acest tip de sistem este unul analitic și folosește implementare numerică. Este
necesară reținerea unghiului dintre axa accelerometrului și cea a vectorului gravitație, precum
și scăderea componentei vectorului . Ieșirile accelerometrelor sunt proporționale cu
cosinusul vectorului gravitație.
Astfel, dacă se consideră:
(1.2)
În figura 1.9 după [1], se observă în mod exact axele folosite de cele două
echipamente și modul lor de transformare pentru a obține în final poziția și viteza vehiculului.
8
Irina Diana Vidrașcu CAPITOLUL I – CONSIDERAȚII TEORETICE
Cele două tipuri de sisteme inerțiale folosesc, în linii mari, aceleași principii de
funcționare, dar diferă prin felul în care sunt montați senzorii și sistemele de referință alese
față de care se raportează măsuratorile parametrilor.
Sistemele tip “strapdown” reduc foarte mult complexitatea mecanică, sunt mult mai
mici în privință dimensiunilor și chiar mai ieftine, lucru datorat costurilor mici necesare
calculului numeric.
9
Irina Diana Vidrașcu CAPITOLUL I – CONSIDERAȚII TEORETICE
Structuri
miniaturizate
Micro- Componente
senzori MEMS microelectronice
Micro-
actuatoare
1.3.1 Accelerometru
Structura prezentată în figura 1.12 după [4] este incompletă deorece masa inerțială
trebuie să aibe suport și pe axele perpendiculare, pe axele de sensibilitate ale
accelerometrului, necesară fiind și amortizarea oscilațiilor masei, însă acest principiu de
funcționare stă la baza tuturor accelerometrelor.
Accelerometrele folosite în sistemele de tip “strapdown” pot fi:
tip pendul (mecanice);
tip rezonant;
tip MEMS.
Accelerometrele tip pendul sunt compuse dintr-o masă suspendată cu ajutorul unor
arcuri, ca în figură. Deplasarea masei este măsurată prin intermediul unui senzor (“pickoff”)
de deplasare care oferă la ieșire un semnal proporțional cu forța F aplicată masei. Pentru a
calcula accelerația propriu-zisă este utilizat al II-lea principiu fundamental al mecanicii,
.
Accelerometrele de tip rezonant pot fi la rândul lor împărțite în funcție de suprafața
rezonantă folosită :
unde acustice de suparafață (SAW – surface acoustic wave);
vibrante;
11
Irina Diana Vidrașcu CAPITOLUL I – CONSIDERAȚII TEORETICE
ce utilizează siliconul;
ce utilizează cuarțul.
Spre exemplu, accelerometrul de tip SAW, ilustrat mai jos în figura 1.13 după [1],
conține o grindă încastrată în consolă care rezonează la o anumită frecvență. Un capăt al
grinzii este fixat de carcasă, în timp ce cel de-al doilea capăt are atașată masa inerțială.
Atunci când este aplicată o accelerație pe direcția axelor, grinda începe să se îndoaie.
Comprimarea grinzii reprezintă scăderea frecvenței, în timp de dilatarea (întinderea)
reprezintă creșterea frecvenței. Astfel, măsurând frecvența cu care grinda rezonează, se poate
determina forța ce acționeză de-a lungul axelor de sensibilitate ale acceleronetrului.
Performanțele obținute pot fi îmbunătățite dacă se folosește o pereche de grinzi astfel
încât, în timp ce una se comprimă, cealaltă se dilată. În această situație se pot folosi fie una,
fie două mase inerțiale. Un astfel de accelerometru este prezentat în figura 1.14 după [4].
12
Irina Diana Vidrașcu CAPITOLUL I – CONSIDERAȚII TEORETICE
Această structură prezentată mai sus se dublează sau triplează în funcție de numărul
de axe ale accelerometrului, pe fiecare dintre axe fiind posibilă determinarea unei accelerații.
1.3.2 Giroscop
Giroscopul este un aparat care, antrenat de o mișcare de rotație în jurul uneia dintre
axe, se poate deplasa astfel încât să nu modifice direcția axei sale de rotație, iar o a doua
conotație a termenului ar fi de dispozitiv ce asigură stabilitatea și orientarea în spațiu a unui
avion sau submarin (conform DOOM 2005).
Inițial, domeniul giroscopului făcea referire numai la cele ce implicau rotația unei
mase inerțiale, extinzându-se ulterior și incluzând toți senzorii ce măsoară viteze unghiulare și
nu necesită un sistem de referință extern.
Giroscoapele pot fi împărțite în trei categorii principale:
mecanice (care la rândul lor sunt de mai multe tipuri);
optice;
vibrante (MEMS).
Astfel, giroscoapele mecanice sunt de mai multe feluri, dar cel mai des întâlnite sunt
cele ce se bazează pe un rotor aflat în interiorul a două cadre cardanice ce permit rotația pe
toate cele trei axe ca în figura 1.15 după [1]. Are loc conservarea orientării ce se traduce prin
conservarea poziției rotorului atunci când se schimbă orientarea.
Practic, când are loc o rotație, rotorul își păstrează orientarea față de sistemul de
referință global, modificându-se de fapt unghurile dintre cardane. Giroscoapele conveționale
măsoară orientarea, în timp ce cele moderne măsoară viteza unghiulară.
Principalul dezavantaj al giroscoapelor mecanice îl reprezintă existența părților
mobile ce cauzează fricțiune ducând la devieri în timp. De asemenea, ele necesită un anumit
timp pentru a fi puse în funcțiune, ceea ce poate fi un inconvenient în multe situații.
fascicule coerente de lumină, care se propagă în sensuri opuse printr-un inel de fibră optică
sau de oglinzi aflat în rotație. În figura de mai jos, este schematizat acest efect. Linia punctată
reprezintă traiectoria fasciculului pe direcția de rotație, în timp ce linia normală reprezintă
traiectoria fasciculului în sens contrar rotației. De asemenea, unghiul simbolizează unghiul
de rotație al giroscopului cât timp fasciculul acționează.
Avantajul folosirii giroscopului optic constă în faptul că acest nu are părți mobile și
necesită un timp foarte scurt pentru punerea în funcțiune. Precizia datelor oferite este
dependentă de lungimea de undă a radiației utilizate ce poate suferi constrângeri din cauza
dimensiunilor dispozitivului.
Giroscoapele optice se împart funcție de mediul folosit pentru inelul de propagare:
giroscop laser inelar;
giroscop cu fibră optică.
Giroscopul tip laser inelar (RLG – ring laser gyroscope), ilustrat în figura 1.17 după
[4], are la bază același principiu Sagnac, dar se consideră că fasciculul de lumină este
direcționat cu ajutorul unor oglinzi pentru a forma bucla închisă. Există o cavitate activă a
laserului ce are cel puțin trei canale sub forma unui triunghi echilateral care sunt umplute cu
un amestec de gaz He-Ne. Sursa laser genereză două fascicule coerente ce se propagă în
sensuri opuse. Anodul și catodul sunt folosiți pentru a crea o diferență de potențial a
amestecului gazos, generând câmp electric. Pentru a putea măsura și viteze unghiulare foarte
mici, RLG-urile au implementate și o roată oscilantă („dither wheel”) [19].
14
Irina Diana Vidrașcu CAPITOLUL I – CONSIDERAȚII TEORETICE
Giroscopul cu fibră optică (FOG – fiber optic gyroscope), ilustrat în figura 1.18 după
[4], folosește interferența luminii pentru a măsura viteza unghiulară. Acesta conține o buclă
de fibă optică (cerc). Sursa de lumină genereză un fascicul ce este divizat în două fascicule
coerente de sens opus. Viteza unghiulară este determinată conform principiului Sagnac.
Modulatorul de fază are drept scop introducerea unui defazaj, astfel încât fasciculele ce ajung
la detector, folosind cele două traiectorii, să poată fi distinse.
FOG-urile prezintă avantajul costului și al faptului că sunt robuste, fiind folosite cu
preponderență în sistemele ce nu necesită performanțe ridicate. Creșterea lungimii fibrei
determină creșterea preciziei, dar scăderea semnalului de ieșire al giroscopului.
(1.4)
1.4 Microcontrolere ( )
16
Irina Diana Vidrașcu CAPITOLUL I – CONSIDERAȚII TEORETICE
Software
Microcontroler/ PC Elemente de
excuție
(Servomotoare)
Senzori
17
Irina Diana Vidrașcu CAPITOLUL I – CONSIDERAȚII TEORETICE
În aplicații tip UAV, există câteva aspecte ce trebuie considerate atunci când se
optează pentru un anumit model de microcontroler, și anume existența unor caracteristici
hardware care să simplifice interfațarea sistemului cu senzorii și elementele de execuție. Aici
pot fi incluse convertoarele, timere-le, canale de modulație în durată a impulsurilor (PWM).
Câteva exemple de proiecte ce au ca tematică UAV-urile pot indica tipurile de
microcontrolere indicate: Atmel (gama AT90S2313, AT90S8515) au fost utilizate într-un
proiect similar ca cerințe și anume “Microcontroller Systems for UAV; Autopiloting and
camera trigger system” (Alexandros Skafidas, decembrie 2002), PIC 16F877 în
“Development of Unmanned Aerial Vehicle Manual Control System” (Thae Su Aye, Pan Thu
Tun, Zaw Min Naing, and Yin Mon Myint, 2008), PIC 18F8720 în „A Small Semi-
Autonomous Rotary-Wing Unmanned Air Vehicle (UAV)” (Scott D. Hanford, Lyle N. Long,
and Joseph F. Horn, The Pennsylvania State University, University Park, PA).
Proiectarea unui sistem de reglare necesită un efort ciclic, în care se integreză etapele
de modelare, proiectare, simulare, testare și implementare. Un sistem de reglare automată
(SRA) asigură menținerea variabilei reglate la o valoare constantă, impusă prin referința
sistemului. Proiectarea unui SRA presupune: instalația tehnologică, senzori, elemente de
execuție, obiective, comunicații, arhitecturi și interfețe, perturbații, incertitudini și algoritmi
[5].
În aplicațiile industriale, în cadrul structurilor cu unul sau mai multe grade de
libertate, sunt utilizate legi de reglare convenționale sau neconvenționale. Astfel, în ceea ce
privește industria aeronautică, se folosesc cu preponderență algoritmi convenționali, în primul
rând datorită simplității acestora, ușurinței de implementare și a faptului că foarte multe
dintre performanțele impuse sistemelor de zbor se mulează foarte bine pe ceea ce oferă acești
algoritmi. Dar, se constată, din ce în ce mai mult, o tendință de migrare a structurilor de
conducere convenționale către structuri neconvenționale, mai complexe, ce au un grad ridicat
de abstractizare a proceselor conduse. Algoritmii neconvenționali sunt utilizați, în special, în
sistemele în care apar dificultăți majore în caracterizarea matematică riguroasă [5].
Algoritmii convenționali cei mai folosiți sunt: P, PI, PD și PID, precum și variante de
PID – modificați (PI-D, I-PD), iar dintre cei neconvenționali se remarcă sistemele adaptive și
expert, tehnici fuzzy, rețele neurale și algoritmi genetici.
Unele dintre cele mai complexe regulatoare folosite în industrie sunt PID-urile
(proporțional-integral-derivativ), ele asigurând performanțe de reglare superioare. Așa cum îi
spune și denumirea, ele combină efecte proporțional, intergal și derivativ conform cu legea:
unde:
18
Irina Diana Vidrașcu CAPITOLUL I – CONSIDERAȚII TEORETICE
20
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE
CAPITOLUL II
DETALIEREA
PLATFORMEI DEZVOLTATE
În figura prezentată mai jos se poate vizualiza întreaga structură descrisă, avându-se
în vedere că dimensiunile aproximative ale montajului sunt 30 x 15 x 20 cm.
2.2 Senzori
În cadrul sistemului dezvoltat, s-au folosit pentru achiziția datelor doi senzori
inerțiali MEMS, aleși în concordanța cu microcontroler-ul, dar și cu cerințele specificate,
aceștia oferind avantajul dimensiunii reduse. În figura 2.2 după [7] s-a dorit evidențierea
structurii miniaturizate de la nivelul componentelor senzoriilor MEMS pentru a înțelege cum
aceștia reușesc să atingă dimensiuni foarte reduse.
Astfel, s-a mers pe varianta combinării unui accelerometru și a unui giroscop, ambele
oferind date pe trei direcții.
22
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE
Senzorii pentru care s-a optat sunt un accelerometru ADXL 345 Breakout și un
giroscop ITG-3200 Breakout, ambele produse de Sparkfun. Menționez că aceste dispozitive
mi-au fost puse la dispoziție în urma stabilirii specificațiilor proiectului, acestea fiind
suficiente pentru atingerea obiectivelor propuse, urmând ca ulterior să existe opțiunea
înlocuirii lor cu unele mai performante.
Acești senzori oferă game de valori suficient de largi pentru aplicație și potrivite
pentru tipul de proiect ce implică stabilizare în aer a sistemului.
ADXL 345 Breakout este un echipament de înaltă precizie și calitate ridicată, dar și
consum mic de putere.
Fiind un accelerometru digital ce măsoară accelerația pe toate cele 3 axe (X, Y, Z),
de mici dimensiuni (3 x 4 x 1 mm), are o rezoluție fixă de 10 biți cu posibilitatea de extindere
până la 13 biți în cazul în care se utilizează gama de măsurare de ±16 g. Un aspect important
care a dus la alegere sa este și folosirea de către acesta a magistralei de comunicație I2C, dar și
SPI. Ieșirea pe care acest dispozitiv o oferă este una pe 2 octeți complementari.
Echipamentul dispune și de funcționalități speciale cum ar fi detecția prezenței sau
lipsa mișcării, prin compararea accelerației citite cu un prag setat de utilizator. De asemenea,
este capabil să detecteze lovituri simple sau duble (“taps”), precum și căderi în gol. Toate
acestea pot fi configurate cu ajutorul pinilor de întrerupere de care dispune senzorul.
23
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE
Un alt aspect îl reprezintă sensibilitatea senzorului care, în acest caz, este de 256
LBS/g pe fiecare dintre cele 3 axe. Asta înseamnă că valoarea de la ieșirea giroscopului va fi
divizată prin această valoare pentru a obține rezultatele scalate.
24
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE
În privința giroscopului, s-a ales pentru acest proiect ITG-3200 Breakout deoarece
acest senzor indică, la ieșirea sa digitală, viteza unghiulară pe cele 3 axe ca în figura 2.6 după
[16]. Este un chip folosit în aplicații multiple de control remote, controlere pentru jocuri,
echipamente de monitorizare sportivă.
Acesta este tot un senzor de tip MEMS (4 x 4 x 0.9 mm) ce folosește 3 convertoare
ADC de 16 biți pentru calcularea ieșirii, iar ca magistrală de comunicație, I2C. El are inclus și
un senzor de temperatură în structura sa. De asemenea, calibrarea este redusă foarte mult prin
stabilitatea sensibilității la temperatură și înclinație.
Sensibilitatea senzorului este de 14.375 LSB per grade/sec, dispunând de o scală de
măsură între -2000 și +2000 de grade/sec. În structura sa este inclus și un filtru trece-jos
configurabil.
25
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE
Valorile de la ieșire giroscopului sunt date digitale, fără semnificație fizică. Pentru a
obține date care să ajute ulterioarele prelucrări din sistem, le vom scala cu ajutorul factorului
de sensibilitate.
(2.2)
Inițializările necesare ale regiștriilor în cazul giroscopului sunt prezentate mai jos:
verifică identitate device
char WHO_AM_I = 0x00;
determină rata de eșantionare
char SMPLRT_DIV= 0x15;
selectare scalei maxime de măsurare
char DLPF_FS = 0x16;
date axa X pe byte low și high
char GYRO_XOUT_H = 0x1D;
char GYRO_XOUT_L = 0x1E;
date axa Y pe byte low și high
char GYRO_YOUT_H = 0x1F;
char GYRO_YOUT_L = 0x20;
date axa Z pe byte low și high
char GYRO_ZOUT_H = 0x21;
char GYRO_ZOUT_L = 0x22;
configurare frecvență 42 Hz pentru FTJ inclus în giroscop și configurare
frecvență internă 1kHz
char DLPF_CFG_0 = 1<<0;
char DLPF_CFG_1 = 1<<1;
char DLPF_CFG_2 = 1<<2;
char DLPF_FS_SEL_0 = 1<<3;
char DLPF_FS_SEL_1 = 1<<4;
adresa dispozitiviului I2C
char itgAddress = 0x69;
26
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE
2.3 Microcontroler
Figura 2.9 Vedere de sus (a) Vedere spate (b) Arduino Duemilanove
Așa cum este menționat anterior, mediul de dezvoltare este unul open-source, ușor de
utilizat, cum se poate vedem în cele ce urmează. Pentru realizarea unui nou proiect se
deschide o noua fereastră și, cel mai important, din Tools > Board se selectează
microcontroler-ul folosit. Odată cu conectarea Arduino, se poate vedea portul pe care acesta
în folosește pentru comunicația cu PC-ul.
În scrierea unui program (sketch) există câteva secțiuni care nu trebuie să lipsească.
Este necesară includerea librăriilor ce se vor utiliza, declararea variabilelor auxiliare folosite,
declararea constantelor, dacă există, și două secțiuni:
setup() – se execută o singură dată la deschiderea portului de comunicație,
este folosită pentru inițializări;
loop() – conține programul principal, rulează pe microcontroler în mod
repetat.
28
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE
Pe lângă funcțiile de bază care nu pot lipsi, se pot declara funcții auxiliare. Acestea
se declară ca orice funcție, începând cu header și terminând cu definiția acesteia.
În cazul de față, Arduino IDE este folosit pentru a implementa următoarele
funcționalități ale proiectului:
ARDUINO
29
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE
2.4 Motoare
Acest servomotor are dimensiunile standard ale unui astfel de motor. Pe lângă
angrenajul metalic, dispune și de doi rulmenți care ajută la obținerea cuplului. În kit sunt
incluse și extensiile ce permit fixarea lui în cadrul unui montaj mecanic.
Dimensiunile sale respectă cerințele: 40.7 x 20.5 x 39.5 mm, având o greutate de 60g
per servomotor. Power HD 1501 MG folosește modulația analogică (Pulse Width
Modulation). În cadrul acestei tip de modulații este foarte importantă lățimea impulsului care
în acest caz particular pot varia între 500-2100 . Se ia în considerare și rata de refresh a
unui ciclu PWM, care aici este de 20 ms. Aceste aspecte sunt deosebit de importante, ele fiind
necesare la reconfigurarea librăriei Arduino.
30
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE
31
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE
Celelalte două motoare sunt cuprinse în cadrul interior al montajului, fiecare dintre
ele necesitând modificări la nivelul cuplajului dintre axul servomotorului și partea mecanică
ce trebuie acționată.
Figura 2.17 Cuplaj montare servo pentru direcția “pitch”(a) ; direcția “roll” (b)
32
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE
33
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE
ACCELEROMETRU
Filtru Trece – Jos
(FTJ) Unghi de
rotație
Integrare
Numerică
Viteză
GIROSCOP unghiulară
34
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE
Integrarea numerică presupune că din viteză (v) integrată obțin poziție, iar din viteză
unghiulară ( ) integrată obțin o rată unghiulară (unghi).
(2.3)
gyroRateX=-1.0f*dtime*xg;
gyroRateY=dtime*yg;
gyroAngleZ_dt=dtime*zg;
Totuși, pentru realizarea finală a proiectului s-a ales implementarea unui filtru
Kalman. Acesta se bazeză pe reducerea erorii în urmărire și folosește predicția unei valori
viitoare plecând de la o valoare curentă și anterioară. Acestă nouă predicție suferă apoi o
corecție. Noua valoare trebuie să se regăsescă într-un interval definit de valoarea predicată și
cea măsurată.
În acest mod se pot reduce erorile și zgomotele introduse în sistem. Dar, de-a lungul
timpului, există totuși erori cumulative. Pentru sistemul descris aici, se recomandă
combinarea datelor accelerometrului și giroscopului, cele două fiind capabile prin intermediul
filtrului Kalman de a-și compesa erorile.
35
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE
(2.4)
Implementarea ecuațiilor este prezentată mai jos sub forma utilizată pentru
implementare:
p_x_acc = p_x_acc + q_x_acc;
k_x_acc = p_x_acc / (p_x_acc + r_x_acc);
x_acc_f = x_acc_f + k_x_acc * (xa - x_acc_f);
p_x_acc = (1 - k_x_acc) * p_x_acc;
36
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE
k+1
Estimare
nouă
Alegerea acestui filtru se dovedește a fi cea mai bună, întrucât este cel mai complex
dintre toate tipurile de filtre pentru că încorporează toate măsurătorile, indiferent de precizia
lor și îmbunătățește considerabil acuratețea sistemului [13].
În cea de-a doua fotografie se poate vedea și sistemul de coordonate folosit, putând fi
vizualizate cele trei axe (X, Y și Z). Această simulare a fost făcută înaintea proiectării practice
a montajului cu scopul de a stabili niște dimensiuni potrivite ale pieselor componente.
37
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE
În cadrul montajului există câteva aspecte importante de care s-a ținut cont și anume,
faptul că senzorii trebuie montați la unison unul cu celălalt, pentru ca acuratețea mișcăriilor
realizate pe baza datelor achiziționate de la ei să fie cât mai mare.
Pentru o mai bună montare, s-a ales varianta folosirii unui breadboard pe care sunt
montați senzorii, dar care este folosit și pentru realizarea conexiunilor auxiliare ale
microcontroler-ului. Pe plăcuța breadboard-ului au fost scose conexiuni ale pinilor AN4 și
AN5 ale Arduino (SDC și SCL) pentru a-i putea oferi ca intrări ambilor senzori. Dat fiind
faptul ca Arduino dispune de un singur pin de tensiune 3V3, s-a procedat în aceeași manieră
pentru a obține 2 intrari de 3V3.
38
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE
39
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE
M1 M2 M3
YAW PITCH ROLL
Y X Z
O
X
O 1800
Z Y
00 1800
1600
00
500
O observație de care trebuie ținută seama este acordarea unei atenții permanente
motorului montat pe direcția “roll” deoarece este mai vulnerabil la comenzi bruște. Mișcarea
de rotație este transformată în mișcare de translație printr-o articulație tip bielă-manivelă
foarte sensibilă.
40
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE
SETARE POZIȚIE
INIȚIALĂ
CALCUL
CALCUL
COMENZII
ÎNCADRARE NU LIMITARE
COMANDĂ MINIMĂ/MAXIMĂ A
SERVO COMENZII
DA
PRIMIRE COMENZI
SERVO
FEEDBACK
Alegerea unui algoritm de reglare portrivit unei anumite aplicații trebuie să țină
seama de câteva aspecte deosebit de importante cum ar fi:
timpul mort și constanta de timp a sistemului;
eroare staționară admisă ( );
timpul tranzitoriu;
gradul de stabilitate al sistemului;
costul maxim admis.
Date fiind aceste criterii și tipul de proces pentru care se dorește calcularea unui
regulator (platforma), cel mai bine, din punct de vedere teoretic, ar funcționa PID-ul.
Controlul proporțional-integrator-derivativ (PID) este o metodă folosită la scară largă
pentru a atinge și menține o referință a unui proces de orice fel. Principiul de bază rămâne
neschimbat, indiferent de tipul de proces.
Forma standard a comenzii unui PID este:
41
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE
42
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE
În cele ce urmează este prezentată secvența de cod prin care se face ajustarea acestor
factori ai regulatoarelor tip P:
alfa+=0.3
beta+=0.1
gama+=0.3
În capitolul III este prezentată explicit maniera în care are loc testarea acestui tip de
regulator, studiul de caz pe care se bazează practic rezultatele obținute.
43
Irina Diana Vidrașcu CAPITOLUL III - REZULTATE
CAPITOLUL III
REZULTATE
În acest capitol voi detalia rezultatele obținute în urma testelor efectuate. Dată fiind
împărțirea proiectului pe etape și momentele diferite la care au fost realizate anumite secvențe
de program, rezultatelor le sunt atribuite secțiuni individuale.
Rezultatele sunt prezentate, în cea mai mare parte, sub forma unor grafice Matlab.
S-a dorit ilustrarea unor mișcări simple până la complexe. Sunt reprezenatate rezultate
obținute în urma testelor efectuate cu senzorii în mod individual, dar și montați la unison pe
montaj. Datele pe care se bazeză toate aceste grafice au fost achiziționate prin intermediul
portului serial al PC-ului ce primește date de la microcontroler-ul Arduino.
44
Irina Diana Vidrașcu CAPITOLUL III – REZULTATE
Se poate observa că, deși mișcările sunt unele aleatoare, variația datelor achiziționate
reflectă multe date parazite, afectate de erori și care nu fac decât să se propage în întreaga
implementarea. Din grafic se observă și gama pe care accelerometrul o folosește pentru datele
măsurate, aceasta fiind ± 2g.
Pentru a evita problemele apărute din cauza acestor erori de măsura, s-a apelat la
implementarea folosind filtre.
În etapa inițială s-a încercat implementarea unui filtru complementar însă acest s-a
dovedit a nu fi suficient de performant.
Astfel, opțiunea imediat următoare a fost folosirea filtrului Kalman. S-au studiat mai
multe tipuri de mișcări luând drept exemplu câteva cazuri particulare pentru a demonstra
eficiența acestei metode introduse suplimentar. Au fost efectuate teste pe fiecare plan
individual al accelerometrului. Astfel, s-a considerat o mișcare complet aleatoare pe toate trei
axele de coordonate pentru a vedea care este comportarea senzorului.
45
Irina Diana Vidrașcu CAPITOLUL III – REZULTATE
Atunci când senzorul este staționar, valorile afișate nu sunt zero absolut, au un
anumit offset. Se remarcă, de asemenea, că datorită existenței accelerației gravitaționale, axa
pe care se regăsește va indică mereu o valoare aproximativ egală cu 1 g. Se observă în figura
3.4 momentele în care z ≈ 0 ceea ce înseamnă fixarea accelerometrului având axa Z în plan
orizontal.
46
Irina Diana Vidrașcu CAPITOLUL III – REZULTATE
Același test s-a efectuat și pentru deplasare pe axa Y, dar spre dreapta, în sensul
pozitiv ale axei.
Comparând cele două cazuri, se pot observa diferențele apărute pe axa Y, în cazul
figurii 3.5 (spre stânga) se observă o majoritate a valorilor negative, în timp ce în figura 3.6
(spre dreapta), după filtrare, sunt preponderente valorile pozitive.
De remarcat în cele două figuri sunt axele X și Z. Axa X suferă câteva modificări,
deși nu ar fi trebuit, deoarece ea acționează în același plan cu axa Y. Axa Z prezintă în ambele
situații valori apropiate de 1g, așa cum este și normal datorită accelerației gravitaționale. O a
47
Irina Diana Vidrașcu CAPITOLUL III – REZULTATE
treia situație în plan orizontal este o deplasare a sistemului pe diagonala unui pătrat, situație ce
simulează o mișcare compusă pe axele X și Y.
48
Irina Diana Vidrașcu CAPITOLUL III – REZULTATE
Atenuare spike-uri
49
Irina Diana Vidrașcu CAPITOLUL III – REZULTATE
Din figură se poate observa că, deși mișcările sunt unele aleatoare, variația datelor
achiziționate reflectă multe date parazite, există erori care nu fac decât să se propage în
întreaga implementarea. Se remarcă și o gamă de variație a valorilor giroscopului care, pentru
reprezentarea de față, este de ( ) grade/s.
Mare parte dintre erori pot fi evitate dacă se aplează la folosirea unui filtru.
Giroscopul ITG-3200 Breakout dispune totuși de un filtru trce-jos încorporat, însă acesta nu
este suficient pentru a evita fluctuațiile senzorului. În implementarea proiectului s-a introdus o
metodă suplimentară de filtrare, filtrul Kalman. Inițial s-a încercat utilizarea unui filtru
complementar, dar atenuarea pe care acest o oferea nu era suficientă.
Astfel, ca și la accelerometru, testarea giroscopului și a corectitudinii datelor s-a
realizat pe mai multe tipuri de mișcări, prima dintre ele fiind o mișcare aleatoare.
Mai jos sunt ilustrate rezultatele obținute pe fiecare dintre cele trei axe de
coordonate:
Se observă reprezentarea simultană a datelor brute și a celor cărora le-a fost aplicat
un filtru. Pentru axa X, datele filtrate urmăresc aliura celor provenite direct de la senzori ceea
ce înseamnă că erorile în acest caz nu sunt foarte mari. În cazul spike-rilor apar totuși
diferențe sesizabile, iar filtrul își face efectul.
Gama de variație a accelerației unghiulare în această situație a crescut, ea putând
varia în cadrul testelor în gama maximă admisibilă definită de producătorul senzorului. Se iau
în considerare și faptul că pentru stabilizare vor fi utilizate accelerații nu foarte mari, însă în
practică accelerațiile pot atinge valori ridicate dacă ne referim la o perturbație a mișcăriilor
aerului (vânt) care are un model neliniar și este foarte dificilă de compensat.
Aceleași concluzii sunt valabile și pe axele Y și Z. Pentru axa Y se observă
diferențele celor două situații, cu date nefiltrate și filtrate.
50
Irina Diana Vidrașcu CAPITOLUL III – REZULTATE
În cazul axei Z se poate menționa faptul că, pentru această mișcare aleatoare, valorile
filtrului diferă considerabil ceea ce implică precauții și o atenție sporită la mișcăriile pe
orizontală (axa Z) ale giroscopului.
În cazul unor mișcării mai precise s-a considerat întâi mișcarea pe orizontală, axa Z.
Au fost luate două cazuri separate și anume, mișcarea spre stânga și cea spre dreapta pentru a
evidenția mai exact sensurile și semnele datelor achiziționate.
51
Irina Diana Vidrașcu CAPITOLUL III – REZULTATE
52
Irina Diana Vidrașcu CAPITOLUL III – REZULTATE
În cazul figurii 3.17 este reprezentată o situație asemănătoare cu cea a rotirii în jurul
axei X.
O observație de luat în seamă este faptul că rotirile în jurul axelor X și Y au fost
realizate fără un suport precis al axei de referință (așa cum este cazul lui Z), astfel putând fi
introduse erori considerabile de poziționare a senzorului. Din această cauză, valorile de la nici
53
Irina Diana Vidrașcu CAPITOLUL III – REZULTATE
una dintre axe nu pot fi considerate zero (normal ar fi ca celelalte două axe care nu sunt
implicate în rotație să aibă valori ≈ 0 grade/s).
10 cm
10 cm
proporționaliate foarte mic. În plus, pe acest grafic, se observă o urmărire parțială a referinței
impuse și anume poziția mediană a tutoror servomotoarelor tradusă prin:
yaw = 90º;
pitch = 90º;
roll = 90º.
S-au mai considerat încă două situații posibile având aceiași parametrii. Astfel,
aceste sunt ilustrate mai jos:
55
Irina Diana Vidrașcu CAPITOLUL III – REZULTATE
Având drept scop reglare pe direcția orizontală, parametrii cu care se lucrează sunt
axele X și Y ale accelerometrului și axa Z a giroscopului. Astfel, aceștia reprezintă mărimile
funcție de care se stabilește o eroare și se calculează noua comandă pentru fiecare servomotor
în parte.
Se constată, în urma testelor, că există situații în care comanda a cel puțin unui motor
se poate satura din cauza măsurătorilor eronate ale senzorilor, dar și a simplității regulatorului
care acumulează erori, astfel încât eroare staționară crește progrseiv.
56
Irina Diana Vidrașcu CAPITOLUL IV - CONCLUZII
CAPITOLUL IV
CONCLUZII
4.1 Concluzii
57
Irina Diana Vidrașcu CAPITOLUL IV - CONCLUZII
58
Irina Diana Vidrașcu CAPITOLUL IV - CONCLUZII
59
Irina Diana Vidrașcu BIBLIOGRAFIE
BIBLIOGRAFIE
[4] Groves D. Paul. 2008. Principles of GNSS, inertial and multisensor integrated
navigation systems. p. 3-159. Artech House
[7] Varesano F. Aprilie 2011. Using Adruino for tangible human computer interaction.
Departement of informatics, Univerity of Torino
[8] Atif M, Serdaroglu S. 2011. A measurement system for human movement analysis.
Chalmers University of Technology, Sweden
[10] PID Control: A brief introduction and guide using Arduino. Septembrie 2011. Jacobs
School of Engineering. Disponibil pe
http://www.maelabs.ucsd.edu/mae156alib/control/PID-Control-Ardunio.pdf
[12] Baker W. Ianuarie 2013. Kalman sense. Digital processing of gyroscope and
accelereometer measurements
[14] Colton S. Iunie 2007. The balance filter. A simple solution for integrating
accelerometer and gyroscope measurements for a balancing platform. Disponibil pe:
http://web.mit.edu/scolton/www/filter.pdf
60
Irina Diana Vidrașcu BIBLIOGRAFIE
[17] Ettinger S., Nechybal P., Ifju G., Waszak M. Vision-Guided Flight Stability and
Control for Micro Air Vehicles. Departament of electrical and computer engineering,
Departament of aerospace engineering, mechanics and engineering, University of
Florida. Dynamics and control branch, NASA Langley Research Center
[18] Kim J.H., Wishart S., Sukkarieh S. 2006. Real-time navigation, guidance and control
of a UAV using low-cost sensors. Australian Centre for Field Robotics, University of
Sydney
[20] Specificații și cerințe – temă proiect licență date de ITA (Institutul de Tehnologii
Avansate)
[24] Strickland J. What is a gimbal - and what does it have to do with NASA? Disponibil
pe: http://science.howstuffworks.com/gimbal1.htm accesat la data 20.05.2013
[25] Basta O.P. Mai 2012. Quad Copter Flight. California State University, Northridge
[29] Kada B., Ghazzawi Y. 2011. Robust PID Controller Design for an UAV. World
Congress on Engineering and Computer Science, Vol II, San Francisco, USA
61
Irina Diana Vidrașcu ANEXĂ
ANEXĂ
În cele ce urmeză este listat scriptul codului folosit pentru relizarea proiectului,
program elaborat în Arduino IDE.
Introducerea codului are alăturată și comentarii făcute la momentul întocmării pentru
a facilita ulteriorele revizuiri ale acestuia și a face înțelegerea lui mai ușoară.
#include <Servo.h>
#include <Wire.h>
// Cabling for i2c using Sparkfun breakout with an Arduino Uno /
Duemilanove:
// Arduino <-> ADXL 345 Breakout board
// Gnd - GND
// 3.3v - VCC
// 3.3v - CS
// Analog 4 - SDA
// Analog 5 - SLC
// Arduino <-> ITG-3200 Breakout board
// Gnd - GND
// 3.3v - VCC
// 3.3v - CS
// 20 - SDA
// 21 - SLC
//accel
#define DEVICE (0x53) //adresa accel
//gyro
char itgAddress = 0x69;//adresa gyro
byte _buff[6];
62
Irina Diana Vidrașcu ANEXĂ
Servo servoY;
Servo servoP;
Servo servoR;
float kP=-0.1591;
float kI=-0.0157;
float kD=0.0029; //de test
//Kp: 19.56 Ki: 0.71 Kd: 134.26
//calcul pozitie
float accel[3]={0,0,0};
float velocity[3]={0,0,0};
float displacement[3]={0,0,0};
float time;
float newTime;
float deltaTime;
float alfa=1; //proportinal factor scalare motor Y
float beta=1;
float gama=1;
//filtru accel
double x_acc_f=1; //val filrata
double p_x_acc=1; //eroare estimata
double q_x_acc=0.4; //zgomot proces
double r_x_acc=0.5; // zgomot senzor
double k_x_acc=1; //castig kalman
63
Irina Diana Vidrașcu ANEXĂ
//filtru gyro
double x_gyro_f=1; //val filrata
double p_x_gyro=1; //eroare estimata
double q_x_gyro=0.4; //zgomot proces
double r_x_gyro=0.5; // zgomot senzor
double k_x_gyro=1; //castig kalman
float
v[12]={0.05,0.06,0.07,0.10,0.11,0.12,0.13,0.27,0.28,0.29,0.14,0.08};
int i=1;
void setup()
{
Wire.begin();
Serial.begin(115200);
Serial.print("init");
delay(300);//incalzire
//put the ADXL345 into +/- 4G range by writing the value 0x01 to
the DATA_FORMAT register.
writeTo(DATA_FORMAT, 0x00);
//put the ADXL345 into Measurement Mode by writing 0x08 to the
POWER_CTL register.
writeTo(POWER_CTL, 0x08);
char id=0;
id = itgRead(itgAddress, 0x00);
Serial.print("ID: ");
Serial.println(id, HEX);
servoY.attach(9);
64
Irina Diana Vidrașcu ANEXĂ
servoP.attach(10);
servoR.attach(11);
servoY.write(90);
servoP.write(90);
servoR.write(90);
time = millis();
void loop()
{
readAccel();
readGyro();
if((i!=1)&&(i!=2)&&(i!=3)){
readAccel();
readGyro();
//printValoriAccGyro();
//printUtil();
//calcul comanda dpls stg-dr
//boolean ok=true;
//for(int i=0;i<12;i++){
// if(v[i]==y_acc_f) ok=false;
//}
if((alfa>=0)&&(alfa<=2)){
if(y_acc_f<0.0){ alfa-=0.3;
}else if((y_acc_f>0.04)){
alfa+=0.3;
}
servoY.write(alfa*90);
}
if((beta>=0.6)&&(beta<=2)){
if(x_acc_f<0.0){
beta+=0.1;
}else if(x_acc_f>0.06){
if(beta>=0.7)beta-=0.1;//limitare motor P
}
servoP.write(beta*90);
}
if((gama>=0)&&(gama<=2)){
if(z_gyro_f<0.0){
gama-=0.3;
}else if(z_gyro_f>11.0){
gama+=0.3;
}
servoR.write(gama*90);
}
Serial.print(y_acc_f);
Serial.print('\t');
Serial.print(alfa);
Serial.print('\t');
Serial.print(alfa*90);
Serial.print('\t');
Serial.print(x_acc_f);
65
Irina Diana Vidrașcu ANEXĂ
Serial.print('\t');
Serial.print(beta);
Serial.print('\t');
Serial.print(beta*90);
Serial.print('\t');
Serial.print(z_gyro_f);
Serial.print('\t');
Serial.print(gama);
Serial.print('\t');
Serial.print(gama*90);
Serial.println('\t');
}
i=i+1;
//Serial.println(i);
delay(100);
}
void printUtil(void){
Serial.print(x_acc_f);
Serial.print('\t');
Serial.print(y_acc_f);
Serial.print('\t');
Serial.print(z_gyro_f);
Serial.println('\t');
}
void printEr(void){
for(int i=0;i<2;i++){
// Serial.print("viteza ");
// Serial.print(newSpeed[i]);
// Serial.print('\t');
//
Serial.print("poz: ");
Serial.print(newDisplacement[i]);
Serial.print('\t');
Serial.print("Er: ");
Serial.print(er[i]);
Serial.print('\t');
}
Serial.println("");
//Serial.print("v_u ");
//Serial.print();
}
void readAccel() {//g
uint8_t howManyBytesToRead = 6;
readFrom( DATAX0, howManyBytesToRead, _buff); //read the
acceleration data from the ADXL345
delay(100);
}
Wire.beginTransmission(DEVICE);
Wire.requestFrom(DEVICE, num); // request 6 bytes from device
int i = 0;
while(Wire.available()) // device may send less than
requested (abnormal)
{
_buff[i] = Wire.read(); // receive a byte
i++;
}
Wire.endTransmission();
}
Wire.beginTransmission(address);
67
Irina Diana Vidrașcu ANEXĂ
}
Wire.endTransmission();
return data; //return the data read during the operation
}
int readX(void)
{
int data=0;
data = itgRead(itgAddress, GYRO_XOUT_H)<<8;
data |= itgRead(itgAddress, GYRO_XOUT_L);
return data;
}
int readY(void)
{
int data=0;
data = itgRead(itgAddress, GYRO_YOUT_H)<<8;
data |= itgRead(itgAddress, GYRO_YOUT_L);
return data;
}
int readZ(void)
{
int data=0;
data = itgRead(itgAddress, GYRO_ZOUT_H)<<8;
data |= itgRead(itgAddress, GYRO_ZOUT_L);
return data;
}
void readGyro(){//grade/secunda
int xRate, yRate, zRate;
xRate=readX();
yRate=readY();
68
Irina Diana Vidrașcu ANEXĂ
zRate=readZ();
xg=xRate/14.375f;
yg=yRate/14.375f;
zg=zRate/14.375f;
delay(100);
}
void alg_PID(void){
actualY = analogRead(9);
actualP = analogRead(10);
actualR = analogRead(11);
errorY=setY-actualY;
errorP=setP-actualP;
errorR=setR-actualR;
//?????
if (abs(errorY) < IntThresh){ // prevent integral 'windup'
integralY = integralY + errorY; // accumulate the error
integral
}
else {
integralY=0; // zero it if out of bounds
}
if (abs(errorP) < IntThresh){
integralP = integralP + errorP;
}
else {
integralP=0;
}
if (abs(errorR) < IntThresh){
integralR = integralR + errorR;
}
else {
integralR=0;
}
driveY=errorY*kP+integralY*kI+(lastY-actualY)*kD;
driveP=errorP*kP+integralP*kI+(lastP-actualP)*kD;
driveR=errorR*kP+integralR*kI+(lastR-actualR)*kD;
69
Irina Diana Vidrașcu ANEXĂ
if ((abs(driveY)>255)) {
driveY=255;
}
if ((abs(driveP)>255)) {
driveP=255;
}
if ((abs(driveR)>255)) {
driveR=255;
}
servoY.write(driveY);
servoP.write(driveP);
servoR.write(driveR);
//analogWrite (Motor,Drive); // send PWM command to motor board
lastY=actualY;
lastP=actualP;
lastR=actualR;
void printValoriAccGyro(void){
//accel
Serial.print("Acc: ");
Serial.print(xa);
Serial.print('\t');
Serial.print(x_acc_f);
Serial.print('\t');
Serial.print(ya);
Serial.print('\t');
Serial.print("yf");
Serial.print(y_acc_f);
Serial.print('\t');
Serial.print(za);
Serial.print('\t');
Serial.print(z_acc_f);
Serial.print('\t');
//gyro
Serial.print("Gyro: ");
Serial.print(xg);
Serial.print('\t');
Serial.print(x_gyro_f);
Serial.print('\t');
Serial.print(yg);
70
Irina Diana Vidrașcu ANEXĂ
Serial.print('\t');
Serial.print(y_gyro_f);
Serial.print('\t');
Serial.println(zg);
Serial.print('\t');
Serial.print(z_gyro_f);
Serial.println('\t');
}
void ramasite(void){
newTime = millis();
deltaTime = newTime - time;
//studiu de caz al miscarii functie de y_acc_f (orizontal)
//newAccel[0]=x_acc_f; //xa
//newAccel[1]=y_acc_f; //ya
//newGyro=z_gyro_f;
// if(y_acc_f>0.03){
// alfa=alfa-0.3;
// }else if(y_acc_f<0){
// alfa=alfa+0.3;
// }
// servoY.write(alfa*90);
//newAccel[2]=z_acc_f;
//calcul pozitie pe y
void filter(){
gyroRateX=-1.0*dtime*xg;// calcul e(t)=integrala(viteza
unghiulara)
gyroRateY=dtime*yg;
gyroAngleZ_dt=dtime*zg;
//gyroAngleZ +=-1.0*gyroAngleZ_dt*(1/(cos(Deg2Rad*Roll)))
// if(GyroAngleZ<0) GyroAngleZ+=360;
// if(GyroAngleZ>=360) GyroAngleZ-=360;
Serial.print("GyroRateX: ");
Serial.println(gyroRateX);
71
Irina Diana Vidrașcu ANEXĂ
Cod Matlab :
function afisare_a
fid=fopen('misc_vert.txt','r');
%read data from file
A=textscan(fid,'%f%f%f%f%f%f');
x_a=A{1};
x_a_f=A{2};
y_a=A{3};
y_a_f=A{4};
z_a=A{5};
z_a_f=A{6};
acc=[x_a,x_a_f,y_a,y_a_f,z_a, z_a_f];
fclose(fid);
t=0:1:214;
%t=t(1:116);
figure(1);
title('Accelerometru deplasare spre stanga');
subplot(3,1,1);
plot(t,x_a,'r');
xlabel('timp [s]');
ylabel('Acceleratie_x [g]');
hold on
plot(t,x_a_f,'b');
hold on
legend('x','x(filtrat)','Location','EastOutside');
title('Accelerometru pe x functie de timp');
figure(2);
subplot(3,1,2);
plot(t,y_a,'r');
xlabel('timp [s]');
ylabel('Acceleratie_y [g]');
hold on
plot(t,y_a_f,'b');
hold on
legend('y','y(filtrat)','Location','EastOutside');
title('Accelerometru pe y functie de timp');
figure(3);
subplot(3,1,3);
plot(t,z_a,'r');
xlabel('timp [s]');
ylabel('Acceleratie_z [g]');
hold on
plot(t,z_a_f,'b');
hold on
legend('z','z(filtrat)','Location','EastOutside');
title('Accelerometru pe z functie de timp');
72