Proiectarea unei structuri robotice ce se echilibrează dinamic pe o sferă [301886]

UNIVERSITATEA POLITEHNICA din BUCUREȘTI

Facultatea de Inginerie Mecanică și Mecatronică

Departamentul de Mecatronică și Mecanică de Precizie

Proiectarea unei structuri robotice ce se echilibrează dinamic pe o sferă

Conducător științific:

Ș.l. dr. ing. Viorel Gheorghe

Absolvent: [anonimizat]

1. Introducere……………………………………………………………………………………………………………..1

2. Stadiul actual al structurilor robotice echilibrate dinamic pe o sferă……………………………….3

2.1 Ballbot……………………………………………………………………………………………………….3

2.2 BallIP…………………………………………………………………………………………………………6

2.3 Ballbot Lego……………………………………………………………………………………………….8

2.4 Rezero………………………………………………………………………………………………………..9

2.5 Robot miniatural echilibrat pe o sferă…………………………………………………………..10

2.6 Robot cu motor de inducție sferic………………………………………………………………..11

2.7 Concluzii………………………………………………………………………………………………….11

3. Soluția proprie……………………………………………………………………………………………………….13

4. Memoriu tehnic……………………………………………………………………………………………………..19

4.1 Algoritmul de echilibrare……………………………………………………………………………19

4.2 Algoritmul de deplasare……………………………………………………………………………..21

4.3 Simularea contactului dintre o roată și sferă………………………………………………….24

5. [anonimizat]………………………………………………………..27

5.1 Realizarea structurii mecanice……………………………………………………………………..27

5.2 Realizarea blocului electronic……………………………………………………………………..36

5.4 Testarea structurii robotice obținute……………………………………………………………..42

6. Eficiența economică……………………………………………………………………………………………….46

6.1 Prezentare generale…………………………………………………………………………………….46

6.2 Calcularea cheltuielilor materiale…………………………………………………………………47

6.3 Calcularea cheltuielilor cu resursele umane…………………………………………………..49

6.4 Calcularea amortizării echipamentelor………………………………………………………….50

6.5 Calcularea profitului…………………………………………………………………………………..50

7. [anonimizat]. Concluzii………………………………………………………52

Bibliografie………………………………………………………………………………………………………………53

Anexe……………………………………………………………………………………………………………………..54

1. Introducere

Mecatronica este o ramură interdisciplinară a științei care combină ingineria mecanică, electronica și informatica.[1] Pe măsură ce tehnologia avansează, aceste ramuri ale ingineriei se diversifică, iar mecatronica are ca scop unificarea acestora într-un proces de proiectare tehnologic din care rezultă produse mai adaptabile și cu o funcționalitate îmbunătățită.[2]

Standardul francez NF E 01-010 definește mecatronica astfel: „abordare care urmărește integrarea sinergistică a mecanicii, electronicii, teoriei controlului și știința calculatoarelor în proiectarea și fabricarea produselor, pentru a îmbunătăți/optimiza funcționalitatea lor”.[3]

Produsul mecatronic este definit astfel: „produs ce are abilitatea de a percepe mediul înconjurător, procesa informații, și capabilitatea de a comunica și interacționa cu acest mediu”.[4]

Termenul „mecatronică” își are originile în Japonia anilor '70. Yasakawa Electric Corporation, companie înființată în 1915, aplică și obține un brevet pentru termenul „mecatronică” în 1971. În acest document, termenul era descris ca ansamblul de activități de proiectare și constructive care folosesc componente electronice și mecanică de precizie. Cuvântul în sine este format prin combinarea cuvintelor mecanică și electronică. În 1982, însă, compania renunță la drepturile sale asupra termenului, acesta devenind universal, tradus în toate limbile.[5]

În prezent, majoritatea dispozitivelor de uz cotidian, roboții industriali sau automobilele sunt un produs al proiectării mecatronice. În momentul în care s-a realizat faptul că soluțiile mecanice propuse nu îndeplineau cerințele impuse de automatizare și robotică, a luat ființă mecatronica. Aceasta oferă multe beneficii pe care ingineria mecanică sau informatica nu le pot oferi individual. Mecatronica are posibilitatea de a utiliza informație cu precizie, care vine sub diferite forme (chimică, biologică, optică, pneumatică, electrică, pneumatică, etc.). Astfel, un sistem mecatronic permite utilizatorului să se concentreze asupra funcțiilor de nivel înalt al sistemului, iar sistemul în sine rezolvă problemele de bază pe cont propriu.

Figura 1 – Ilustrarea conceptului „mecatronică”

În mod normal roboții mobili sunt proiectați în așa fel încât să își mențină poziția de echilibru în mod static, evitând astfel un anumit consum energetic. Dacă se dorește realizarea unor structuri înalte și suple, o metodă ar fi folosirea unui sistem de stabilizare dinamic, ce pot permite menținerea poziției structurii în cazul unor perturbații.

Scopul acestei lucrări este proiectarea unui robot ce se va echilibra dinamic pe o sferă. Se va studia, de asemenea, și modalitatea de control a unui asemenea robot în scopul realizării unei traiectorii impuse. Pentru acestea se va proiecta și realiza o structură fizică, un bloc electronic de comandă și se va implementa o structură de reglare.

2. Stadiul actual al structurilor robotice echilibrate dinamic pe o sferă

Dezvoltarea structurilor robotice stabile dinamic are la bază cercetarea în domeniul roboților de tip „pendul inversat” cu două roți (fig. 2). În anul 1989, K. Yamafuji și T. Kawamura au publicat în Jurnalul Societății de Robotică din Japonia lucrarea „Controlul posturii unui biciclu monoaxial” (trad. eng.) [6], în care se prezintă rezultatele experimentale obținute prin testarea unui robot al cărui corp se echilibrează pe axul celor două roți cu care se deplasează. Înclinarea corpului este determinată cu ajutorul unui senzor aflat în contact cu solul. Aceștia au reușit să echilibreze robotul, dar nu se putea deplasa decât pe o singură direcție, ca urmare a faptului că ambele roți erau acționate de același motor. Au urmat multiple încercări și ale altor cercetători de a aduce îmbunătăți acestui tip de robot. În 1994, Yun-Su Ha și Shin'ichi Yuta proiectează și testează un robot de tip pendul inversat autonom, cu traiectorie bidimensională și abilitatea de a se deplasa cu viteze relativ mari pe suprafețe plane și în pantă [7]. Această lucrare a servit drept inspirație pentru primul robot echilibrat pe o sferă.

Figura 2 – nBot, robot de tip pendul inversat cu două roți

2.1 Ballbot

Pornind de la ideea unei structuri instabile static, Tom Lauwers, Ralph Hollis și George Kantor, cercetători în cadrul Institutului de Robotică al Universității Carnegie Mellon din Statele Unite ale Americii, publică, la Simpozionul Internațional de Robotică de la San Francisco din 2005, lucrarea „Una este de-ajuns!” (trad. eng.) [8], în care este prezentată o structură mobilă care se deplasează cu ajutorul unei singure roți sferice (fig. 3). Titlul face referire la prima parte a lucrării, în care autorii justifică faptul că „roboții mobili cu mai multe roți, stabili static, destinați operării în mediul uman sunt un capăt de drum evolutiv”. Denumirea „ballbot” provine din alăturarea cuvintelor „ball” (din engleză: minge, sferă) și „robot”.

Figura 3 – Conceptul ballbot-ului și implementarea sa

cu cele 3 picioare extinse, (b) cu picioarele retrase, (c) menținându-și poziția și echilibrul

Procesul detaliat al proiectării și rezultatele experimentale mai cuprinzătoare sunt publicate într-o altă lucrare din 2006, intitulată „Un robot mobil cu o singură roată, stabil dinamic, cu mecanism tip bilă de mouse inversat” (trad. eng.) [9].

Corpul robotului este un cilindru cu înălțimea de 1,5 m, cu un diametru de 400 mm și masa de 45 kg. Structura este definită de 3 profile de aluminiu, cu un număr de etaje circulare prinse între ele. Trei picioare retractabile, asamblate pe profile, ajută robotul să își mențină poziția atunci când acesta nu este alimentat. Componentele robotului (controlerul, sursa de alimentare și senzorii) sunt așezate pe etaje, permițând o poziție variabilă a acestora pe axa sa.

Ballbotul este complet autonom. Acesta este alimentat de o baterie de 48V, iar controlul este realizat de un procesor Pentium de 200 MHz. Comunicarea cu ballbotul se face printr-o conexiune wireless 802.11b.

Măsurarea unghiurilor și vitezelor de rotație în jurul axelor de ruliu și tangaj este efectuată cu ajutorul unei senzor inerțial VG700CA-200, care simulează un giroscop vertical. Motoarele folosesc amplificatoare PWM și encodere cu o rezoluție de 4096 de impulsuri pe rotație, pentru a cunoaște în permanență poziția arborilor. Cunoașterea acestor parametri sunt suficienți pentru controlul în buclă închisă.

Mecanismul de acționare (fig. 4) este, în principiu, inversul celui de la mouse. Bila mouse-ului este cea motoare, aceasta rotind rolele, iar prin această rotație calculatorul îi determină poziția. În acest caz rolele sunt cele acționate, rotind sfera robotului, cauzând mișcările.

Figura 4 – Mecanismul de acționare a sferei

Sfera este realizată din aluminiu, cu un diametru de 190,5 mm, acoperită cu un strat de 12,7 mm de ester carbamic (uretan). Aceasta se află în contact cu două role din oțel inoxidabil cu un diametru de 12,7 mm, plasate ortogonal una față de cealaltă la ecuatorul sferei. Mișcarea rolelor este transmisă de la servomotoare prin curele. Diametral opus rolelor motoare se află două role pasive, care, prin intermediul unor arcuri, asigură contactul dintre rolele motoare și sferă. Corpul robotului este așezat pe sferă prin intermediul a trei rulmenți omnidirecționali.

Arhitectura de control a robotului constă din două bucle închise, una interioară, cu regulator PI care calculează comanda cu ajutorul vitezei unghiulare θ a sferei și una exterioară, cu regulator optimal (LQR), care utilizează toți parametrii mișcării (fig. 5).

Figura 5 – Structura de reglare a robotului

Ulterior, acest robot a fost îmbunătățit prin adăugarea unui mecanism de control al unghiului de girație, precum și două brațe, destinate interacțiunii cu omul. Aceste potențiale interacțiuni pot consta în ghidarea persoanelor, asistența celor cu afecțiuni motoare sau transportul colaborativ al greutăților.

Una din îmbunătățirile care trebuie aduse acestui ballbot-ului este prevenirea unei potențiale răsturnări în timpul funcționării datorită defecțiunilor. Picioarele utilizate în această iterație a robotului nu se extind destul de rapid și nu sunt îndeajuns de puternice pentru a opri o răsturnare bruscă. Acest potențial pericol previne, deocamdată, utilizarea sa în medii umane.

2.2 BallIP

BallIP este un robot construit de Masaaki Kumagai și Takaya Ochiai la Universitatea Tohoku Gakuin din Japonia în 2009. Aceștia au pornit de la ideile echipei de la Universitatea Carnegie Mellon și de la o lucrare în care este prezentat un scaun mobil plasat pe o minge de basket, cu un mecanism omnidirecțional cu patru roți [10].

În lucrarea „Dezvoltarea unui robot echilibrat pe o minge – primul raport, implementarea robotului și controlul de bază” (trad. eng.) [11], M. Kumagai și T. Ochiai prezintă o structură echilibrată pe o sferă, cu trei roți omnidirecționale (fig. 6). Avantajul acestui tip de robot este posibilitatea și simplitatea controlului girației fără a fi nevoie de un mecanism în plus.

Figura 6 – BallIP

Robotul are o înălțime de 500 mm și o masă de 8,7 kg, fiind proiectat pentru asistență în transportul obiectelor. Sfera este o minge de bowling cu un diametru de 220 mm și masa 3,8 kg, acoperită cu un strat de cauciuc aplicat în formă lichidă.

Cele trei roți omnidirecționale de diametru 100 mm au fost plasate la 120° una față de cealaltă, la un unghi de 50° față de axa verticală a sferei. Roțile au fost atașate direct pe arborii motoarelor, pentru a evita jocurile apărute la schimbarea direcției introduse de reductoare. Motoarele sunt de tip pas cu pas unipolare (KH56QM2-913), dar au fost utilizate ca unele bipolare pentru a putea obține un cuplu mai mare. Fiecare motor este dotat cu un controler TA8435 pentru a putea obține o diviziune de 0,225° pe pas. Motoarele și controlerele sunt alimentate de câte o baterie Ni-MH de 7,2 V.

Sunt utilizate două perechi de giroscoape (ADXRS401) și accelerometre (ADXL203), ale căror date sunt folosite de către microcontrollerul de 16 biți H8/3052 pentru a menține echilibrul robotului. Controlul este cel al pendulului inversat pe două direcții și regulator PD pentru accelerația unghiulară a sferei, care asigură robustețea sistemului.

Deși acest model de robot își menține echilibrul și se poate deplasa cu ușurință în orice direcție, înălțimea sa scăzută limitează domeniile în care acesta poate fi utilizat.

2.3 Ballbot Lego

În 2009, Justin Fong și Simon Uppill, studenți ai Universității din Adelaide, Australia, au realizat, în cadrul proiectului de diplomă [12], un robot de tip ballbot, construit cu componente din setul Lego NXT Mindstorms 8527 (fig. 7).

Figura 7 – Ballbot-ul Lego

Ținând cont de posibilitățile oferite de acest set, autorii au decis că o structură similară ballbot-ului construit la UCM (două roți acționate, două libere, plasate ortogonal la ecuatorul sferei) este cea mai avantajoasă.

Controlerul utilizat este NXT Brick, ce utilizează un procesor principal Atmel pe 32 de biți cu 48 MHz, și unul secundar pe 8 biți cu 8 MHz. Programarea este realizată cu software specific Lego NXT și Simulink din MATLAB .

Două servomotoare de 9 V, cu encoder, sunt folosite pentru a roti sfera. Senzorii utilizați sunt trei giroscoape HiTechnic, un accelerometru și un senzor tactil pentru detecția sferei.

Controlul este bazat pe un program dezvoltat de Y. Yamamoto pentru un ballbot NXT, disponibil pe pagina MathWorks [13].

Acest robot a fost realizat pentru a putea fi utilizat în scop didactic. În cadrul aceleiași lucrări, autorii încep proiectarea unui ballbot de dimensiuni umane, pornind de la ballbot-ul Lego.

2.4 Rezero

În cadrul Focus Project Ballbot 2009/10, organizat de ETH Zürich (Institutul Federal Elvețian al Tehnologiei), 8 studenți la inginerie mecanică, 2 studenți la inginerie electrică și 3 studenți de la design industrial au proiectat și construit, pe durata a 9 luni, un robot numit Rezero (fig. 8) [14].

Figura 8 – Rezero și componentele sale

Similar cu BallIP de la Universitatea Tohoku Gakuin, robotul se deplasează cu ajutorul unei roți sferice, acționată de trei motoare de curent continuu fără perii, de 200 W, dotate cu reductoare ce permit un cuplu maxim de 5 Nm, și roți omnidirecționale. Sfera este goală pe interior, confecționată dintr-un strat de aluminiu de 2 mm și un strat de material sintetic cu un coeficient de frecare ridicat, cu o grosime de 4 mm. Cele trei opritoare asigură forța de contact dintre sferă și roți.

Senzorul principal este de tip inerțial, care măsoară cele trei unghiuri Euler și vitezele unghiulare. Un microcontroler de 157,8 Hz comunică cu o placă Linux prin conexiune RS-232.

Modelarea matematică și controlul robotului a fost realizată de Péter Fankhauser și Corsin Gwerder în cadrul lucrării de licență la aceeași universitate [15].

Mulțumită programării sale, Rezero este primul ballbot care se poate folosi în întregime potențialul oferit de deplasarea pe o singură roată sferică. Acesta a fost prezentat public de numeroase ori, inclusiv la o conferință TED. Viitoare îmbunătățiri ale acestui sistem pot consta în implementarea unui regulator neliniar pentru un control îmbunătățit și utilizarea unui reductor armonic pentru a reduce zgomotul și erorile induse de un reductor planetar.

2.5 Robot miniatural echilibrat pe o sferă

În 2015, un grup de cercetători ai Universității din San Diego, California, au dezvoltat și testat mai multe tipuri de roboți, toți având înălțimi sub 25 cm și mase mai mici de 580 g [16]. Utilizând prototiparea rapidă, aceștia au construit peste 10 roboți funcționali, trei dintre care sunt reprezentați în figura 9, pentru a decide care dintre variantele de acționare a sferei sunt mai avantajoase.

Figura 9 – Variante de mecanisme testate: (a) tip bilă de mouse inversat, (b) cu roți omnidirecționale plasate în mod convențional, (c) cu roți omnidirecționale plasate ortogonal

Una dintre concluziile lucrării au fost că, în vederea construirii unui robot miniatural, cea mai bună soluție în ceea ce privește acționarea este cea cu roți omnidirecționale, deoarece mecanismul tip bilă de mouse inversat introduce forțe de frecare asimetrice asupra sferei. Deoarece robotul are o masă redusă, aceste forțe cauzează ieșirea sferei din mecanism.

În general, acești roboți sunt dotați cu trei motoare cu reductoare, folosiți la jucării, encodere optice, iar sfera este o minge de cauciuc cu un diametru de 6,4 cm. Corpul este obținut prin printare 3D, iar pe acesta se plasează controlerul, un microprocesor BeagleBone Black, și bateriile litiu-ion.

Avantajele acestei soluții sunt dimensiunile miniaturale, greutatea redusă, mulțumită căreia, robotul poate dezvolta și viteze mai mari decât celelalte modele, și costurile scăzute. Pe de altă parte, miniaturizarea ridică probleme din punct de vedere mecanic, ceea ce complică proiectarea sau limitează performanțele robotului.

2.6 Robot cu motor de inducție sferic

În cadrul Conferinței Internaționale de Robotică și Automatizare din 2015, Ralph Hollis (creatorul primului ballbot) și Masaaki Kumagai (creatorul BallIP) prezintă un motor creat special pentru roboții ce se echilibrează [17].

Din experiența construcțiilor anterioare, s-a determinat faptul că cele două tipuri principale de acționare a roboților ce se echilibrează (cu role sau cu roți omnidirecționale) nu sunt ideale. Astfel, autorii au fost motivați să găsească o soluție care să implice contactul minim dintre sferă și mecanismul de acționare.

Figura 10 – Motor de inducție sferic

Acest motor (figura 10), este construit cu șase înfășurări statorice, așezate la un unghi pe un cadru din aluminiu, iar rotorul este sferic, un miez de oțel acoperit de un strat de cupru. Fiecare stator poate produce aproximativ 40 N de forță tangențială asupra rotorului, rezultând într-un cuplu maxim de 8 Nm. Mecanismul tip bilă de mouse inversat poate produce un cuplu de până la 40 Nm, dar momentele rezistente sunt aproximativ de 10 ori mai mari decât la motorul de inducție.

În 2016, Greg Seyfarth, masterand al Universității Carnegie Mellon, proiectează și testează un robot care se deplasează utilizând acest motor [18]. Construcția este aceeași cu cea a robotului prezentat la 2.1, iar rezultatele lor au fost comparate. În general, performanțele sunt similare: robotul cu role are o viteză maximă de 2,25 m/s și o accelerație maximă de 5,3 m/s2, iar cel cu motor de inducție o viteză de 1,9 m/s și accelerație de 3 m/s2. Cu toate acestea, robotul cu motor de inducție este de două sau trei ori mai puțin eficient din punct de vedere energetic față de tipurile convenționale de acționare.

2.8 Concluzii

Din 2006 până în prezent, roboții mobili care se echilibrează pe sferă au trecut prin multiple iterații. Cei care au proiectat un astfel de robot au urmărit creșterea performanțelor fie prin testarea a noi soluții constructive sau a îmbunătățirilor soluțiilor precedente. În tabelul următor sunt comparați parametrii funcționali ai câtorva dintre roboții documentați până acum.

Tabelul 1 – Performanțele roboților existenți

3. Soluția proprie

O problemă semnificativă, dar deseori ignorată, a structurilor robotice mobile clasice (o bază cu 3 sau 4 roți, stabilă în mod static) este faptul că acestea pot deveni instabile în timpul mișcării. Dacă centrul de greutate se află la o înălțime prea mare față de sol, atunci când robotul accelerează sau frânează brusc sau se află pe o suprafață înclinată, există riscul răsturnării structurii.

În cazul unui robot proiectat pentru interacțiunea umană sau cu mediul uman, acesta trebuie să aibă o înălțime corespunzătoare. De asemenea, trebuie să fie îndeajuns de suplu pentru a se putea deplasa eficient, fără să intre în contact cu obiectele din mediu sau să îngreuneze circulația persoanelor.

Figura 11 – Reprezentarea siluetelor și pozițiilor centrelor de greutate în diverse structuri mobile.

Dreptunghiul reprezintă dimensiunea aproximativă a ușii unei case.

structură balansată pe o sferă, (b) – (e) diverși roboți stabili static

Pentru a rezolva aceste probleme, acești roboți au baze relativ mari comparativ cu dimensiunile medului (de exemplu, o ușă, așa cum este ilustrat în figura 11), centrele lor de greutate sunt la o înălțime cât mai redusă (prin adăugarea unor greutăți) și se deplasează îndeajuns de lent pentru a evita răsturnarea.

Se poate deduce că roboții prezenți în medii umane au nevoie de un alt fel de mobilitate față de cel obișnuit. Ei trebuie să fie siguri; agili și capabili de mișcări fluide, să fie îndeajuns de supli pentru a putea naviga medii aglomerate și trebuie să poată fi mișcați cu ușurință de către persoane. O soluție care acoperă toate aceste aspecte este stabilitatea dinamică. Aceasta este inspirată din felul în care oamenii și animalele se deplasează, având o stabilitate în mișcare intrinsecă.

[9]

Ținând cont de această problemă și de literatura de specialitate studiată, s-a optat pentru proiectarea și construirea unui robot stabil dinamic, cu o singură roată sferică.

Figura 12 – Model conceptual al robotului

Proiectarea constructivă a pornit de la robotul construit la Universitatea Tohoku Gakuin, cel cu trei roți omnidirecționale. A fost ales acest tip de structură deoarece acesta permite rotația în jurul propriei axe, spre deosebire de mecanismul tip bilă de mouse inversat care necesita un mecanism în plus pentru materializarea acestui grad de libertate. În figura 12 este ilustrată forma generală a acestui tip de robot: un corp care conține electronica de comandă, senzorii și sursa de alimentare, trei motoare asamblate pe acesta și trei roți omnidirecționale care se află în contact cu sfera. În lucrarea de față s-a urmărit și adăugarea unei caracteristici originale: posibilitatea reglării poziției motoarelor, astfel încât robotul poate funcționa cu sfere de diverse diametre. Acest lucru poate fi realizat prin proiectarea unei soluții de prindere al motoarelor care permite un grad de libertate, ce se bazează pe o cuplă de rotație.

Figura 13 – Bucla de control

Principiul de funcționare este similar cu cel al unui pendul inversat, reprezentat în figura 13. Diferența principală este că, în acest caz, dezechilibrul are loc în două planuri. Cu ajutorul senzorului inerțial (UMI), este măsurată în permanență înclinarea și viteza unghiulară față de cele trei axe. Prin intermediul traductoarelor de rotație (encodere) ale celor trei motoare, este determinată viteza și poziția sistemului în orice moment. Folosind aceste date, asemănător cu sistemele care echilibrează un pendul inversat, un algoritm de reglare implementat în unitatea de calcul determină acțiunile necesare pentru a contracara înclinările și a menține robotul în poziție verticală. Aceste acțiuni reprezintă, de fapt, mișcarea uneia sau a mai multor roți. Pentru a putea menține echilibrul, măsurarea, calculul și mișcările roților trebuie să fie foarte frecvente, de ordinul zecilor de Hz.

Figura 14 – Mișcarea sferei în funcție de mișcarea roților

Rotația sferei, și, implicit, mișcarea întregului sistem este direct influențată de sensul de rotație al roților omnidirecționale, ilustrat în figura 14. Când roțile 1 și 2 se rotesc ca în sensul figurat, sfera se rotește în jurul axei X, iar întreg sistemul avansează de-a lungul axei Z. Roata 3 este lăsată liberă și, datorită faptului că este omnidirecțională, nu se opune mișcării sferei.

Deplasarea pe o anumită direcție a unui ballbot este inspirată din mersul omului. Pentru a se pune în mișcare într-o direcție, robotul trebuie mai întâi să „cadă” pe acea direcție, ca apoi să accelereze și să ajungă la destinație în poziție verticală.

Avantajul unui astfel de robot comparat cu cei clasici este, în primul rând, mobilitatea ridicată. Robotul se poate deplasa în orice direcție fără a fi nevoie de manevre în plus datorită faptului că acesta se poate roti pe loc. Mulțumită stabilității în mișcare, acesta nu se va răsturna în nicio situație, chiar și în prezența perturbărilor exterioare. Structurile de acest tip pot avea înălțimi mari, apropriate de înălțimea medie a unui om, fără a se pune problema stabilității. De asemenea, ca urmare a faptului că acești roboți au o singură roată sferică, amprenta lor pe sol este, teoretic, un singur punct, ceea ce va ușura navigarea în jurul obstacolelor.

Figura 15 – Prima variantă de structură a robotului

În figura 15 este prezentat modelul 3D al primei variante constructive a robotului. Corpul robotului este împărțit pe trei nivele, trei discuri în care sunt executate diverse decupaje pentru a permite prinderea componentelor electronice și trecerea cablurilor (fig. 16). Pe nivelul superior (1) va fi așezat, pe o poziție centrală, senzorul inerțial. Nivelul mediu (2) va conține bateria și controlerul. Nivelul inferior (3) va include circuitele de comandă pentru motoare și trei seturi a câte două găuri ovale în care se asamblează sistemele de prindere ale motoarelor (4), care vor permite reglarea distanței față de centrul discului a motoarelor. Discurile sunt asamblate între ele prin intermediul a trei tije filetate (5), iar distanța dintre ele este materializată prin intermediul unor distanțiere (6) inserate pe tije.

(a) (b) (c)

Figura 16 – Cele trei discuri care constituie corpul robotului.

(a) discul superior, (b) discul mediu, (c) discul inferior

Figura 17 – Ansamblul de prindere al motorului

Ansamblul de prindere al motorului (figura 17) va permite varierea poziției motorului în funcție de diametrul sferei pe care robotul va fi așezat. Piesa fixă (1) este asamblată pe discul inferior prin intermediul a patru șuruburi. Piesa mobilă (2) se articulează de piesa fixă cu o tijă (3). Rotația ei este limitată de decupajul circular din piesa fixă, prin care va trece un șurub (4) asamblat cu o piuliță în piesa mobilă. Motorul (5) este, în final, prins de piesa mobilă cu o flanșă (6), iar pe arborele motorului este plastă roata omnidirecțională (7, figura 18), fixată cu două șuruburi.

Figura 18 – Roata omnidirecțională în vedere și cu capacul îndepărtat

Roțile omnidirecționale, în general, sunt roți ce au role pe circumferință, orientate perpendicular pe direcția de rotație a întregului ansamblu. Efectul acestor role este faptul că roata se va putea roti dar nu va opune rezistență la mișcări laterale.

În acest caz, roata este compusă din două piese circulare (1 și 2) cu zece decupaje pentru role (3). Acestea sunt așezate pe una din piese, iar cealaltă le va acoperi, nepermițând deplasarea tijelor pe care se află rolele (4). Cele două piese sunt strânse împreună cu șuruburi și piulițe. Acestui ansamblu i se adaugă o bară hexagonală cu găuri filetate pe două fețe opuse (5), ce are rolul de a realiza legătura cu motorul.

4. Memoriu tehnic

Funcțiile de echilibrare și cea de deplasare sunt implementate prin doi algoritmi ce utilizează două structuri de reglare diferite.

4.1 Algoritmul de echilibrare

Structura de reglare automată a înclinării robotului utilizează câte un regulator P-D (proporțional – derivativ) modificat pentru cele două axe de înclinare (X și Y) ale robotului. Structura are forma următoare:

Figura 19 – SRA pentru echilibrare [19]

R(s) reprezintă referința, valoarea înclinării dorită pe axa respectivă. În acest caz, aceasta va fi în permanență 0. ε(s) este diferența dintre referință și Y(s), înclinarea curentă a robotului, numită și eroare. HR1(s) reprezintă componenta K a regulatorului și este de forma:

unde Kr este un factor determinat experimental. La ieșirea HR1 se obține comanda U(s), calculată în funcție de eroare, pentru axa respectivă. Componenta D a regulatorului, HR2(s), are forma:

Diferența dintre U1(s) și ieșirea U2(s) a blocului HR2(s) este U(s), comanda efectivă dată procesului (robotul).

După calcularea comenzilor pe fiecare axă, acestea trebuie convertite în comenzi pentru fiecare dintre cele trei motoare utilizând cinematica inversă.

Figura 20 – Sistemul de coordonate al robotului și al motoarelor [20]

Datele oferite de către senzor sunt înclinări față de axele X și Y, reprezentate în figura 20 ca rx și ry. Comenzile calculate de către regulatoare vor anula înclinările pentru a păstra robotul în poziție verticală, dar aceste comenzi trebuie traduse în sistemul de coordonate ale celor trei motoare. Motorul 3 este este aliniat cu axa Y a sistemului, iar celelalte două sisteme de coordonate pot fi obținute aplicând rotații succesive de 120° sistemului 3.

(4.1)

(4.2)

Pentru a determina rx1, rx2 se utilizează matricea de rotație A, de forma:

(4.3)

Transformarea de la coordonatele sistemului 3 la sistemele 2 și 1 se realizează astfel:

(4.5)

(4.6)

Rezultă următorul sistem:

(4.7)

(4.8)

(4.9)

Având aceste ecuații, se pot obține comenzile în PWM pentru fiecare motor în parte. Codul acestui algoritm se află în Anexa 2.

4.2 Algoritmul de deplasare

Deplasarea este implementată prin utilizare unei structuri de reglare în cascadă a poziției fiecărui motor cu regulator PD în interiorul căreia se regăsește o buclă de reglare a vitezei motorului cu regulator PI (proporțional integrativ).

Figura 21 – Sistemul de reglare în cascadă

Bucla externă, a poziției, este cea care primește referința r2: o distanță de parcurs pe axele X și Y și o orientarea sa la destinație. Eroarea, ε2, este diferența dintre parametrii actuali de poziție ai robotului (y2) și referință. Regulatorul buclei externe, HR2(s), este de tip PD și are forma:

(4.5)

calculează o comandă (u2) ce servește drept referință pentru bucla internă. Eroarea ε1 se calculează ca diferența dintre referința de viteză, r1, și viteza curentă a motorului, y1. Regulatorul buclei interne este de tip PI, de forma:

(4.6)

Comanda calculată de acest regulator este dată procesului buclei interne, HP1(s), care reprezintă motorul. Semnalul u1 constă din valori PWM transmise motoarelor. Viteza fiecărui motor are rol de comandă pentru procesul HP2(s), care este, de fapt, poziția întregului sistem.

Pentru a putea obține comenzile pentru fiecare motor în parte, trebuie aplicată aceeași transformare din coordonate robot în coordonate motor ca în ecuațiile 4.2 – 4.4.

Pentru controlul vitezei este necesară obținerea caracteristicii statice a fiecărui motor în parte. Pentru această determinare se aplică o valoare de PWM fiecărui motor și este măsurată viteza:

Tabel 2 – Vitezele motoarelor la diverse valori PWM

Grafic 1 – Caracteristica celor 3 motoare

La sistemele liniare, factorul de amplificare K al fiecărui motor se determină cu formula:

(4.7)

unde V este viteza iar U este valoarea PWM.

Însă, după cum se poate observa din datele colectate, caracteristicile statice ale celor 3 motoare, adică dependențele Vj(U), sunt neliniare, ceea ce înseamnă că factorul de amplificare al motoarelor variază in funcție de viteza de lucru. Pentru a compensa acest comportament în buclă închisă trebuie ca factorul de amplificare Kj al fiecărui motor să fie recalculat in timpul funcționării. Așadar, având în vedere că forma caracteristicilor statice din graficul 1 nu prezintă o curbură foarte pronunțată, se poate utiliza o aproximare polinomială de ordinul 1 care să descrie dependența dintre factorii de amplificare și viteza de lucru (din punct de vedere numeric, o aproximare de ordinul 2 a generat un parametru de câteva ordine de mărime mai mic decât celelalte două, care devine neglijabil relativ la restul parametrilor). In cazul de față, viteza de lucru va fi viteza dorită a motoarelor, dată de referința buclei de reglare a vitezei.

Așadar, se obțin:

(4.8)

(4.9)

(4.10)

Cu aceste valori ale factorului de amplificare se pot calcula parametrii regulatoarelor PI utilizate pentru buclele interne de reglare a vitezei. Astfel, pentru fiecare bucla de reglare a vitezei, regulatorul este de forma:

(4.11)

unde parametrii KR si KI depind de K1, K2 și K3.

Dorim ca răspunsul în buclă închisă pentru viteza motoarelor să nu aibă suprareglaj și eroare staționară, iar timpul de răspuns sa fie mic. Astfel, utilizând metoda poli-zerouri [21], obținem:

(4.12)

(4.13)

unde T reprezintă timpul de răspuns în buclă deschisă al motoarelor, iar T0 reprezintă timpul de răspuns dorit. Deoarece T nu poate fi măsurat cu precizie ridicată, aceasta a fost setat la valoarea 1, iar acordarea parametrului KR pentru fiecare motor a fost acordată utilizând raportul T/T0 în timp ce acordarea parametrului KI a fost realizată prin varierea raportului 1/T0.

Implementarea acestui algoritm se află în Anexa 3.

4.3 Simularea contactului dintre o roată și sferă

Pentru ca robotul să se poată deplasa, acesta trebuie să acționeze asupra sferei pe care se bazează prin intermediul roților acționate de motoare. Este necesar ca forța de frecare dintre roată și sferă să fie suficient de mare astfel încât să se poată realiza transmiterea mișcării prin fricțiune, fără patinări. Trebuie ca accelerația indusă roții să fie mereu mai mică decât o accelerație maximă, determinată astfel:

Forța (F) este determinată de masa ansamblului (m) și accelerația la acel moment (a).

(4.7)

Forța de frecare (Ff) este produsul dintre coeficientul de frecare () și normala (N).

(4.8)

Normala poate fi exprimată astfel:

(4.9)

Unde g este accelerația gravitațională, .

Condiția pentru ca mișcarea să se realizeze fără patinare este:

(4.10)

(4.11)

(4.12)

Pentru a verifica această condiție, se realizează o simulare utilizând metoda echivalenței electro-mecanice [22].

Figura 22 – Reprezentarea motorului, reductorului, roții și sferei

În această schemă este reprezentat unul dintre cele trei motoare ale robotului (M), un reductor (R1 și R2) și contactul dintre roata acționată (R3) și sfera (R4). În acest exemplu se iau în considerare momentul de inerție al arborilor și al sferei.

Pe baza acestei reprezentări se obține circuitul echivalent.

Figura 23 – Circuitul echivalent

Cu ajutorul acestui circuit se poate scrie sistemul de ecuații caracteristic sistemului simulat.

Valoarea accelerației pentru care apare patinarea este de aproximativ 8 m/s2. În urma simulării, se poate observa în graficul 2 că accelerația nu depășește valoare de 3 m/s2.

Grafic 2 – Variația accelerației în timp la urmărirea unei referințe de distanță

5. Tehnologii de execuție, montaj și experimentare

5.1 Realizarea structurii mecanice

În figura 24 este prezentată structura robotului implementată fizic (a) alături de modelul proiectat (b). Astfel se observă similaritatea dintre modelul CAD și obiectul fizic, fapt datorat tehnicilor de prelucrare moderne utilizate (debitare laser, printare 3D, etc.). Datorită faptului că majoritatea tehnicilor de execuție au fost bazate pe echipamente cu comandă numerică, s-au obținut următoarele avantaje: costul prelucrării scăzut cu o precizie ridicată și viteză și eficiență mare. În continuare sunt prezentate piesele realizate alături de modelul lor proiectat și metoda prin care au fost obținute.

(a) (b)

Figura 24 – Robotul și modelul său

Cele trei discuri care susțin componentele robotului au fost realizate prin debitare cu laser din placaj MDF cu o grosime de 4 mm, după un desen în format .dwg. Pe toate cele trei discuri au fost executate trei găuri la distanțe egale prin care trec tijele filetate pentru susținerea lor și un orificiu central pentru trecerea cablurilor.

Figura 25 – Discul superior

Pe discul superior sunt prezente patru găuri plasate pe centru destinate prinderii senzorului inerțial. De asemenea, pe suprafața sa a fost gravată cu laser sigla universității.

Figura 26 – Discul mediu

Pe discul mediu au fost executate diverse decupaje și orificii ovale pentru prinderea componentelor electronice (acumulator, controler, sursa coborâtoare, etc.).

Figura 27 – Discul inferior

Pe discul inferior se regăsesc trei seturi de câte două orificii ovale care permit poziționarea variabilă a sistemului de prindere al motoarelor și alte trei seturi pentru prinderea driverelor de motoare.

Figura 28 – Vedere laterală a ansamblului de prindere al motoarelor – realizare fizică și model

Figura 29 – Vedere frontală a ansamblului de prindere al motoarelor

Piesa fixă (1), piesa mobilă (2), roțile omnidirecționale și rolele lor au fost realizate prin prototipare rapidă (printare 3D). Acest proces pornește de la un model 3D (în general în format .stl) realizat cu programe de tip CAD. Un program numit „slicer” (feliator) împarte obiectul reprezentat în model într-un număr finit de „felii” (secțiuni), cu o grosime constantă, predefinită. Bazat pe acea secționare, slicer-ul creează un fișier de tip G-code care conține instrucțiunile necesare imprimantei. Imprimanta 3D (fig. 30) este un dispozitiv care, cu ajutorul unui cap de extrudare cu mobilitate pe trei axe, topește un material sub formă de filament și îl depune strat cu strat pentru a forma un obiect.

Figura 30 – Imprimanta 3D Anet A8

Figura 31 – Printarea roților omnidirecționale

BaBB

Figura 32 – 1 – Piesa fixă

Figura 33 – 2 – Piesa mobilă

Piesa fixă (1, figura 32) se asamblează împreună cu piesa mobilă (2, figura 33) cu o tijă (4, figura 34) de ⌀3 mm debitată dintr-o bară de duraluminiu.

Figura 34 – Tija de duraluminiu

Flanșa (3, figura 35) folosită la asamblarea motorului de piesa mobilă este disponibilă comercial.

Figura 35 – Flanșa motorului

Figura 36 – Roata omnidirecțională

Figura 37 – Roata omnidirecțională cu capacul îndepărtat

Roata omnidirecțională (figura 36, 37) este compusă din două piese circulare cu zece decupaje pentru role. Rolele (figura 38) au fost obținute prin prototipare rapidă, în centrul lor au fost inserate țevi debitate dintr-o bară de duraluminiu, iar pe diametrul exterior rolele au fost acoperite cu un strat de cauciuc sintetic.

Figura 38 – Componența rolei: 1 – cauciuc sintetic; 2 – rola printată 3D; 3 – țeava de duraluminiu

Figura 39 – Bara hexagonală

În centrul roții se asamblează o piesă (figura 39) obținută prin debitare dintr-o bară hexagonală de alamă (CuZn39Pb3). Această bară este găurită în centru cu diametrul de 6 mm pentru intrarea arborelui motorului. În gaura de M4 se înfiletează șurubul de prindere pe roata omnidirecțională. Pe două fețe opuse ale hexagonului sunt practicate două găuri de M3 străpunse în care se vor asambla două șuruburi de blocare a arborelui. Prelucrarea acestei piese a fost realizată prin metode convenționale (debitare pe strung, găurire cu burghiul și filetare cu tarod), reprezentate în figurile 40 și 41. Desenele de execuție pentru această piesă, tije și distanțiere se regăsesc în Anexa 4.

Figura 40 – Obținerea găurii de ⌀6

Figura 41 – Filetarea găurii de M4

5.2 Realizarea blocului electronic

Figura 42 – Schema electrică

Figura 43 – Acumulatorul

Pentru alimentarea circuitului (fig. 42) este utilizat un acumulator LiPo (Litiu-Ion Polimer) cu o capacitate de 2650 mAh (fig. 43). Având 4 celule, tensiunea maximă furnizată de aceasta este de 14,8 V. Curentul maxim se calculează în funcție de constanta C a acumulatorului și capacitate:

(5.1)

Pentru un curent mediu de 4 A, timpul de funcționare este:

(5.2)

Figura 44 – Regulator de tensiune LM2596

La ieșirea acumulatorului este conectat un buton care întrerupe alimentarea cu energie a întregului circuit. Deoarece tensiunea oferită de acumulator este mai mare decât tensiunea necesară alimentării driverelor și controlerului se utilizează un regulator de tensiune, care la ieșire are o tensiune la o valoare ajustată de utilizator (în acest caz, 12 V). LM2596 funcționează cu tensiuni de intrare între 5 și 60 V și poate furniza între 1,25 și 26 V, cu un curent maxim de 5 A.

Figura 45 – Arduino MEGA2560

Arduino MEGA2560 (figura 45) este controlerul utilizat în această aplicație, ales pentru numărul mare de pini disponibili: 54 de pini input/output digitali, dintre care 15 cu capabilitate PWM (Pulse Width Modulation), și 16 pini analogici. Acesta este alimentat de la sursa coborâtoare cu 12 V prin pinii Vin și GND.

Figura 46 – Driverul VNH2SP30

Pentru a putea comanda pornirea și oprirea celor trei motoare, sensul de rotație și turația prin controler este necesară utilizarea câte unui circuit special de comandă numit driver. În cazul acesta, VNH2SP30 (fig. 46) este un driver ce poate fi alimentat cu o tensiune între 5,5 și 16 V și poate furniza un curent maxim de 14 A pe fiecare canal. În acest circuit, driverele sunt alimentate cu 12 V de la sursa coborâtoare pentru etajul de putere și 5 V pentru etajul loc de la Arduino, prin pinii 5V și GND. Pinul EN (Enable) este cel care permite funcționarea motorului atunci când este scrisă o valoare HIGH de la controler. Utilizând pinul CS (Current Sense) se poate monitoriza valoarea curentului prin driver. Pinii INA și INB sunt cei care determină sensul de rotație al motorului, după următorul tabel de adevăr:

Tabel 3 – Sensul de rotație în funcție de comandă

Viteza de rotație este setată prin pinul PWM, pe care este transmisă o valoare între 0 și 255. Motorul este conectat la driver prin bornele OUT + și OUT -.

Figura 47 – Motorul de curent continuu JGB37-540B

Motoarele folosite în această aplicație sunt motoare de curent continuu JGB37-540B. Acestea operează cu tensiuni cuprinse între 6 și 12 V. La mersul în gol, alimentate cu 12 V, acestea dezvoltă o turație maximă de 600 rot/min cu un consum de curent de 0,2 A. În sarcină, turația devine 450 rot/min, iar cuplul dezvoltat este de 0,15 Nm.

Pe arborele motorului se află un traductor de rotație incremental bazat pe efectul Hall. Acesta este alimentat cu 5 V din controler, iar citirea se face conectând canalele A și B la pini digitali ai Arduino ce permit programarea întreruperilor.

În primă fază, motoarele, encoderele și driverele au fost testate separat, utilizând o sură de 12V (figura 48). Pentru acest test, Arduino a fost programat cu un cod simplu, în care sunt transmise driverelor diverse valori de PWM și sunt citite encoderele, pentru a putea testa funcționalitatea acestora. Codul de test se regăsește în Anexa 1.

Figura 48 – Testarea motoarelor

Figura 49 – Senzorul inerțial BNO055

Măsurarea înclinării și a vitezei unghiulare pe fiecare axă a robotului se realizează prin intermediul senzorului inerțial. BNO055 este un ansamblu de senzori (accelerometru, giroscop, magnetometru) dotat cu un procesor Cortex M0 ARM care realizează fuziunea senzorială la o frecvență de 16 MHz. Datele ce pot fi furnizate de acest senzor sunt: orientare absolută (unghiuri Euler și matrice cuaternară), viteze unghiulare, accelerație, intensitatea câmpului magnetic, vectorii gravitaționali și temperatură. Senzorul este alimentat cu 5 V de la controler, iar pinii SDA (linia de date) și SCL (linia de temporizare) sunt conectați cu pinii SDA și SCL ai controlerului.

Deoarece acest senzor nu conține o memorie internă, el trebuie calibrat înainte de fiecare utilizare. Giroscopul se calibrează automat dacă senzorul este menținut în jur de un minut într-o poziție stabilă. Accelerometrul se calibrează rotind senzorul în jurul unei axe cu un increment de 45°. Calibrarea magnetometrului se realizează prin mișcarea senzorului pe o traiectorie în formă de 8.

Un avantaj al acestui senzor este simplitatea preluării și prelucrării datelor furnizate de acesta. Utilizând o bibliotecă creată special pentru acest senzor, obținerea unghiurilor Euler de orientare absolută se realizează direct, apelând o funcție, fără a mai fi necesară prelucrarea ulterioară a datelor.

În tabelul următor se regăsește configurația finală a pinilor pe Arduino MEGA. Această configurație a fost realizată ținând cont de tipul de pin necesar fiecărei componente, dar și de distanța fizică dintre componentă și Arduino.

Tabel 4 – Configurația pinilor

5.3 Testarea structurii robotice obținute

Primele teste au fost efectuate cu sfera blocată (fig. 50). S-a observat faptul că în această situație robotul își menține cu succes echilibrul. Acesta oscilează în jurul poziției sale de echilibru și poate respinge perturbații până într-o anumită amplitudine și înclinări până în 10°. Când sfera a fost eliberată, acesta nu și-a mai putut menține echilibrul deoarece deplasarea sferei introduce cel puțin un pol în originea procesului, iar soluția actuală de reglare nu este îndeajuns de performantă pentru această situație.

Figura 50 – Robotul menținându-și echilibrul

Datele senzorului de orientare au fost preluate în timpul funcționării, rezultând graficul următor:

Grafic 4 – Înclinările robotului în timp

În graficul de mai sus se poate observa comportamentul relativ stabil al robotului și felul în care acesta respinge perturbații. Cele două linii reprezintă valoarea unghiului pe care robotul îl formează cu axa X, respectiv Y. Valoarea în jurul căreia variază este deplasată față de 0 deoarece masa pe care au fost realizate testele avea o ușoară înclinare.

Din acest grafic reiese natura oscilatorie a robotului față de poziția de echilibru, suprareglajul și timpul de răspuns relativ mici. La momentele t1 ≈ 20 și t2 ≈ 115 sunt introduse perturbații pe care sistemul le atenuează în timp. În figura 51 se poate vizualiza mișcarea de revenire a robotului atunci când acesta este împins.

Figura 51 – Perturbarea robotului și mișcarea sa de revenire

Testele de echilibrare au fost realizate pe mai multe sfere, cu diametre și suprafețe diferite (fig. 52)

Figura 52 – Sferele de test

Sfera (a) este de plastic, goală pe interior, cu o suprafață lucioasă. Sfera (b) este o minge de polo pe apă, (c) este o minge de basket cu diametrul iar sfera (d) este o minge medicinală de 2 kg. S-a observat faptul că funcționarea optimă are loc pe sferele (b) – (c) datorită coeficientului de frecare mai mare. Diferențele de diametru nu au influențat echilibrarea.

Deoarece robotul nu se echilibrează pe sfera mobilă, testarea deplasării a fost realizată la sol. Primul test a fost urmărirea unei referințe de distanță: o distanță de 400 mm de-a lungul axei X (fig. 53).

Figura 53 – Urmărirea unei referințe de distanță

Se poate observa un suprareglaj de aproximativ 20 mm. Următorul test a fost comandarea orientării (fig. 54). Și în acest caz există un suprareglaj de aproximativ 3°.

Figura 54 – Orientarea comandată

În urma acestor teste a fost observat efectul saturației inferioare al motoarelor.

Grafic 5 – Comanda unui motor în timp

O comandă în PWM mai mică de 19% nu are niciun efect mecanic asupra structurii motorului, de aceea comanda în intervalul [-19;19] are salturi bruște de la un capăt al intervalului la celălalt. Acest fapt are ca efect suprareglaj mare al poziției motorului la schimbarea sensului. Având în vedere că majoritatea erorilor sunt mici, un răspuns amplu nu este corespunzător.

6. Eficiența economică

6.1 Prezentare generală

Acest studiu tehnico-economic are rolul de a evidenția caracteristicile economice a resurselor folosite pentru realizarea sistemului robotic, care, în urma vânzării, va obține profit.

În continuare sunt calculate cheltuielile totale pentru un produs de serie mică în care se fabrică un număr redus de exemplare.

Profitul se calculează cu formula următoare:

(6.1)

unde:

– prețul de vânzare;

– cantitatea de produse vândute;

– costuri de producție totale;

Calculul costurilor de producție totale:

(6.2)

unde:

– cheltuieli cu materiale;

– cheltuieli cu resurse umane;

– amortizarea echipamentelor;

6.2 Calcularea cheltuielilor materiale

Cheltuielilor materiale se calculează cu formula de mai jos:

(6.3)

unde:

– cheltuieli cu produse finite;

– cheltuieli cu materia primă;

– cheltuieli cu unități;

În tabelul 5 sunt prezentate prețurile produselor finite necesare sistemului robotic.

Tabel 5 – Prețurile produselor finite necesare sistemului robotic

Cheltuielile cu produsele finite se obțin însumând prețul total al fiecărui produs din tabel.

(6.4)

În tabelul 6 sunt prezentate prețurile materiilor prime necesare sistemului robotic.

Tabel 6 – Prețurile materiilor prime necesare fabricării componentelor specifice

Cheltuielile cu materiile prime se obțin însumând prețul total al fiecărui produs din tabel.

(6.5)

Cheltuielile cu unitățile se rezumă în special consumul de curent electric. Acesta se estimează la o valoare de aproximativ 250 de lei pe lună și se raportează la numărul de roboți construiți pe lună.

(6.6)

6.3 Calcularea cheltuielilor cu resursele umane

În tabelul 7 sunt prezentate operațiile care necesită resurse umane pentru a fi realizate.

Tabel 7 – Costurile de timp ale operațiilor de prelucrare necesare

Cheltuielile cu resursele umane se calculează în ecuația de mai jos:

(6.7)

unde:

– cheltuieli salariale;

– cheltuieli cu salariu brut;

Pentru realizarea operațiilor din tabel este nevoie de un tehnician mecanic cu un salariu brut de aproximativ 2200 lei pe lună. Sumele datorate bugetului de stat pentru cheltuielile salariale este egal cu 4400 lei.

(6.8)

6.4 Calcularea amortizării echipamentelor

Datorită costului ridicat al echipamentelor, amortizarea acestora se realizează în timp. În tabelul de mai jos sunt prezentate echipamentele necesare realizării operațiilor.

Tabel 8 – Prețurile echipamentelor și amortizarea lunară a acestora

Cheltuielile cu amortizarea se obțin însumând amortizarea lunară a fiecărui echipament din tabel.

(6.9)

6.5 Calcularea profitului

Din calculele de mai sus, cu un tehnician angajat vom avea o producție de 15 roboți pe lună. Astfel cheltuielile cu unitățile vor fi egale:

(6.10)

Se calculează cheltuielile materiale:

(6.11)

Se calculează cheltuielile cu resursele umane:

(6.12)

Se calculează cheltuielile cu amortizarea:

(6.13)

Se calculează cheltuielile totale:

(6.14)

Pentru calcularea prețului de vânzare al unui robot se stabilește rata profitului ca fiind egală cu 21%.

(6.15)

Se calculează profitul lunar impozabil:

(6.16)

Se calculează impozitul datorat bugetului de stat:

(6.17)

Se calculează profitul final lunar:

(6.18)

În concluzie pentru a produce în serie acest sistem robotic trebuie să analizăm bine piața si potențialii clienți. Cu o investiție inițială de aproximativ 1300 lei se poate obține un profit de aproximativ 3500 de lei pe lună.

7. Parametrii tehnico-funcționali obținuți. Concluzii

În lucrarea de față a fost obținută o structură robotică ce se echilibrează dinamic pe o sferă cu o poziție blocată. Această structură se poate deplasa pe sol, urmărind o referință de distanță pe cele două axe și o orientare la destinație. Viteza este controlată constant, în funcție de distanța rămasă de parcurs și limitată pentru a preveni răsturnarea. Viteza maximă la sol este de 2.4 m/s. Factorul inovator constă în construcția ce permite poziționarea reglabilă a motoarelor ce permite robotului să funcționeze similar pe sfere cu diametre cuprinse între 150 – 400 mm.

Una din îmbunătățirile care ar putea fi adusă sistemului pentru a permite echilibrarea dinamică pe sfera lăsată liberă este utilizarea unei structuri de reglare îmbunătățite, de exemplu un regulator optimal sau unul bazat pe logică fuzzy. Aceste structuri, deși mai dificil de proiectat și implementat, au performanțe și comportamente mai bune decât cele bazate pe legi PID.

În momentul de față, echilibrarea se realizează cu mișcări relativ bruște, care influențează negativ stabilitatea robotului. Acest fapt se datorează saturației inferioare pe care o prezintă motoarele de curent continuu, deci aceasta poate fi îndepărtată numai prin folosirea unui alt tip de motor.

Schimbările bruște de sens cu viteze relativ mari produc multe vibrații. Aceste vibrații au ca efect slăbirea șuruburilor din structura mecanică după o perioadă de funcționare. Pentru atenuarea efectului acestor vibrații este necesară o structură mecanică mai rigidă. O altă soluție poate fi introducerea unor arcuri lamelare în ansamblul motoarelor, între piesa mobilă și cea fixă, pentru a atenua și mai mult șocurile introduse de aceste vibrații.

Bibliografie

1. Isermann R.: Integrierte mechanisch-elektronische Systeme. VDI-Z, nr 10, 1993

2. Bishop, Robert H., Mechatronics: an introduction. CRC Press, 2006

3. Standardul NF E 01-010

4. Godfrey C., Mechatronics: principles and applications. Butterworth-Heinemann, 2005

5. De Silva, Clarence W., Mechatronics: an integrated approach. CRC Press, 2005

6. K Yamafuji, T Kawamura – Postural control of a monoaxial bicycle, Journal of the Robotics Society of Japan, 1989

7. Y.-S. Ha and S. Yuta. – Trajectory tracking control for navigation of self contained mobile inverse pendulum, 1994

8. T. Lauwers, G. Kantor and R. Hollis – One is Enough!, 2005

9. T. B. Lauwers, G. A. Kantor, and R. L. Hollis – A Dynamically Stable Single-Wheeled Mobile Robot with Inverse Mouse-Ball Drive, 2006

10. T. Endo, Y. Nakamura – An Omnidirectional Vehicle on a Basketball, 2005

11. M. Kumagai, T. Ochiai – Development of a Robot Balanced on a Ball – First Report, Implementation of the Robot and Basic Control, 2009

12. J. Fong, S. Uppill – Ballbot – Preliminary Report, 2009

13. Y. Yamamoto – NXT Ballbot (Self-Balancing Robot On A Ball) Controller Design

14. Team Ballbot – Rezero – dynamisch stabil auf einer Kugel. Report, Swiss Federal Institute of Technology, Switzerland, 2010

15. P. Fankhauser, C. Gwerder – Modeling and Control of a Ballbot

16. D. Yang, E. Sihite, J. M. Friesen, T. Bewley – Design and control of a micro ball-balancing robot with orthogonal midlatitude omniwheel placement, UCSD Robotics Lab, 2015

17. A. Bhatia, M. Kumagai, R. Hollis – Six-Stator Spherical Induction Motor for Balancing Mobile Robots

18. G. Seyfarth – Design, construction, and evaluation of a ballbot with a spherical induction motor

19. I. Dumitrache – Ingineria Reglării automate, note de curs

20. I. Calciu, M. Pătrașcu, L.M. Vasilescu, A. Drăghici, D. Rosner – Three Omni-wheel Ballbot Optimum Implementation, 2015

21. M. Pătrașcu – Sisteme de conducere în robotică, note de curs

22. B. Grămescu – Proiectarea sistemelor mecatronice, note de curs

Anexe

Anexa 1 – Codul Arduino pentru testare driverelor, motoarelor și encoderelor

//Declararea pinilor

#define encoder1A 19

#define encoder1B 16

#define encoder2A 2

#define encoder2B 3

#define encoder3A 18

#define encoder3B 17

#define EN1 47

#define INA1 45

#define INB1 43

#define PWM1 10

#define EN2 35

#define INA2 33

#define INB2 31

#define PWM2 9

#define EN3 41

#define INA3 39

#define INB3 37

#define PWM3 8

volatile long encoder1Pos = 0;

volatile long encoder2Pos = 0;

volatile long encoder3Pos = 0;

long newposition1, newposition2, newposition3;

long oldposition1 = 0;

long oldposition2 = 0;

long oldposition3 = 0;

unsigned long newtime;

unsigned long oldtime = 0;

long vel1, vel2, vel3;

void setup()

{

//Setarea pinilor pentru drivere

pinMode(INA1, OUTPUT);

pinMode(INB1, OUTPUT);

pinMode(EN1, OUTPUT);

pinMode(PWM1, OUTPUT);

pinMode(INA2, OUTPUT);

pinMode(INB2, OUTPUT);

pinMode(EN2, OUTPUT);

pinMode(PWM2, OUTPUT);

pinMode(INA2, OUTPUT);

pinMode(INB2, OUTPUT);

pinMode(EN2, OUTPUT);

pinMode(PWM2, OUTPUT);

//Setarea pinilor pentru encodere

pinMode(encoder1A, INPUT);

digitalWrite(encoder1A, HIGH);

pinMode(encoder1B, INPUT);

digitalWrite(encoder1B, HIGH);

attachInterrupt(4, doEncoder1, RISING);

pinMode(encoder2A, INPUT);

digitalWrite(encoder2A, HIGH);

pinMode(encoder2B, INPUT);

digitalWrite(encoder2B, HIGH);

attachInterrupt(0, doEncoder2, RISING);

pinMode(encoder3A, INPUT);

digitalWrite(encoder3A, HIGH);

pinMode(encoder3B, INPUT);

digitalWrite(encoder3B, HIGH);

attachInterrupt(5, doEncoder3, RISING);

//Activarea driverelor

digitalWrite(EN1, HIGH);

digitalWrite(EN2, HIGH);

digitalWrite(EN3, HIGH);

Serial.begin (9600);

Serial.println("start");

}

//Valoarea PWM de testare

int comanda = 100;

void loop()

{

//Scrierea comenzii pe driver și determinarea sensului de rotație

digitalWrite(INA1, HIGH);

digitalWrite(INB1, LOW);

analogWrite(PWM1, comanda);

digitalWrite(INA2, HIGH);

digitalWrite(INB2, LOW);

analogWrite(PWM2, comanda);

digitalWrite(INA3, HIGH);

digitalWrite(INB3, LOW);

analogWrite(PWM3, comanda);

//Citirea valorii encoderelor

newposition1 = encoder1Pos;

newposition2 = encoder2Pos;

newposition3 = encoder3Pos;

newtime = millis();

//Calculul vitezelor encoderelor

vel1 = newposition1 – oldposition1;

vel2 = newposition2 – oldposition2;

vel3 = newposition3 – oldposition3;

Serial.print (millis());

Serial.print (",");

Serial.print (vel3);

Serial.print (",");

Serial.println (comanda);

oldposition1 = newposition1;

oldposition2 = newposition2;

oldposition3 = newposition3;

oldtime = newtime;

delay(250);

}

//Funcțiile care se execută atunci când pinul de întrerupere

//detectează o schimbare de stare

void doEncoder1()

{

if (digitalRead(encoder1A) == digitalRead(encoder1B))

{

encoder1Pos++;

} else {

encoder1Pos–;

}

}

void doEncoder2()

{

if (digitalRead(encoder2A) == digitalRead(encoder2B))

{

encoder2Pos++;

} else {

encoder2Pos–;

}

}

void doEncoder3()

{

if (digitalRead(encoder3A) == digitalRead(encoder3B))

{

encoder3Pos++;

} else {

encoder3Pos–;

}

}

Anexa 2 – Algoritmul de echilibrare

#define encoder1A 19

#define encoder1B 16

#define encoder2A 2

#define encoder2B 3

#define encoder3A 18

#define encoder3B 17

#define EN1 47

#define INA1 45

#define INB1 43

#define PWM1 10

#define EN2 35

#define INA2 33

#define INB2 31

#define PWM2 9

#define EN3 41

#define INA3 39

#define INB3 37

#define PWM3 8

#include <Wire.h>

#include <Adafruit_Sensor.h>

#include <Adafruit_BNO055.h>

#include <utility/imumaths.h>

Adafruit_BNO055 bno = Adafruit_BNO055(55);

volatile long encoder1Pos = 0;

volatile long encoder2Pos = 0;

volatile long encoder3Pos = 0;

long newposition1, newposition2, newposition3;

long oldposition1 = 0;

long oldposition2 = 0;

long oldposition3 = 0;

unsigned long newtime;

unsigned long oldtime = 0;

float vel1, vel2, vel3;

float rx, ry;

float ux, uy;

float u1, u2, u3;

float ex, ey;

static float prev_rx, prev_ry;

float deriv1, deriv2, deriv3;

short comanda1, comanda2, comanda3;

const float pi = 3.14159;

int Kp1 = 5;

int Kp2 = 5;

int Kp3 = 5;

int Kd1 = 1;

int Kd2 = 1;

int Kd3 = 1;

void setup()

{

pinMode(encoder1A, INPUT);

digitalWrite(encoder1A, HIGH);

pinMode(encoder1B, INPUT);

digitalWrite(encoder1B, HIGH);

attachInterrupt(4, doEncoder1, RISING);

pinMode(encoder2A, INPUT);

digitalWrite(encoder2A, HIGH);

pinMode(encoder2B, INPUT);

digitalWrite(encoder2B, HIGH);

attachInterrupt(0, doEncoder2, RISING);

pinMode(encoder3A, INPUT);

digitalWrite(encoder3A, HIGH);

pinMode(encoder3B, INPUT);

digitalWrite(encoder3B, HIGH);

attachInterrupt(5, doEncoder3, RISING);

pinMode(INA1, OUTPUT);

pinMode(INB1, OUTPUT);

pinMode(EN1, OUTPUT);

pinMode(PWM1, OUTPUT);

pinMode(INA2, OUTPUT);

pinMode(INB2, OUTPUT);

pinMode(EN2, OUTPUT);

pinMode(PWM2, OUTPUT);

pinMode(INA2, OUTPUT);

pinMode(INB2, OUTPUT);

pinMode(EN2, OUTPUT);

pinMode(PWM2, OUTPUT);

digitalWrite(EN1, HIGH);

digitalWrite(EN2, HIGH);

digitalWrite(EN3, HIGH);

Serial.begin (1000000);

Serial.println("start");

if (!bno.begin())

{

Serial.print("Ooops, no BNO055 detected … Check your wiring or I2C ADDR!");

while (1);

}

bno.setExtCrystalUse(true);

}

#define CO -0.5

#define SN -0.866

#define INTARZIERE 6

float hist_x[INTARZIERE], hist_y[INTARZIERE];

int idx = 0;

#define INTARZIERE_V 6

float hist_1[INTARZIERE_V], hist_2[INTARZIERE_V], hist_3[INTARZIERE_V];

#define max_int 0.7

float factor = 1.5; //mm pe impuls

void loop()

{

sensors_event_t imu;

bno.getEvent(&imu);

newposition1 = encoder1Pos;

newposition2 = encoder2Pos;

newposition3 = encoder3Pos;

newtime = millis();

vel1 = (newposition1 – oldposition1) * 50;

vel2 = (newposition2 – oldposition2) * 50;

vel3 = (newposition3 – oldposition3) * 50;

hist_1[idx] = vel1;

hist_2[idx] = vel2;

hist_3[idx] = vel3;

vel1 = 0;

vel2 = 0;

vel3 = 0;

for (int i = 0; i < INTARZIERE_V; i++)

{

vel1 += hist_1[i];

vel2 += hist_2[i];

vel3 += hist_3[i];

}

vel1 = vel1 / INTARZIERE_V * factor / 2400;

vel2 = vel2 / INTARZIERE_V * factor / 2400;

vel3 = vel3 / INTARZIERE_V * factor / 2400;

//Citirea senzorului de orientare

float thx = -imu.orientation.y;

float thy = imu. orientation.z;

hist_x[idx] = rx;

hist_y[idx] = ry;

float thxd = thx – hist_x[(idx + 1) % INTARZIERE];

float thyd = thy – hist_y[(idx + 1) % INTARZIERE];

float k1 = -90;

float k2 = 65;

float k3 = 0.01;

//Regulatorul axei X

float err_x = -thx;

static float old_err_x = 0;

static float old_cmd_x = 0;

float cmd_x = k1 * err_x + k2 * old_err_x – k3 * old_cmd_x;

old_cmd_x = cmd_x;

//Regulatorul axei Y

float err_y = -thy;

static float old_err_y = 0;

static float old_cmd_y = 0;

float cmd_y = k1 * err_y + k2 * old_err_y – k3 * old_cmd_y;

old_cmd_y = cmd_y;

//**********************************************

//Transformarea de coordonate

float rp1 = -0.8660254037844386 * cmd_y – 0.5 * cmd_x;

float rp2 = 0.8660254037844386 * cmd_y – 0.5 * cmd_x;

float rp3 = 0 * cmd_y + 1 * cmd_x;

float comanda1 = constrain(nelin(rp1 * 255 / 1000), -255, 255);

float comanda2 = constrain(nelin(rp2 * 255 / 1000), -255, 255);

float comanda3 = constrain(nelin(rp3 * 255 / 1000), -255, 255);

digitalWrite(INA1, (comanda1 > 0));

digitalWrite(INB1, (comanda1 < 0));

analogWrite(PWM1, abs(comanda1));

digitalWrite(INA2, (comanda2 > 0));

digitalWrite(INB2, (comanda2 < 0));

analogWrite(PWM2, abs(comanda2));

digitalWrite(INA3, (comanda3 > 0));

digitalWrite(INB3, (comanda3 < 0));

analogWrite(PWM3, abs(comanda3));

Serial.print (micros()); Serial.print (","); //timp

Serial.print (thx); Serial.print (","); //inclinare pe x

Serial.print (thy); Serial.print (","); //inclinare pe y

Serial.print (comanda1); Serial.print (",");

Serial.print (comanda2); Serial.print (",");

Serial.println (comanda3); //Serial.print (",");

idx++;

if (idx >= INTARZIERE)

{

idx = 0;

}

oldposition1 = newposition1;

oldposition2 = newposition2;

oldposition3 = newposition3;

oldtime = newtime;

delay(20);

}

inline int nelin (int u)

{

if (u == 0)

{

return 0;

}

if (abs(u) < 50)

{

return 50 * ((u < 0) ? -1 : 1);

}

if (abs(u) > 255)

{

return 255 * ((u < 0) ? -1 : 1);

}

return u;

}

float sign(float x)

{

return x < 0 ? -1 : 1;

}

void doEncoder1()

{

if (digitalRead(encoder1A) == digitalRead(encoder1B))

{

encoder1Pos–;

} else {

encoder1Pos++;

}

}

void doEncoder2()

{

if (digitalRead(encoder2A) == digitalRead(encoder2B))

{

encoder2Pos–;

} else {

encoder2Pos++;

}

}

void doEncoder3()

{

if (digitalRead(encoder3A) == digitalRead(encoder3B))

{

encoder3Pos++;

} else {

encoder3Pos–;

}

}

Anexa 3 – Algoritmul de deplasare

#define encoder1A 19

#define encoder1B 16

#define encoder2A 2

#define encoder2B 3

#define encoder3A 18

#define encoder3B 17

#define EN1 47

#define INA1 45

#define INB1 43

#define PWM1 10

#define EN2 35

#define INA2 33

#define INB2 31

#define PWM2 9

#define EN3 41

#define INA3 39

#define INB3 37

#define PWM3 8

volatile long encoder1Pos = 0;

volatile long encoder2Pos = 0;

volatile long encoder3Pos = 0;

long newposition1, newposition2, newposition3;

long oldposition1 = 0;

long oldposition2 = 0;

long oldposition3 = 0;

unsigned long newtime;

unsigned long oldtime = 0;

float vel1, vel2, vel3;

short comanda1, comanda2, comanda3;

const float pi = 3.14159;

void setup()

{

pinMode(encoder1A, INPUT);

digitalWrite(encoder1A, HIGH); // turn on pullup resistor

pinMode(encoder1B, INPUT);

digitalWrite(encoder1B, HIGH); // turn on pullup resistor

attachInterrupt(4, doEncoder1, RISING);

pinMode(encoder2A, INPUT);

digitalWrite(encoder2A, HIGH); // turn on pullup resistor

pinMode(encoder2B, INPUT);

digitalWrite(encoder2B, HIGH); // turn on pullup resistor

attachInterrupt(0, doEncoder2, RISING);

pinMode(encoder3A, INPUT);

digitalWrite(encoder3A, HIGH); // turn on pullup resistor

pinMode(encoder3B, INPUT);

digitalWrite(encoder3B, HIGH); // turn on pullup resistor

attachInterrupt(5, doEncoder3, RISING);

pinMode(INA1, OUTPUT);

pinMode(INB1, OUTPUT);

pinMode(EN1, OUTPUT);

pinMode(PWM1, OUTPUT);

pinMode(INA2, OUTPUT);

pinMode(INB2, OUTPUT);

pinMode(EN2, OUTPUT);

pinMode(PWM2, OUTPUT);

pinMode(INA2, OUTPUT);

pinMode(INB2, OUTPUT);

pinMode(EN2, OUTPUT);

pinMode(PWM2, OUTPUT);

digitalWrite(EN1, HIGH);

digitalWrite(EN2, HIGH);

digitalWrite(EN3, HIGH);

Serial.begin (250000);

Serial.println("start");

}

#define INTARZIERE 6

float hist_1[INTARZIERE], hist_2[INTARZIERE], hist_3[INTARZIERE];

int idx = 0;

#define max_int 1

float factor = 1.5;

void loop()

{

newposition1 = encoder1Pos;

newposition2 = encoder2Pos;

newposition3 = encoder3Pos;

newtime = millis();

vel1 = (newposition1 – oldposition1) * 50;

vel2 = (newposition2 – oldposition2) * 50;

vel3 = (newposition3 – oldposition3) * 50;

hist_1[idx] = vel1;

hist_2[idx] = vel2;

hist_3[idx] = vel3;

vel1 = 0;

vel2 = 0;

vel3 = 0;

for (int i = 0; i < INTARZIERE; i++)

{

vel1 += hist_1[i];

vel2 += hist_2[i];

vel3 += hist_3[i];

}

vel1 = vel1 / INTARZIERE * factor / 2400;

vel2 = vel2 / INTARZIERE * factor / 2400;

vel3 = vel3 / INTARZIERE * factor / 2400;

float factor = 1.5; //mm pe impuls

//**********************************************

//Referințele de pozitie

float ref_p_x = 0;

float ref_p_y = 0;

float ref_p_t = 0;

if(millis() > 3000)

{

ref_p_x = 500;

ref_p_y = 500;

}

//**********************************************

//Transformarea de coordonate

float rp1 = -0.8660254037844386 * ref_p_y – 0.5 * ref_p_x + ref_p_t;

float rp2 = 0.8660254037844386 * ref_p_y – 0.5 * ref_p_x + ref_p_t;

float rp3 = 0 * ref_p_y + 1 * ref_p_x + ref_p_t;

//**********************************************

//Parametri regulatoarelor

float Kp1 = 0.01;

float Kd1 = 0.15;

float Kp2 = 0.01;

float Kd2 = 0.15;

float Kp3 = 0.01;

float Kd3 = 0.15;

//**********************************************

//REGULATOR MOTOR 1

float ref_p_1 = rp1;

float ies_p_1 = newposition1 * factor;

static float old_ies_p_1 = 0;

float err_p_1 = ref_p_1 – ies_p_1;

float cmd_p_1 = err_p_1 * Kp1 – (ies_p_1-old_ies_p_1) * Kd1;

old_ies_p_1 = ies_p_1;

//**********************************************

//REGULATOR MOTOR 2

float ref_p_2 = rp2;

float ies_p_2 = newposition2 * factor;

static float old_ies_p_2 = 0;

float err_p_2 = ref_p_2 – ies_p_2;

float cmd_p_2 = err_p_2 * Kp2 – (ies_p_2-old_ies_p_2) * Kd2;

old_ies_p_2 = ies_p_2;

//**********************************************

//REGULATOR MOTOR 3

float ref_p_3 = rp3;

float ies_p_3 = newposition3 * factor;

static float old_ies_p_3 = 0;

float err_p_3 = ref_p_3 – ies_p_3;

float cmd_p_3 = err_p_3 * Kp3 – (ies_p_3-old_ies_p_3) * Kd3;

old_ies_p_3 = ies_p_3;

//**********************************************

//Referințele de viteză

float rv1 = constrain(cmd_p_1, -0.1, 0.1);

float rv2 = constrain(cmd_p_2, -0.1, 0.1);

float rv3 = constrain(cmd_p_3, -0.1, 0.1);

//**********************************************

// REGULATOR VITEZĂ MOTOR 1

float ref_v_1 = rv1;

float ies_v_1 = vel1;

float err_v_1 = rv1 – vel1;

float cmd_v_1 = 0;

static float int_v_1 = 0;

float k_1 = 11.28 – 0.009 * ref_v_1;

float next_int_v_1 = int_v_1 + err_v_1 * 0.4 / k_1;

if (next_int_v_1 > max_int) int_v_1 = max_int;

else if (next_int_v_1 < -max_int) int_v_1 = -max_int;

else int_v_1 = next_int_v_1;

cmd_v_1 = 2 * err_v_1 / k_1 + int_v_1;

//**********************************************

// REGULATOR VITEZĂ MOTOR 2

float ref_v_2 = rv2;

float ies_v_2 = vel2;

float err_v_2 = rv2 – vel2;

float cmd_v_2 = 0;

static float int_v_2 = 0;

float k_2 = 6.6 – 0.005 * ref_v_2;

float next_int_v_2 = int_v_2 + err_v_2 * 0.4 / k_2;

if (next_int_v_2 > max_int) int_v_2 = max_int;

else if (next_int_v_2 < -max_int) int_v_2 = -max_int;

else int_v_2 = next_int_v_2;

cmd_v_2 = 2 * err_v_2 / k_2 + int_v_2;

//**********************************************

// REGULATOR VITEZĂ MOTOR 3

float ref_v_3 = rv3;

float ies_v_3 = vel3;

float err_v_3 = rv3 – vel3;

float cmd_v_3 = 0;

static float int_v_3 = 0;

float k_3 = 5.76 – 0.004 * ref_v_3;

float next_int_v_3 = int_v_3 + err_v_3 * 0.4 / k_3;

if (next_int_v_3 > max_int) int_v_3 = max_int;

else if (next_int_v_3 < -max_int) int_v_3 = -max_int;

else int_v_3 = next_int_v_3;

cmd_v_3 = 2 * err_v_3 / k_3 + int_v_3;

float comanda1 = constrain(cmd_v_1 * 255, -255, 255);

float comanda2 = constrain(cmd_v_2 * 255, -255, 255);

float comanda3 = constrain(cmd_v_3 * 255, -255, 255);

digitalWrite(INA1, (comanda1 > 0));

digitalWrite(INB1, (comanda1 < 0));

analogWrite(PWM1, abs(comanda1));

digitalWrite(INA2, (comanda2 > 0));

digitalWrite(INB2, (comanda2 < 0));

analogWrite(PWM2, abs(comanda2));

digitalWrite(INA3, (comanda3 > 0));

digitalWrite(INB3, (comanda3 < 0));

analogWrite(PWM3, abs(comanda3));

Serial.print ("r = ");

Serial.print (ref_v_1);

Serial.print ("\t y = ");

Serial.print (ies_p_1);

Serial.print ("\t u = ");

Serial.print (comanda1);

Serial.print ("\t k1 = ");

Serial.println (k_1);

idx++;

if (idx >= INTARZIERE)

{

idx = 0;

}

oldposition1 = newposition1;

oldposition2 = newposition2;

oldposition3 = newposition3;

oldtime = newtime;

delay(20);

}

void doEncoder1()

{

if (digitalRead(encoder1A) == digitalRead(encoder1B))

{

encoder1Pos–;

} else {

encoder1Pos++;

}

}

void doEncoder2()

{

if (digitalRead(encoder2A) == digitalRead(encoder2B))

{

encoder2Pos–;

} else {

encoder2Pos++;

}

}

void doEncoder3()

{

if (digitalRead(encoder3A) == digitalRead(encoder3B))

{

encoder3Pos++;

} else {

encoder3Pos–;

}

}

Anexa 4 – Desenul de execuție al barei hexagonale