Realizarea Sistemului Mobil Autonom

Cuprins

1. INTRODUCERE

Sistemele mobile autonome sunt sisteme mecatronice inteligente cu rolul de a ușura munca omului, capabile să ia singure decizii în scopul efectuării unor anumite sarcini.

Sistemele mobile tip line follower sunt sisteme autonome create special pentru a urmării trasee marcate cu o culoare diferită față de teren. Acestea au o importantă aplicabilitate în industrie, pentru transportarea diferitelor componente, iar, la nivel academic, reprezintă obiectul multor competiții.

Prezentul proiect de diplomă își propune realizarea unui sistem mobil autonom de tip line follower, în vederea selecției pentru participarea la Olimpiada Națională de Mecatronica 2013, secțiunea Roboți Mobili.

Lucrarea este structurată pe 6 capitole, o scurtă descriere a acestora fiind prezentată în continuare.

După introducerea de față, capitolul al doilea, intitulat “Stadiul actual al cercetărilor și realizărilor privind sistemele mobile autonome comandate de microcontroller”, descrie două dintre cele mai avansate sisteme autonome de actualitate urmate de o introducere în cadrul sistemelor tip line follower, exemplificând trei astfel de proiecte.

Capitolul al treilea, “Considerații teoretice privind sistemele autonome comandate de microcontroller”, prezintă structura generală a unui sistem mobil autonom și descrie, în detaliu, elementele componente:

Subsistemul mecanic: tipuri de ghidaje, tipuri de motoare cu sau fără reductoare;

Subsistemul electronic: microcontroller-e, driver-e de motoare, senzori, interfețe utilizator, surse de curent;

Subsistemul software: modul de citire a poziției și controlul direcției.

Capitolul al patrulea, intitulat “Proiectarea sistemului mobil autonom”, este cel mai consistent din cadrul proiectului. Începând de la condițiile de proiectare, în vederea participării la competiție, sunt concepute: subsistemul hardware ce are în componență soluția pentru deplasare, șasiul, sursa de curent, senzorii și placa de dezvoltare Arduino Mega; subsistemul electronic pentru comanda motoarelor și interfațarea utilizator(fizică/wireless); subsistemul software care cuprinde programele de funcționare pentru sistemul cu microcontroller.

În capitolul 5, “Realizarea sistemului autonom”, este descrisă realizarea pas cu pas a sistemului, precum și modul de asamblare și utilizare al acestuia.

Proiectul se încheie cu concluziile finale, contribuții personale și perspective de dezvoltare.

Prezenta lucrare se dorește a fi un punct de plecare în dezvoltarea ulterioară a sistemelor mobile tip line follower performante. Pe parcursul proiectării, atât subsistemului hardware cât și software, s-a urmărit stabilirea unor soluții ușor de adaptat în cadrul altor proiecte dar și studierea posibilității anticipării traseului folosind senzori antemergători.

2. STADIUL ACTUAL AL CERCETĂRILOR ȘI REALIZĂRILOR PRIVIND SISTEMELE MOBILE AUTONOME COMANDATE DE MICROCONTROLLER

Sistemele mobile autonome sunt sisteme mecatronice ce se pot deplasa într-un mediu variabil, fiind capabile de adaptare la factori externi fără intervenția continuă a omului, cu scopul de a exercita anumite forțe asupra mediului sau de monitoriza anumite aspecte ale acestuia. Aceștia se deplasează folosind mijloace proprii și sunt, în general, folosiți pentru explorare, transport, cercetare, mentenanță, reparații sau chiar divertisment.

Autonomia unui astfel de sistem este dată de capacitatea acestuia de a opera în medii cât mai puțin reglementate pentru perioade îndelungate de timp fără intervenție umană.

Un robot complet autonom trebuie să îndeplinească următoarele condiții :

Să obțină informații despre mediul său înconjurător;

Să funcționeze o perioadă îndelungată de timp fără intervenția omului;

Să se deplaseze în zona de lucru fără asistență umană;

Să evite situații în care ar putea fi dăunător oamenilor, obiectelor din jur sau sie însuși.

La nivel mondial a fost realizată o diversitate de roboți autonomi capabili să lucreze subacvatic, aerian și terestru. Deplasarea terestră este cel mai des realizată cu roți sau șenile dar sunt și cazuri în care au fost copiate soluțiile din natură și au rezultat roboți pășitori, târâtori sau săritori.

În continuare vom prezenta două dintre cele mai avansate sisteme mobile autonome la nivel internațional și patru sisteme tip line follower ce au avut succes la competițiile de profil.

2.1 Robotul de transport „BigDog”

Robotul „BigDog” (Fig.1) realizat de către cei de la Boston Dynamics, se deplasează folosind 4 picioare, asemănător mersul patruped . Scopul unui astfel de robot este să transporte provizii traversând medii dificile precum păduri, râuri, zone înzăpezite unde roboții clasici cu roți nu s-ar fi descurcat. Mișcările sunt generate de cilindrii hidraulici alimentați de la un motor cu combustie internă în 2 timpi ce dezvoltă 11,2 KW. Viteza maximă atinsă în laborator este de 3,1 m/s. Robotul are aproape 50 de senzori ce măsoară accelerația corpului, forțele și mișcările din articulații, presiunea, temperatura și debitul sistemului hidraulic precum și viteza și temperatura motorului cu combustie.

Robotul este în general condus de către un operator ce îi transmite wireless comenzi de tipul: direcției, vitezei, start/stop motor iar computerul onboard controlează direct actuatorii. Folosind un sistem bazat pe două camere video, robotul se poate deplasa și autonom sau poate urmării persoana din fața sa.

2.2 Robotul de explorare „Curiosity”

În 2011 NASA a trimis roverul “Curiosity” (Fig. 2) pe suprafața planetei Marte cu scopul de a o studia din punct de vedere biologic și chimic, de a urmări nivelul de radiații și evoluția atmosferei. Întregul robot are dimensiunea unui automobil, iar pentru a se deplasa folosește 6 roți motoare de 50 cm diametru, dintre care 4, cele din față și din spate, sunt viratoare. Deși viteza maximă este de 90 m/h, viteza medie de deplasare este de 30 m/h.

Robotul folosește două camere video alb-negru ce formează imagini 3D pentru navigație și alte patru perechi de camere similare pentru monitorizarea mediul înconjurător de eventuale pericole.

În 2012 misiunea sa de doi ani a fost prelungită pe termen nelimitat.

2.3 Roboți tip line follower

Un robot tip line follower este un sistem mobil autonom capabil să urmărească o linie de culoare diferită față de suprafața de rulare. În industrie, acest tip de robot este adesea folosit pentru transport în cadrul halelor de producție sau depozitelor. Urmărirea liniei se face folosind camere video sau o serie de senzori în infraroșu ce transmit semnale unității de comandă și control integrate.

La nivel academic întâlnim roboți similari, construiți în scopuri educaționale. Un astfel de sistem implică cunoștințe în cele 3 domenii ce definesc disciplina de mecatronică: mecanică, electronică și software.

În continuare sunt prezentate diferite modele de roboți de tip line follower

2.3.1 Robotul Zero

Realizat în scopuri educaționale de către “Club de Informática, robótica y Electrónica” din Madrid, robotul (Fig. 3) folosește componente Pololu și o construcție foarte ușoară, 104g, pentru a atinge o viteză medie de 2.10m/s .

Placa de control folosită este un Baby Orangutan B-328 ce înglobează un microcontroller ATmega328P și un driver de motoare TB6612FNG cu 2 canale. Motoarele, 10:1 Micro Metal Gearmotor HP, dezvoltă o turație în gol de 3000RPM iar împreună cu roțile de diametru 32 mm rezultă o viteză maximă teoretică de 5.02 m/s. Pentru a ajuta motoarele este folosit un stabilizator ridicător de tensiune ce ridică tensiunea de la 3.7 V, de la baterii, la 6.4 V. Raportat la masa sa, robotul are o accelerație teoretică de 16,97 m/s2.

Șasiul este realizat din PVC, are o lățime de 112 mm, în zona de montare a motoarelor, o lungime de 220 mm, iar ca punct de sprijin folosește un ball caster.

2.3.2 Robotul Jet

Realizat de către David Cook, robotul (Fig. 4) a fost construit pentru a participa la concursul „ChiBots Spring 2005” unde a reușit să atingă o viteză medie de 1,15 m/s.

Robotul folosește două motoare Maxon, diametru ϕ13mm, 1,2W 6V, cu o turație în gol de 16500RPM. Acestora le sunt montate reductoare cu raport de 17:1 și le este mărită tensiunea de alimentare cu 40% ajungând la 8.4V. Toate acestea duc la o turație în gol de 1359RPM. Cuplate la roți LEGO cu diametru ϕ30,4mm și ignorând masa acestuia de 212g, rezultă o viteză maximă teoretică de 2,16 m/s.

Senzorii în infraroșu sunt dispuși în două linii, una în fața și alta în spatele roților (Fig. 4b). Ce are aparte acest robot este absența unei roți/bile de sprijin, în schimb acesta folosește distanțiere din nailon care au și rolul de a proteja senzorii de surse exterioare de lumină.

Robotul nu dispune de butoane sau comutatoare iar comenzile sunt transmise fie printr-o telecomandă cu infraroșu fie prin comunicație serială pe fir sau wireless.

2.3.3 Robotul Pololu 3pi

Realizat de către compania Pololu, acesta reprezintă o soluție ideală pentru cei care caută un robot programabil cu o platformă flexibilă. (Fig. 6)

Șasiul robotului îl reprezintă placa de electronică ce include: microcontroller-ul ATmega 328, driverul de motoare TB6612FNG, stabilizatoare de tensiune, interfața de programare, interfața utilizator și 5 senzori cu infraroșu.

Specificațiile microcontroller-ul ATmega328 sunt următoarele:

Memorie program: 32KB;

Memorie RAM: 2KB;

Frecvență oscilator: 20 MHz;

Intrări analogice: 8;

Intrări/ieșiri digitale: max. 23.

Specificațiile pentru driver-ul de motoare TB6612FNG sunt:

2 canale de control (poate controla două motoare);

Tensiune output: max. 15V;

Curent output: 1.2A.

Robotul folosește două stabilizatoare de tensiune. Primul este unul în comutație ce ridică tensiunea bateriilor de maxim 6 V la 9,25 V. Această este folosită și pentru alimentarea motoarelor și ca sursă de intrare pentru stabilizatorul liniar ce coboară tensiunea la 5V pentru componentele digitale (microcontroller, senzori, ecran LCD…).

În afară de două butoane de start/stop și reset, interfața utilizator este realizată prin 3 butoane ce pot fi configurate de utilizator și un ecran LCD 8×2. Programarea sistemului cu ajutorul programatorului AVR USB (Fig. 7) conectat la portul ISP.

Pentru a se deplasa, robotul folosește două motoare 30:1 Micro Metal Gearmotor MP cuplate la roți cu diametrul de 30mm. Viteza maximă teoretică rezultată este de 1.1m/s și o accelerație de 14,1m/s2. Producătorii susțin că, în practică, robotul poate ajunge la o viteză de 0,9m/s.

Construcția robotului este una circulară, având diametru de 95 mm și o masă totală aproximativ 200g.

2.3.4 Robotul Silvestre

Realizat de către Daniel Alvarez, Spania, este unul dintre cei mai avansați roboți tip line follower (Fig. 8).

Caracteristicile robotului sunt:

8 senzori IR;

2 motoare de curent continuu Maxon cu encoder;

Modul Bluetooth pentru telemetrie și configurare wireless;

Microcontroller LPC2148 ARM7 32-bit cu frecvența de 60Mhz;

Accelerometru și giroscop pentru a detecta alunecările și urmării performanțele robotului;

După două ture de traseu robotul extrage punctele de frânare și accelerare.

2.4 Concluzii, motivul abordării temei și obiectivele proiectului

În urma analizei stadiului actual asupra cercetărilor și realizărilor privind sistemele mobile autonome comandate de microcontroller se desprind următoarele concluzii:

Sistemele de acest tip sunt destinate atât mediului industrial cât și mediului academic;

Pentru realizarea unor sisteme performante de competiție se folosesc:

Microcontroller-e cu frecvență de lucru ridicată, care încorporează linii de intrare-ieșire digitale, linii de intrare pentru semnale analogice provenite de la senzori de urmărire de linie, linii de ieșire pentru semnale PWM folosite la comanda motoarelor;

Comunicație wireless pentru monitorizarea în timp real a funcționării sistemelor;

Sisteme de alimentare cu baterii sau acumulatori de eficiență ridicată, prevăzute în unele cazuri cu surse stabilizatoare ridicătoare de tense de tensiune (power booster);

Motoare de curent continuu cu cutii de viteză încorporate și eventual encodere;

Senzori de urmărire de linie și respectiv de distanță (pentru sesizarea prezenței pereților) de tip analogic, care să permită citirea rapidă a valorilor acestora.

Scopul acestui proiect este realizarea unui sistem mobil autonom de tip line follower, destinat competițiilor, care trebuie să parcurgă un traseu de viteză și un traseu cu obstacole într-un timp cât mai scurt și fără penalizări (ieșiri de pe traseu sau nedepășirea unor obstacole). Traseul cu obstacole include diferite forme geometrice ale liniei ce trebuie urmărite, întreruperi ale acesteia pe diferite direcții și lungimi și urmărirea unor pereți laterali.

Obiectivele proiectului vor avea în vedere:

Proiectarea și realizarea structurii mecanice a sistemului mobil autonom.

Proiectarea și realizarea părții electronice pentru comanda și controlul sistemului.

Proiectarea și realizarea software-ului de comandă și control.

Sistemul va fi proiectat și realizat în vederea selecției pentru participarea la Olimpiada Națională de Mecatronica 2013, secțiunea Roboți mobili.

3. Considerații teoretice privind sistemele mobile autonome comandate de microcontroller

Sistemele mobile autonome sunt alcătuite din trei subsisteme:

Mecanic;

Electronic;

Software.

3.1 Subsistemul mecanic

Are rolul de a asigura atât integritatea structurală cât și mobilitatea sistemului. Aceasta este proiectată în funcție de mediul de lucru si obiectivele sistemului.

Deoarece sistemele mobile tip line follower sunt nevoite să parcurgă un traseu perfect plan într-un mediu pasiv, soluția ideală pentru deplasare este folosirea roților sau, mai rar, a șenilelor. Din punct de vedere al șasiului și al felului în care se realizează virajele întâlnim două soluții: sisteme cu direcție diferențială sau sisteme cu element mobil de ghidare.

3.1.1 Sisteme mobile cu direcție diferențială

Acesta implică un șasiu format dintr-o singură placă pe care sunt montate două roți și un sprijin, cel mai adesea un ball caster sau un LED. La roboții studiați întâlnim două astfel de configurații.

În cazul sistemului din Fig. 9, se observă amplasarea roților în spate, iar în față este situat punctul de sprijin și senzorii. O astfel de structură prezintă două dezavantaje: centrul de greutate trebuie să fie cât mai pe spate pentru a nu îngreuna virajele, iar raza dată de întoarceri la 180o este mai mare decât la soluția a doua.

În Fig. 10, roțile sunt montate la jumătatea șasiului și ball caster-ul în spate, acest lucru îl face predispus la debalansări, micșorând distanța senzori – teren implicit producând erori la citirea liniei. Pololu 3pi a redus acest dezavantaj folosind încă un punct de sprijin în față, însă nu l-a eliminat complet. Avantajul acestei configurații este posibilitatea întoarcerii în spații înguste.

În ambele situații, direcția este asigurată acționând diferit cele două roți(Fig. 11).

Indiferent de configurație, acest tip de șasiul este des utilizat în cazul sistemelor tip line follower fiind o soluție flexibilă și eficientă.

3.1.2 Sisteme cu element mobil de ghidare

O astfel de soluție constructivă poate fi observată în Fig.7. Șasiul, similar motocicletelor, este împărțit în două componente: cea motoare și cea viratoare. În spate sunt montate două roți motoare iar în față este atașată partea de senzori și roata de ghidaj. Un servomotor pivotează ansamblul premergător iar roata de ghidaj dă o nouă traiectorie robotului. Această soluție implică o rază foarte mare de întoarcere iar viteza de reacție este direct dependentă de rapiditatea servomotorului.

3.1.3 Motoare folosite pentru acționare

Deoarece electronica unui sistem mobil oricum necesită o sursă de curent continuu, de cele mai multe ori motoarele alese sunt motoare de curent continuu cu perii (brushed), fără perii (brushless) sau pas cu pas (stepper).

Motoarele cu perii sunt ieftine, ușor de folosit și se găsesc într-o gamă largă de dimensiuni. Acestea sunt alcătuite dintr-un stator, un rotor, perii și un colector (Fig. 13).

Statorul, fixat de carcasă, conține magneți permanenți ce generează un câmp magnetic staționar. Rotorul, aflat în centrul motorului, este realizat din una sau mai multe înfășurări. Când acestea sunt alimentate se generează un câmp magnetic ce interacționează cu cel staționar. Astfel polii magnetici ai rotorului sunt atrași de către polii opuși ai statorului și încep să se rotească. În momentul în care polii sunt aliniați, colectorul schimbă sensul curentului din înfășurarea rotorului, polaritatea rotorului se inversează și acesta va continua să se rotească până la următoarea aliniere a polilor. Alimentarea colectorilor se face prin intermediul periilor ce au rolul de a asigura un bun contact electric în timpul rotirii acestuia.

Reglarea turației se realizează variind tensiunea de alimentare iar sensul schimbând polarizarea rotorului.

Motoarele fără perii sunt asemănătoare, ca principiu de funcționare, cu cele anterioare dar, în loc să varieze polaritatea câmpului magnetic format de rotor, variază cel format în stator. Prin urmare rotorul va fi realizat din magneți permanenți iar statorul va avea una, două sau trei înfășurări/faze.

Comutarea fazelor tot este necesară dar nu mai este realizată mecanic ca în cazul motoarelor cu perii ci electronic. Funcționarea motorului (Fig. 14) implică alimentarea succesiva a celor 3 seturi de bobine în felul următor: A(100), AB(110), B(010), BC(011), C(001), CA(101). Deoarece motorul dezvoltă moment maxim în momentul în care rotorul este la 90o de bobina activă, se preferă folosirea senzorilor pentru a deduce poziția acestuia. Acționând exact bobina optimă se pot obține randamente foarte bune.

Motoarele fără perii oferă rapoarte cuplu/greutate și cuplu/watt mai bune decât cele cu perii; zgomot redus, durată de viață ridicată și necesar scăzut de mentenanță. Pentru că înfășurările sunt în stator, aflat pe interiorul carcasei, acestea pot fi răcite prin conducție termică fără să mai fie nevoie de un flux de aer. Prin urmare componentele motorului pot fi incluse într-o carcasă completă ce le va proteja de factorii de mediu. Marele dezavantaj al unui astfel de motor este complexitatea operării acestuia ce duce și la costuri ridicate.

Motoarele pas cu pas sunt motoare folosite preponderent în aplicații de control sau măsurători precum: imprimante ink-jet sau mașini CNC. Diferența dintre aceste motoare și cele fără perii este forma rotorului și numărul de poli ai acestuia. (Fig. 15)

Există mai multe tipuri de control în funcție de felul în care sunt alimentate înfășurările :

Comanda simplă – înfășurările sunt alimentate pe rând: A, B, C, D;

Comanda dublă – înfășurările sunt alimentate câte două: AB, BC, CD, DA;

Comanda mixtă – A, AB, B, BC, C, CD, D, DA;

Micropășire – înfășurările nu mai sunt alimentate digital ci analog permițând poziționarea rotorului oriunde între înfășurări.

Motoarele pas cu pas au avantajul de fi precise fără a fi nevoie de senzori și bucle de control și de a putea fi folosite în regim de frânare. Atâta timp cât sarcina nu depășește un maxim admis, indiferent de necesarul de cuplu, motorul se va rotii cu viteză constantă iar numărul pașilor executați este cunoscut.

Dezavantajele motoarelor pas cu pas:

Consum ridicat de curent indiferent de sarcină;

La turații ridicate cuplu dezvoltat scade semnificativ;

Pot apărea vibrații în timpul funcționării;

Greutate ridicată.

3.1.4 Reductoare/ angrenaje

Alegerea motoarelor trebuie astfel făcută încât necesarul de turație și accelerație să fie atins. Motoarele electrice cu sau fără perii obișnuiesc să atingă turații foarte mari(10000) însă cuplu dezvoltat să fie insuficient pentru a putea fi folosite în aplicații practice. Caracteristica principală a motoarelor este puterea acestora. Din punct de vedere mecanic, puterea motoarelor P exprimată în W este dată de ecuația (1).

unde este cuplul dezvoltat [Nm];

este viteza unghiulară [rad/s].

Cum puterea unui motor este constantă, scăderea turației duce la creșterea direct proporțională a cuplului dezvoltat.

Pentru a reduce turația se pot folosi transmisii prin curea sau prin lanț, pârghii, dar mai ales angrenaje cu roți dințate(Fig. 16), uneori integrate în motorului.

Angrenajele cu roți dințate, față de transmisiile prin curele, nu permit alunecări dar, în funcție de precizia de fabricație pot crea jocuri iar în anumite cazuri se pot deteriora la suprasarcini sau turații ridicate.

Raportul dintre turația de intrare și turația de ieșire este dependent de diametrele roților care, la rândul lor dau numărul de dinți pe fiecare roată, ecuația (2).

De asemenea trebuie luat în calcul și dimensiunea roților cuplate la motoare. Întregul ansamblu determină viteza, ecuația (3), și accelerația maximă, ecuația (4), a sistemului.

unde Vmax este viteza liniară maximă [m/s];

este turația motorului [rpm];

este diametrul roții [m].

Deplasarea sistemului are loc datorită forței ce apare în punctul de contact al roților cu solul (Fig. 17). Aceasta este dependentă de cuplul motorului și diametrul roții, ecuația (5):

unde F este forța la roată [N];

a este accelerația sistemului [m/s2];

m este masa sistemului[kg].

În practică însă, motoarele nu ajung la turația maximă când sunt puse în sarcină. Deseori trebuie găsit un echilibru dintre accelerația dorită și viteză. Dacă în calculul angrenajului a rezultat o accelerație foarte mică este posibil, în realitate, acel sistem să nu poată să învingă frecările statice și în concluzie să nu se poată deplasa.

3.2 Subsistemul electronic

Din punct de vedere electronic, dispozitivele folosite în cadrul unui sistem mobil se caracterizează prin:

Tensiunea de alimentare (3,3; 5; 9; 12 V);

Necesarul de curent;

Metoda de control(input) – semnale digitale, analogice, PWM sau serial;

Metoda de răspuns(outputul) – semnale digitale, analogice, PWM sau serial.

Diferența dintre semnalele digitale și cele analogice este aceea că cele digitale pot lua doar valori discrete dintr-un interval de tensiuni (ex. 0 sau 5V), pe când cele analogice pot lua o infinitate de valori din acel interval. Pentru o interpretare corectă a semnalelor analogice de putere scăzută, deseori se folosesc amplificatoare și filtre pentru a limita zgomotul apărut pe traseul acestora.

În construcția unui sistem mobil tip line follower întâlnim următoarele componente:

Microcontroller-ul;

Driver-ul de motoare;

Senzorii;

Interfața utilizator;

Sursa de curent continuu.

3.2.1 Microcontroller-ul

Gestionarea tuturor operațiunilor de comandă și control din cadrul unui sistem mobil este realizată de către microcontroller. Acesta este un circuit integrat capabil să primească și să genereze semnale electrice în funcție de programul încărcat în memorie.

Caracteristici principale:

Numărul de intrări/ieșiri digitale;

Numărul de intrări analogice, rezoluția convertorului analog-digital;

Numărul și rezoluția ieșirilor PWM;

Numărul și tipul interfețelor de comunicație;

Tensiunea de lucru;

Frecvența procesorului;

Numărul de întreruperi;

Numărul și rezoluția temporizatoarelor și numărătoarelor (timer, counter);

Capacitatea memoriei Flash – folosită pentru stocarea programelor de funcționare;

Capacitatea memoriei RAM;

Capacitatea memoriei EEPROM – folosită pentru stocarea permanentă a unor variabile.

Deoarece unele microcontroller-e au dimensiuni foarte mici (pini cu lățime de 0.2mm) sau au nevoie de elemente de circuit suplimentare pentru a fi folosite, în cadrul proiectelor la nivel educațional se folosesc platforme de dezvoltare cu microcontroller (Fig. 18). Acestea oferă o soluție completă de comandă și control, ușor de implementat în orice proiect.

3.2.2 Driver-ul de motoare

Microcontroller-ele nu pot alimenta în mod direct consumatori mari de energie deoarece curentul maxim ce poate fi furnizat pe liniile de ieșire este limitat (ex. 50mA). Nerespectarea acestor limite poate duce la distrugerea ieșirii sau chiar a întregului integrat. Soluția cel mai des întâlnită este utilizarea tranzistoarelor (Fig. 19). Acestea sunt de două tipuri: amplificatoare sau comutatoare. În primul caz, tranzistorul se comportă ca o rezistență variabilă și se opune gradat trecerii curentului în funcție de semnalul de intrare. Acest tip este, în special, folosit pentru semnale analogice. În cazul al doilea, dacă semnalul de intrare trece de un anumit prag atunci tranzistorul închide circuitul. Aceștia sunt folosiți pentru semnale digitale (inclusiv PWM). În același scop se mai pot folosii optocuploare care izolează complet cele două circuite. Acestea sunt alcătuite dintr-un LED și fototranzistor introduse în aceeași capsulă.

La motoarele de curent continuu fără perii, controlul asupra modului de funcționare este realizat folosind puntea H. În fig. 20 se poate observa o astfel de structură în care, pentru simplificare, s-au folosit întrerupătoare în loc de tranzistori. În tabelul următor sunt descrise modurile de funcționare.

Tabelul 1 – Tabelul de funcționare a punții H

Controlul vitezei la motoarele de curent continuu cu perii se poate face prin:

Schimbarea tensiunii de alimentare, S1 este considerat tranzistor amplificator;

Alimentarea în impulsuri, S1 este considerat tranzistor comutator și permite trecerea curentului în funcție de semnalul PWM cu care este comandat.

Pe piață au apărut o serie de circuite integrate ce acoperă toată electronica necesară comandării unui motor de către un microcontroller.

3.2.3 Senzorii

Senzorii sunt dispozitive ce generează semnale electrice în funcție de anumite semnale sau stimuli. În funcție de factorul sensibil, senzorii pot fi:

Mecanici (ex. senzori de capăt de cursă, de greutate);

Chimici (ex. senzori de fum, de alcool);

Electrici (ex. senzori de curent, tensiune, rezistență electrică);

Magnetici (ex. senzori inductivi);

Termici (ex. senzori de temperatură);

Radiativi (ex. senzori de lumină, ultrasonici)..

În cazul sistemelor mobile tip line follower, detectarea liniei se face folosind senzori de linie sau, mai rar, camere video. Metoda a doua implică procesare de imagini cea ce necesită hardware și software specializat.

Perceperea culorilor se bazează pe reflexia doar a anumitor lungimi de undă din fasciculul incident. Astfel, ochiul percepe alb deoarece suprafața respectivă reflectă în mod egal întreg spectrul de culori. Negru este perceput în momentul în care lumina nu ajunge la ochi. Altfel spus, suprafața respectivă absoarbe întreg spectru de culori. Senzorii de linie folosesc, în general, ca element sensibil, fototranzistorul. Acesta își modifică rezistența electrică în funcție de cantitatea de lumină ce cade asupra lui.

Senzorii de linie utilizați în cadrul sistemelor mobile tip line follower sunt sensibili fie la lumină infraroșie (Fig. 21), fie la lumină vizibilă. Cei în infraroșu au avantajul de a fi mai greu influențați de perturbații (blițuri, lumina ambientală etc.). Pentru o citire corectă, senzorii în infraroșu sunt folosiți împreună cu un LED infraroșu.

Semnalul (output) dat de senzori poate fi analogic sau digital. Senzorii analogici se bazează citirea rezistenței fototranzistorului de către convertorul analog-digital. Microcontroller-ele au un număr mult mai mic de intrări analogice față de cele digitale, cea ce reprezintă un dezavantaj în folosirea acestui tip de senzor.

Senzorii digitali folosesc un condensator (Fig. 22) ce se descarcă prin fototranzistor. Prin urmare, cu cât fototranzistorul se opune trecerii curentului, cu atât condensatorul rămâne încărcat mai mult timp. Citirea gradului de reflexie se realizează prin măsurarea timpului de descărcare a acestuia.

Față de senzorii analogici, linie de semnal de la cei digitali mai este folosită și ca intrare. Citirea senzorului implică următorii pași:

Se setează 1 logic pe linia out;

Se așteaptă cel puțin 10 µs pentru încărcarea condensatorului;

Se setează linia out ca input digital;

Se măsoară timpul de trecere a intrării din 1 logic în 0 logic.

Un mare dezavantaj, în folosirea senzori de linie digitali, este timpul mare de citire, aproximativ 1 ms în condiții normale, dar poate dura și zeci de ms. În cazul senzorilor analogici, întârzierea este dată doar de timpul necesar convertorului analogic-digital să efectueze o citire. Platformei Arduino îi este necesar un interval de 0.1 ms pentru a efectua citirea unei singure linii analogice.

Pentru detectarea pereților sau a altor obiecte se folosesc fie senzori de distanță în infraroșu, fie senzori cu ultrasunete.

Senzorii de distanță în infraroșu (Fig. 23c) funcționează asemănător celor de linie doar că nu se bazează pe reflexia luminii în funcție de culoare ci pe dispersia acesteia în funcție de distanță. Cu cât senzorul este mai departe de obiect, cu atât se întoarce mai puțină lumină. Deși simplu de implementat, acest tip de senzor este ușor influențat de gradul de reflexie a luminii de pe suprafața obiectului și de unghiul de incidență.

Senzorii cu ultrasunete se bazează pe emiterea și reflexia undelor ultrasonice (~40kHz). Aceștia funcționează prin emiterea unui tren de impulsuri, detecția reflexiei lor și măsurarea timpului de întoarcere. În anumite cazuri, senzorii ultrasonici pot integra doar generatorul de impulsuri și detectorul acestora (Fig. 23a). Măsurarea timpului de întoarcere și deducerea distanței fiind realizate în cadrul microcontroller-ului. Există și senzori, mai costisitori, ce oferă rezultatul măsurătorilor ca semnal analogic sau chiar serial, folosind portul UART (Fig. 23b).

În cazul măsurării distanțelor, senzorii cu ultrasunete sunt de preferat în fața celor în infraroșu datorită performanțelor sporite.

3.2.4 Interfața utilizator

Pentru operarea sistemelor mobile este nevoie de o interfață utilizator. Acesta poate fi sub formă hardware (butoane, LED-uri, ecrane LCD, telecomenzi) sau software (sub formă de aplicație cu transmisie wireless de date).

Cel mai ușor de implementat sunt butoanele și LED-urile. Butoanele sunt simple contacte ce închid un circuit care, la rândul său, generează un semnal digital spre microcontroller. LED-urile sunt comandate folosind semnale digitale sau, dacă se dorește o intensitate în trepte, semnale PWM. Deoarece LED-urile au o rezistență electrică foarte mică, acestea tind să consume foarte mult curent, cea ce le reduce durata de viață considerabil. Pentru a soluționa această problemă, LED-urile întotdeauna se folosesc în serie cu o rezistență.

Ecranele LCD (Fig. 24) se folosesc atunci când se dorește furnizarea mai multor informații. Datele afișate de acestea provin de la microcontroller prin comunicație paralelă sau serială.

Pentru comandarea de la distanță, sistemul mobil are nevoie de modulele aferente de comunicație. Telecomandarea în infraroșu reprezintă o soluție simplă și economică. În schimb, aceasta nu se poate realiza la distanțe mari și necesită un traseu neobstrucționat între emitor și receptor. Pentru comunicații bidirecționale și mai puțin restrictive se folosesc module radio. Acestea pot lucra folosind frecvența publică de 433Mhz sau 2.4Ghz împreună cu protocoale bine stabilite precum: bluetooth, wi-fi, ZigBee (Fig. 25).

Modulele radio permit transmisii bidirecționale și astfel, se poate urmări și interveni asupra funcționării sistemului fără a fi nevoie de o conexiune fizică. Pentru dispozitivul de interfațare (PC, smartphone, tablete) se pot dezvolta aplicații software ce facilitează transmisia și recepția de date.

3.2.5 Sursa de curent continuu

În cadrul sistemelor mobile tip line follower, sursa de energie o reprezintă bateriile. Acestea transformă energia chimică stocată, în energia electrică necesară electronicii și motoarelor sistemului. Bateriile sunt de două tipuri: primare (nereîncărcabile) sau secundare (reîncărcabile). Cele primare sunt, în general, folosite la dispozitive cu necesar scăzut de energie. Reacțiile chimice ce au loc în aceste baterii nu pot fi ușor inversate. Bateriile secundare sunt, în schimb, reîncărcabile. Acest lucru se realizează prin aplicarea unui curent ce inversează reacțiile chimice ce au avut loc în timpul descărcării. În funcție de tipul celulelor folosite avem baterii: nichel-cadmiu (NiCd) , nichel-zinc (NiZn), nichel-metal hibrid (NiMH) și litiu-ion (Li-ion).

Bateriile litiu-ion sunt cel mai des întâlnite în electronicele comerciale, oferind un raport al energiei stocate pe greutate excelent și fără să prezinte efectul de memorie. Aceste efect descrie situația particulară în care anumite acumulatoare, în special cele NiCd, își pierd treptat capacitatea maximă de a stoca energie electrică dacă sunt reîncărcate în mod repetat înainte de a fi complet descărcate. Bateriile litiu-ion sunt însă sensibile la supraîncărcări, supraconsum și temperaturi ridicate iar în acest sens majoritatea au un circuit integrat de protecție.

Bateriile nu reprezintă o sursă stabilă de tensiune, aceasta variind în funcție de gradul de descărcare și temperatură (Fig. 26), iar unele cazuri electronica sistemului necesită mai multe nivele de tensiune. Stabilizatoarele de curent continuu sunt dispozitive create special pentru a schimba eficient nivelul de tensiune și a-l menține constant indiferent de variația tensiunii de intrare, curentului consumat sau a temperaturii (Fig. 27).

Clasificarea stabilizatoarelor de tensiune (Fig. 28):

Stabilizatoare liniare – zgomot mic, randament slab, doar coborâtoare de tensiune;

Stabilizatoare în comutație – zgomot mai mare, randament bun.

Stabilizatoarele în comutație pot fi coborâtoare de tensiune tip buck sau ridicătoare de tensiune tip boost. Pentru alimentarea circuitelor sensibile, precum microcontroller-ul, este recomandată folosirea stabilizatoarelor liniare ce prezintă zgomot scăzut.

3.3 Subsistemul software

Gestionarea componentelor hardware (motoare, sisteme de afișare, interfețe de comunicare, senzori) este realizată de către microcontroller pe baza programului încărcat. Acest program reprezintă subsistemul software.

În cadrul unui sistem mobil tip line follower, elementele vitale, din punct de vedere software, sunt:

Citirea și interpretarea corectă a liniei în vederea stabilirii poziției robotului;

Acționarea motoarelor în funcție de abaterea de la traseu.

3.3.1 Interpretarea poziției robotului în funcție de senzori

Senzorii de linie, indiferent de tipul lor, în cadrul programului, generează o valoare numerică. Teoretic, în cazul senzorilor analogici în infraroșu de la Pololu conectați la un convertor analogic-digital cu rezoluție de 10 biți, valorile citite vor fi cuprinse între 0 (pentru alb) și 1023 (pentru negru). Determinarea poziției liniei implică folosirea a cel puțin doi astfel de senzori.

Senzorii mai îndepărtați de axul robotului se consideră mai importanți față de cei din centru deoarece ei indică o abatere mai mare de la linie. În acest sens, formula matematică cea mai des utilizată pentru deducerea poziției este media ponderată.

Se consideră S0-S4 valorile senzorilor, folosind ecuația (6), poziția robotului va avea o valoare numerică cuprinsă între 0 și 4000, unde 2000 este valoarea ideală (linia este pe centru).

Această metodă nu ține cont de valoarea absolută a senzorilor ci doar de diferențele între senzori. De asemenea această metodă poate fi utilizată indiferent de numărul senzorilor, par sau impar.

Abaterea de la traseu se calculează prin diferența poziției actuale față de cea ideală. Ecuația abaterii generalizată pentru n+1 senzori este (7):

3.3.2 Controlul direcției

Controlul direcției se face în funcție de abaterea de la traseu și se poate realiza în două moduri:

Discontinuu – abaterea este comparată cu anumite praguri și se determină un regim de funcționare fix. Un exemplu foarte simplu este următorul:

Dacă abaterea este între -10 și 10 se merge înainte;

Dacă abaterea este mai mică de -10 se merge în stânga;

Dacă abaterea este mai mare de 10 se merge în dreapta.

Continuu – direcția este ajustată în mod continuu în funcție de valoarea abaterii și de evoluția acesteia.

Reglajul proporțional (P) – ieșirea este proporțională cu abaterea (Fig. 29). Din punct vedere matematic, acest reglaj este descris de relația (8). Deși oferă performante superioare reglajelor discontinue, acest tip de control nu ține cont de evoluția anterioară a abaterii.

unde P – reacția (ieșirea) proporțională;

Kp – parametrul de proporționalitate;

e(t) – reprezintă eroarea (abaterea);

t – timpul instantaneu.

Pentru implementarea software a reglajului proporțional, o posibilă soluție este următoare:

Reglajul integrativ (I) – reprezintă suma tuturor abaterilor de la pornirea sistemului care au fost corectate, relația (9) (Fig. 30). Acest control poate accelera reacția și elimina erorile constante. Deoarece depinde de toate erorile anterioare, poate crea instabilitate.

unde I – reacția integrativă;

Ki – constanta integrativă;

– reprezintă timpul de la 0 până în prezent.

Implementarea software a acestui tip de control se poate face în felul următor:

Reglajul derivativ (D) – ia în calcul rata de schimbare a abaterii. În acest fel se poate prezice comportamentul sistemului, cea ce contribuie la stabilizarea mai rapidă (Fig. 31). Reglajul derivativ se calculează folosind relația (10).

unde D – reacția derivativă;

Kd este parametrul derivativ.

Din punct de vedere software, un posibil algoritm este următorul:

Reglajul derivativ este foarte sensibil la zgomot. Citirea eronată a valorilor de intrare poate conduce la calculul greșit al ratei de schimbare și implicit la un control defectuos.

Controlul PID este astfel numit deoarece însumează cele trei tipuri de reglaje menționate anterior: proporțional, integrativ și derivativ. Implementarea unui astfel de sistem necesită aflarea celor 3 parametrii de control: Kp, Ki și Kd. Deseori, calcularea acestora este prea laborioasă și astfel, cel mai des, aceștia sunt determinați prin încercări succesive.

4. Proiectarea sistemului mobil autonom

4.1 Condiții de proiectare

Scopul lucrării este reprezentat de realizarea unui sistem mobil tip line follower în vederea selecției pentru participarea la Olimpiada Națională de Mecatronică 2013, secțiunea Roboți mobili.

Regulamentul concursului impune anumite condiții pentru intrarea în competiție:

Este interzisă participarea roboților obținuți prin modificarea produselor comerciale tip line follower;

Este interzisă folosirea plăcilor cu drivere din comerț;

Este permisă folosirea oricărui senzor, placă de dezvoltare cu microcontroller, cameră sau alt element de circuit;

Dimensiunea fizică a robotului nu trebuie să depășească 200x200x200mm;

Linia este de culoarea neagră pe fundal alb cu grosimea de 15mm.

Obiectivele proiectului sunt:

Stabilirea unei baze hardware pentru dezvoltări ulterioare;

Stabilirea unei baze software pentru dezvoltări ulterioare;

Folosirea senzorilor antemergători pentru anticiparea traseului.

În cadrul probei de viteză, robotul va urmări un traseu format din linii drepte, arce de rază minimă de 100 mm, fără intersecții și fără linii discontinue. Proba a doua presupune parcurgerea unui traseu ce include 5 tipuri de obstacole:

4.2 Soluția hardware

În proiectarea robotului s-a urmărit folosirea unui număr suficient de senzori amplasați în puncte critice astfel încât, indiferent de obstacol să se poată lua decizi bazate pe informații concrete și evitarea intrării în moduri de căutare.

4.2.1 Soluția folosită pentru deplasare

Datorită simplității în utilizare și prețului redus s-a optat pentru folosirea unor motoare electrice cu perii, mai exact motoare din seria „Pololu Micro Metal HP” (Fig. 32) ce includ și reductorul mecanic format din roți metalice dințate, disponibile în mai multe rapoarte: 10:1, 30:1, 50:1. Pentru partea de calcule s-a estimat masa robotului la 250g și s-au considerat două opțiuni de roți, cu diametru de 30 mm și de 60 mm.

Tabelul 2 – Potențiale soluții motoare-roți

Luând în calcul viteza maximă dar în special accelerația, au fost testate practic următoarele soluții: reductor 30:1 cu roți de 30 mm și reductor de 50:1 cu roți de 60 mm (Fig. 33). În urma testelor pe diferite trasee s-a dovedit a fi mai potrivită soluția a doua.

4.2.2 Șasiul

Această formă a șasiului este foarte des utilizată în cadrul roboților pentru competiții, oferind suficientă stabilitate și flexibilitate (Fig.34). Dimensiunile acestuia au fost alese astfel încât să acomodeze toate componentele necesare.

4.2.3 Sursa de curent

Bateriile folosite sunt 2 acumulatori Li-ion 18650, marca Ultrafire (Fig. 36), cu capacitate de 3000mAh, conectate în serie. Încărcate complet acestea dezvoltă, împreună, 8,4V, suficient pentru a alimenta motoarele și microcontroller-ul fără a fi nevoie de stabilizatoare ridicătoare de tensiune. Datorită dimensiunilor diferite față de bateriile mai des întâlnite momentan pe piață, a fost necesară proiectarea unui suport special (Fig.35).

4.2.4 Senzorii de linie

Pentru detectarea liniei s-au folosit senzori analogici în infraroșu de la Pololu. Aceștia folosesc un LED ce emite lumină infraroșie spre sol și un fototranzistor care, în funcție de gradul de reflexie, permite să treacă mai mult sau mai puțin curent. Ieșirea senzorilor este conectată la intrarea analogică din microcontroller unde este analizată de către un convertor analog-digital cu rezoluție de 10biți. În concluzie, gradul de reflexie este măsurat în valori cuprinse în intervalul 0-1023. Pololu oferă acest tip senzori în module unitare (Fig. 21) fie 8 în linie pe același suport (Fig. 37). Cea din urmă fiind o soluție excelentă pentru sistemele line follower ce au capacitatea de a gestiona 8 intrări analogice.

Pentru depășirea cu succes a primului tip de obstacol, linie întreruptă, 3 senzori, în afara celor 8, au fost poziționați astfel încât să se poată cunoaște direcția de continuare a liniei (Fig. 38).

Potrivit datelor de producător, senzorii folosiți funcționează optim poziționați la 3 mm față de traseu, maximul fiind de 6 mm. S-a ales așezarea senzorilor la 4 mm pentru a lua în calcul și imperfecțiunile traseului. Pentru a economisii material în cazul reproiectării, suportul senzorilor a fost realizat din două piese. Prima are rolul de a aduce senzorii la distanța optimă, paraleli cu solul, iar a doua de a-i poziționa pe orizontal (Fig. 39).

4.2.5 Senzorii de perete

Detectarea peretelui s-a realizat folosind 3 senzori cu ultrasunete HC-SR04 (Fig. 23a) poziționați pe laterale (Fig. 40) și în fața robotului. Aceștia înglobează un transmițător și un receptor ultrasonic iar funcționarea lor constă în emiterea unui semnal cu frecvența de 40 kHz și măsurarea timpului necesar întoarcerii ecoului. Poate detecta obiecte între 2cm și 4m. Pinii de conectare ai senzorului sunt: Vcc (5V), GND, Trig și Echo. Declanșarea emitorului are loc în momentul activării pentru cel puțin 10 µs a intrării Trig (trigger = declanșator). În funcție de timpul de întoarcere a ecoului, electronica integrată generează apoi un “1” logic pe ieșirea Echo un timp proporțional cu distanța măsurată (Fig. 41). Pentru aflarea distanței în centimetrii producătorul recomandă împărțirea timpului de activarea a ieșirii Echo la 58.

4.2.6 Placa de dezvoltare Arduino Mega 2560

Comanda și controlul sistemului este asigurată de către platforma Arduino Mega 2560, ce se bazează pe microcontroller-ul ATmega 2560. Printre caracteristicile acestuia amintim:

54 linii intrare/ieșire;

16 intrări analogice;

256Kb memorie flash;

8 Kb memorie SRAM și 4 Kb memorie EEPROM;

Frecvență de ceas: 16Mhz;

Limitele tensiunii de alimentare: 6-20V;

Tensiunea de lucru: 5V;

Stabilizator suplimentar pentru 3.3V până la 50mA;

4 porturi seriale UART.

Programarea platformei se face folosind un cablu USB conectat între calculator și programatorul integrat.

Pentru interfațarea cu utilizatorul a fost proiectat un shield ce vine montat peste Arduino (Fig. 42). Acesta conține 3 butoane cu revenire, 2 LED-uri și pinii de conectare pentru un modulul wireless Xbee. Comunicația fără fir este necesară pentru moni-torizarea programului în timpul funcționării și facilitarea dezvoltării acestuia.

Conectarea la calculator a modului Xbee este realizată folosind adaptorul Xbee Explorer USB.

4.2.7 Modelarea 3D a întregului sistem

Proiectarea 3D a sistemului a fost realizată folosind aplicația Catia V5R19. Utilizând modulul Part Design, au fost create componentele cunoscute: motoare, suporți motoare, roți, senzori, baterii. Deseori au fost necesare măsurători fizice asupra componentelor deoarece specificațiile date de producători nu erau suficient de precise sau chiar inexistente. Următorul pas a fost crearea unui model de bază al șasiului. Asamblarea computerizat a componentelor, utilizând modulul Assembly, a ajutat la determinarea componentelor auxiliare: prinderile senzorilor, suportul motoarelor și al bateriilor (Fig. 43, Fig. 44). Ultima etapă a constat în verificarea dimensiunilor și stabilirea găurilor de montare.

Proiectarea și asamblarea în Catia a ajutat la evitarea greșelilor de dimensionare și formă și au condus spre un sistem complet fără elemente aproximate sau necunoscute.

4.3. Proiectarea subsistemului electronic

Subsistemul electronic este împărțit pe două zone:

Șasiu – elementele vitale funcționării robotului;

Shield – elementele de interfațare.

Proiectarea circuitelor a fost realizată folosind aplicația Eagle 6.3.0 – realizarea schemei logice (adăugarea și interconectarea componentelor) și realizarea cablajului (circuitul propriu-zis).

4.3.1. Electronica șasiului

Aceasta îndeplinește două roluri:

Asigurarea funcționării driverului de motoare;

Asigurarea conexiunilor electrice folosind diverși conectori.

Pentru controlul motoarelor s-a folosit circuitul integrat L298 în format PowerSO20. Acesta este un driver dual full-bridge, cea ce înseamnă că poate controla independent două motoare în ambele sensuri, în scurtcircuit(frânare) sau lăsate liber. Curentul maxim suportat este de 2A pe canal. Comandarea unui motor se face folosind două intrări digitale (i1,i2/i3,i4), pentru modul de operare și o a treia (enable A/B), pentru a stabilii viteza sau puterea de frânare, conectată la port PWM. Analizând diagrama bloc (Fig. 45) se poate deduce modul de comandă al driverului (tabelul 3).

Tabelul 3 – Controlul motoarelor folosind L298

Driverul L298, potrivit producătorilor, pentru a lucra stabil, are nevoie de 2 condensatori pentru nivelarea tensiunilor Vs(tensiune nivel logic) și Vss(tensiune alimentare motoare). Aceștia se leagă intre pinii corespunzători și GND. De asemenea, pentru a proteja integratul și restul circuitului de fenomenul de back EMF, se folosesc două blocuri de diode redresoare (Fig. 46). Back emf se referă la tensiunea apărută în motoare prin autoinducție. În momentul în care un motor se rotește acesta se comportă ca un generator și creează o tensiune în rotor. Această tensiune se opune celei de alimentare și scade tensiunea totală, iar prin urmare scade și curentul consumat. Deoarece motorul se poate comporta și ca generator, în momentul în care driverul nu mai alimentează motoarele, tensiunea acestuia poate prezenta un pericol pentru integrat și, de aceea, trebuie redresată spre baterii.

În proiectarea circuitului de alimentare cu tensiune am luat în considerare și posibilitatea folosirii unui stabilizator ridicător de tensiune ce s-ar fi conectat la portul BOOSTER. În absența acestuia, pe pinii portului DIRECT trebuie pus un conector tip jumper. Portul OUT reprezintă alimentarea pentru Arduino. Comanda driver-ului de motoare se face prin portul CMD (Fig. 47).

Tot pe șasiu vin montați și senzorii ultrasonici pentru detecția peretelui. Aceștia au 4 pini: Vcc, Trig, Echo și GND. Pentru a ușura conectarea la microcontroller, portul pe care se fixează le asigura alimentarea iar comanda se face prin porturile: SONAR_DR și SONAR_ST (Fig. 48).

Cablajul final a fost realizat ținând cont de proiectarea 3D a sistemului (dimensiune, găuri, componente) (Fig. 49).

Traseele marcate cu roșu reprezintă fire conectate peste cablaj iar dreptunghiul hașurat este suprafața pe care este montat suportul pentru baterii. Vederea folosită în imagine este prin placă deoarece aceasta urmează să fie printată și transferată plăcii prin procedeul de transfer termic.

4.3.2. Shield-ul de interfațare cu utilizatorul

Nevoia unei interfețe fizice cu utilizatorul, diferite de comunicațiile fără fir este necesară deoarece regulamentul concursului interzice folosirea acestora. Prin urmare a fost necesară proiectarea unei soluții ca modulul Xbee folosit să poată fi înlăturat cu ușurință pe parcursul probelor.

Conectarea butoanelor la Arduino se poate face prin două metode:

Forțarea intrării să fie normal deschis (0 logic) (Fig. 50). Rezistența are rolul de a bloca scurtcircuitul în momentul apăsării butonului prin devierea curentului la input, unde apare starea de “1” logic. Această metodă necesită folosirea rezistenței de 10k și legării butonului la 5V.

Forțarea intrării să fie normal închis(1 logic) (Fig. 51). Această metodă funcționează pe baza activării rezistenței interne R2. Input-ul final primește 5V, prin rezistență, atâta timp cât nu este apăsat butonul.

Interfața fizică cu utilizatorul este realizată folosind 3 butoane, conectate prin intermediul rezistenței interne, și 2 LED-uri, ce vor indica starea sistemului.

Pentru interfațarea wireless s-au folosit 2 module Xbee Pro 60mW Wire. Acestea funcționează la frecvența de 2.4Ghz și folosesc standardul 802.15.4. Rază maximă de acțiune este de 1.5 km. Conectarea modulelor la Arduino se face prin intermediul portului de comunicație serială UART.

Datorită faptului că modulele Xbee folosesc logica CMOS (3.3V), a fost necesară utilizarea unui modul de conversie spre logica TTL (5V), folosită de Arduino. (Fig. 52)

Cablajul final a inclus interfața fizică (3 butoane și 2 LED-uri) împreună cu soclu pentru convertorul de nivel logic și soclul pentru modulul Xbee. De asemenea, pentru conectarea cu Arduino, au fost proiectați și pinii de legătură. (Fig. 53)

Pentru calculul rezistențelor, folosite la limitarea curentului spre LED-uri, s-a folosit relația (11):

unde R – valoarea rezistenței;

Ualim – tensiunea de alimentare (5V);

Uled – tensiunea necesară LED-ului;

Iled – curentul necesar LED-ului.

Cunoscând specificațiile LED-urilor: roșu – Uled = 1,7 V, Iled = 10mA; verde – Uled = 2,2 V; Iled = 10mA; s-au dedus rezistențele R3 = 280 Ω și, respectiv, R4 = 330 Ω.

4.4. Realizarea programului

Platforma Arduino se programează folosind un limbaj apropriat C++ peste care au fost implementate diverse funcții și metode de lucru pentru porturile de intrare/ieșire.

Programul unui sistem mobil tip line follower trebuie să îndeplinească următoarele funcții:

Interfațarea fizică utilizator;

Citirea senzorilor;

Calibrare senzori;

Filtrare senzori;

Stabilire abatere de la linie;

Gestionare obstacole;

Comandarea motoarelor;

Stabilire regim de funcționare;

Controlul direcției folosind algoritmul PID;

Comandarea wireless;

Raportarea wireless.

Bibliotecile reprezintă secvențe externe de program create special pentru a fi utilizate în cadrul altor proiecte. Rolul acestora este de a simplifica implementarea anumitor operații și de a sporii flexibilitatea programului. Pentru senzorii de perete și de linie s-au folosit următoarele biblioteci:

Arduino Library for the Pololu QTR Reflectance Sensors, creată de Pololu. Aceasta se poate folosi atât pentru senzorii analogici cât și pentru cei digitali și include procedura de calibrare și de generare a poziției liniei.

NewPing Library for Arduino, creată de Tim Eckel. Poate funcționa cu o multitudine de senzori cu ultrasunete: HC-SR04, SRF05, Parallax PING))) ș.a.

Pentru a atinge un grad ridicat de flexibilitate, Arduino permite programarea bazată pe obiecte. În programare, obiectele sunt entități ce pot avea proprietăți și pot primii instrucțiuni. Pentru a ușura implementarea lor, bibliotecile senzorilor lucrează pe bază de obiecte. Astfel, pentru utilizarea senzorilor, obiectele acestora trebuie: declarate (ex. linia de senzori, senzor perete dreapta), comandate (ex. calibrează) și citite (ex. poziția liniei).

Pentru declararea obiectelor tip senzori analogici se folosește următoarea sintaxă:

QTRSensorsAnalog nume((unsigned char[]) {pin1, pin2, …, pinN}, nr_senzori, nr_esantioane, pin_emitor)

unde nume – numele dat obiectului;

pin1, pin2, …, pinN – sunt pinii analogici de intrare ai senzorilor;

nr_senzori – numărul de senzori declarați;

nr_eșantioane – pentru citirea semnalului de la fiecare senzor se efectuează mai multe citiri și se face media aritmetică, acest parametru stabilește numărul de citiri;

pin_emitor – pentru a conserva energie se pot dezactiva LED-urile ce emit infraroșu, atunci când nu sunt necesare.

Declararea obiectelor tip senzor cu ultrasunete urmează sintaxa:

NewPing nume(trig, echo, dist)

unde nume – numel dat obiectului;

trig –pinului la care este conectat semnalul trig al senzorului;

echo – pinul la care este conectat semnalul echo al senzorului;

dist – distanța maximă în centimetrii care se dorește a fi măsurată.

Similar bibliotecilor, funcțiile sunt secvențe de program, scrise în cadrul proiectului, ce dau posibilitatea împărțirii codului în componente mici ce vor fi apelate în cadrul programului principal. Folosirea funcțiilor ajută la simplificarea programelor complexe.

4.4.1 Noțiuni introductive despre programarea Arduino

Orice program Arduino conține două funcții de bază:

void setup() – Secvențele de cod scrise în această zonă rulează o singură dată, la pornirea platformei. Tot aici se stabilesc intrările/ieșirile și tipul acestora.

void loop() – Codul scris în această zonă rulează linie cu linie în mod continuu.

În continuare se descriu funcțiile specifice Arduino folosite în cadrul programului:

pinMode(nr pin, tip) – este folosit pentru stabilirea tipului de intrare/ieșire. Nr pin reprezintă numărul înscris pe platforma în dreptul pinului, iar tipul poate fi INPUT sau OUTPUT. Se folosește doar în cazul pinilor I/O sau PWM;

digitalWrite(nr pin, stare) – este folosit pentru setarea pinilor digitali. Starea acestora poate fi 1 logic (1 sau HIGH) sau 0 logic (0 sau LOW);

digitalRead(nr pin) – citește valoarea digitală a intrării (0 sau 1);

analogWrite(nr pin, valoare) – este folosit pentru comanda PWM a ieșirilor compatibile. Valoarea este cuprinsă între 0 – 255;

analogRead(nr pin) – returnează valoarea analogică a intrării (0 – 1023);

Serial.begin(viteză) – este folosit pentru inițializarea comunicațiilor seriale folosind viteze cuprinse între 300 și 115200 biți/secundă. Cea mai des utilizată rată de transfer este de 9600 biți/secundă. Arduino Mega are 3 porturi seriale iar pentru apelarea lor se folosește Serial1,Serial2 sau Serial3;

Serial.print(“text”) – trimite pe portul serial textul sau valoarea varibilei aflată între paranteze;

Serial.println(“text”) – este similară celei anterioare doar că la sfârșit lasă rând liber;

Serial.available() – este o funcție ce returnează adevărat în momentul în care au sosit informații pe serială;

Serial.read() – returnează octetul primit pe serială;

Serial.parseInt() – returnează primul număr de tip întreg sosit pe serială. Exemplu: dacă se trimite informația “viteza23m”, funcția returnează numărul întreg 23;

Serial.parseFloat() – returnează primul număr de tip real sosit pe serială. Exemplu: dacă se trimite informația “dreapta5.92s”, funcția returnează numărul real 5,92;

delay(timp) – oprește rularea programului o perioadă de timp(milisecunde);

millis() – returnează timpul trecut, în milisecunde, de la pornirea platformei. Pentru salvarea sa se folosesc variabile de tipul unsigned long.

Tipurile de variabile folosite:

boolean – suportă doar două valori: 1 sau 0, adevărat sau fals; cu toate acestea ocupă 8biți de memorie;

byte – numere întregi, pozitive; ocupă 8 biți de memorie și au valori cuprinse între 0 și 28 (0…255);

int – numere întregi; ocupă 16biți de memorie și au valori cuprinse între -215 și 215 (-32767…32767);

unsigned int – numere întregi, pozitive; ocupă 16 biți de memorie și au valori cuprinse între 0 și 216 -1 (0 …65535);

long – numere întregi; ocupă 32 biți și au valori cuprinse între -231 și 231;

unsigned long – numere întregi, pozitive; ocupă 32 biți și au valori cuprinse între 0 și 232;

float – numere reale; ocupă 32 biți de memorie.

4.4.2 Stabilirea intrărilor și ieșirilor

Pentru a simplifica dezvoltarea aceluiași software pe structuri hardware diferite, în cadrul programului nu se vor apela pinii în mod direct, ci prin intermediul variabilelor. Prin urmare, începutul programului conține:

De asemenea sunt declarate și obiectele aferente senzorilor:

4.4.3 Funcția de calibrare senzori

Calibrarea senzorilor este necesară deoarece valorile generate variază în funcție de iluminare, traseu, diferențe de poziționare pe înălțime ș.a. Calibrarea senzorilor implică aflarea valorilor minime și maxime a fiecăruia și transpunerea acelui interval în domeniul 0 – 1000.

La fiecare apelare, sistemul comunică stadiul procedurii de calibrare atât prin intermediul LED-urilor cât și prin interfața serială.

4.4.4 Funcția de acționare motoare

Conform tabelului 3, pentru comandarea motoarelor trebuie acționate șase ieșiri, dintre care două PWM. Această funcție simplifică procesul la folosirea doar a variabilelor v1 și v2 astfel:

Modulul variabilelor v1 și v2 reprezintă viteza cu care se dorește a fi acționate cele două motoare;

Semnul variabilelor dă sensul de rotație;

Valoarea 0 implică frânarea maximă a motorului respectiv;

4.4.5 Citirea și filtrarea senzorilor periferici

Rolul senzorii periferici este de a monitoriza apariția sau dispariția liniei în zona lor. Acest lucru este foarte util în gestionarea obstacolelor, precum linie întreruptă și giratoriu. De asemenea, senzorul din față poate fi folosit ca martor pentru linie dreaptă sau curbă, astfel se poate accelera și frâna, sporind semnificativ performanțele sistemului. Valorile analogice ale senzorilor nu sunt necesare dar, în schimb, este nevoie de un efect de memorie al acestora. Pentru a digitiza semnalul, valoarea analogică este comparată cu un prag iar efectul de memorie este realizat folosind o întârziere la trecerea din 1 logic în 0 logic.

Deoarece senzorul din față va oscila deasupra liniei, filtrarea semnalului este un factor important pentru algoritmul de accelerare și frânare. Aceasta este realizată tot prin digitizare dar se folosesc două întârzieri: pe frontul crescător și descrescător.

4.4.6 Detectarea liniei

Biblioteca pusă la dispoziție de către Pololu nu oferă posibilitatea de a determina dacă sistemul se mai află sau nu pe traseu. Din acest motiv a trebuit intervenit asupra ei și implementate următoarele facilități:

Obiect.on_line – returnează 0 dacă nici un senzor nu depășește pragul de 150;

Obiect.readLine(sensorValues) – returnează -1 dacă linia a fost ultima oară detectată pe mijloc iar acum nici un senzor nu depășește pragul de 150.

Pentru a filtra imperfecțiunile traseelor s-a implementat o funcție cu rolul de stabili dacă sistemul se află, sau nu pe linie.

Apelarea acesteia returnează:

0 – sistemul nu se află pe linie;

1 – sistemul tocmai ce intrat pe linie;

2 – sistemul este stabil pe linie.

4.4.7 Stabilire viteză de funcționare

Folosind senzorul din față ca indicator de linie dreaptă sau curbă, sistemul poate funcționa cu două seturi de parametri și poate frâna în cazuri de urgență. Modul de funcționare este salvat în variabila speed. Aceasta are valoarea 0 pentru viteze mici și 1 pentru viteze mari. Folosirea unei frâne puternice permite folosirea de viteze mari în linii drepte.

Cea mai bună decelerare e realizată rotind motoarele în sens opus un timp direct proporțional cu viteza pe care o aveam setată. Pentru a evita frânările consecutive a fost folosită o temporizare de jumătate de scundă între frânări.

4.4.8 Stabilirea direcției și gestionarea obstacolelor

Parcurgerea obstacolelor implică izolarea lor din punct de vedere al senzorilor. Nu toate obstacolele au nevoie de secvențe de program speciale, spre exemplu zigzag și intersecția în cruce se rezolvă în mod automat datorită controlului PID și al metodei de stabilirea a poziției: dacă toți senzorii au aceeași valoare, abaterea va fi 0.

Obstacolul linie întreruptă necesită folosirea senzorilor periferici. În momentul ieșirii de pe linie, senzorul care a rămas activ, datorită întârzierilor, generează noua direcție până la detectarea liniei.

Obstacolul girator s-a dovedit a fi cel mai greu de soluționat. Intrarea în girator se făcea automat din controlul PID dar, datorită vitezei, senzorii nu aveau suficient timp pentru a comanda ieșirea. Soluția a fost detectarea intrării în girator. Condiția ce izola valorile senzorilor de restul obstacolelor a fost: senzorii 3 și 4(mijloc) < 400 iar martorii senzorilor periferici, stânga și dreapta, să fie activi.

Intrarea în funcția dedicată rezolvării peretelui se face în momentul pierderii liniei iar unul din senzorii laterali returnează o valoare mai mică de 100mm.

Parcurgerea peretelui se face într-o funcție separată, folosind un control PD.

4.4.9 Citirea senzorilor cu ultrasunete

Această funcție e folosită pentru calculul distanței înregistrate de către senzori și pentru apelarea funcției de urmărire a peretelui do_perete.

4.4.10 Controlul PID al motoarelor

În funcție de abaterea de la traseu, funcția dedicată controlului PID, generează variabila power_difference. Aceasta reprezintă diferența dintre turațiile motoarelor și, implici, controlează direcția robotului.

4.4.11 Bucla de program loop()

Zona aceasta de program este executată în mod continuu de către microcontroller. Pentru a menține programul cât mai logic și flexibil, de aici sunt executate subprogramele ce se ocupă de sarcinile sistemului.

4.4.12 Comunicația wireless

Funcția este executată de fiecare dată când utilizatorul trimite o comandă sistemului folosind interfața calculator.

5. REALIZAREA SISTEMULUI MOBIL AUTONOM

Pentru realizarea practică a sistemului a fost necesară obținerea următoarelor componente:

Două baterii li-ion Ultrafire 18650;

Linia de 8 senzori în infraroșu QTR-8A Reflectance Sensor Array;

Trei module de senzori în infraroșu QTR-1A Reflectance Sensor;

Două motoare 50:1 Micro Metal Gearmotor HP;

Două roți Pololu Wheel 60x8mm;

Trei senzori cu ultrasunete HC-SR04;

Un ball caster Pololu Ball Caster with 3/8" Metal Ball;

Componente electrice:

Driver de motoare L298 PowerSO20;

Două blocuri de diode;

Doi condensatori de 100 nF;

Două LED-uri, roșu și verde;

Două rezistențe de 280 Ω și de 330 Ω;

Trei butoane tip push button;

Convertor de nivel logic (TTL – CMOS);

Două module de comunicație wireless Xbee;

Pini mamă/tată;

Cabluri de conectare.

Șuruburi și piulițe M2 și M2,5;

Cablaj simplu placat FR04 pentru fabricarea shield-ului și al șasiului;

Fabricarea componentelor auxiliare:

Suport senzori orizontal;

Suport senzori pe înălțime;

Suport baterii;

Suporți motoare.

5.1 Realizarea șasiului și a shield-ului

Pentru realizarea circuitelor imprimate s-a folosit metoda de transfer termic. Acesta presupune imprimarea, cu laser, a cablajului, realizat în Eagle, pe o foaie lucioasă și transferul ei, prin presare și încălzire, pe suprafața de cupru a plăcii de textolit (Fig. 54). Următorul pas este scufundarea plăcii în clorura ferică pentru înlăturarea surplusului de cuprului. Această operațiune durează aproximativ 20 de minute după care placa se spală cu apă și săpun. Finalizarea cablajului constă în decuparea conturului și realizarea găurilor de fixare și de trecere.

Ultimul pas îl reprezintă lipirea componentelor electronice și acoperirea traseelor din cuprul cu fludor pentru a oprii oxidarea acestora în timp (Fig. 55, 56, 57).

5.2 Fabricarea componentelor auxiliare

Componentele de legătură (suporți, prinderi) au fost fabricate folosind instalația de prototipare rapidă, 3D Printing Elite, din dotarea laboratoarelor. Utilizarea acesteia este foarte facilă, fiind nevoie doar de modelul 3D ce se dorește realizat. Principiul de funcționare este acela de depunere start cu start al unui material încălzit aproape de punctul de topire. Rezultatul este obținerea, cu o anumită precizie, a respectivei forme fără a fi necesare prelucrările clasice precum strunjire sau frezare (Fig. 58). Ca material, instalația folosește plastic ABS Plus.

5.3 Asamblarea robotului

După ce au fost obținute plăcile electronice și fabricate componentele auxiliare a mai fost nevoie doar de asamblarea acestora (Fig. 59, 60, 61, 62).

5.4 Interfața hardware cu utilizatorul

Punerea în funcțiune a sistemului se face folosind un comutator, situat în spatele bateriilor, ce închide circuitul de alimentare cu tensiune. După aceasta, cele două leduri, roșu și verde, se vor aprinde. În acest moment sistemul așteaptă începerea procedurii de calibrare prin apăsarea butonului 2. În timpul calibrării ledul verde v-a rămâne aprins iar cel roșu se va stinge intermitent până la trecerea unui interval de timp. La finalul procedurii LED-ul roșu rămâne stins. Calibrarea senzorilor necesită deplasarea manuală a robotului astfel încât toți senzorii să treacă pe deasupra liniei.

După prima calibrare, sistemul intră în modul de pauză. Pentru a facilita dezvoltarea software, în acest mod, sistemul continuă să citească senzorii și să-și execute programul. Singura diferență fiind aceea că motoarele nu sunt comandate. În modul de pauză, procedura de calibrare poate fi repetată tot prin apăsarea butonului 2.

Intrarea în modul normal de lucru se realizează prin apăsarea butonului 1. În acest moment LED-ul verde se aprinde intermitent timp de 2 secunde după care se stinge și se aprinde cel roșu. Pentru revenirea în modul de pauză se apasă butonul 3.

Funcționarea sistemului este reprezentată prin următoarea schemă:

5.5 Interfața wireless

Pentru a grăbi dezvoltarea software sau remedierea problemelor hardware, sistemul transmite wireless o serie de informații legate de valorile senzorilor sau de stadiul programului. Pentru conectarea la calculator a modulului s-a folosit adaptorul Xbee Explorer USB. Transferul de date se face folosind orice aplicație tip serial monitor.

Instrucțiunile ce le poate primii sistemul sunt descrise în tabelul 4.

Tabelul 4 – Instrucțiuni interfață wireless

Deoarece direcția este una diferențială, utilizarea unor viteze de bază apropriate de viteza maximă (255) va împiedica sistemul să efectueze viraje. Din acest motiv, nu sunt recomandate viteze mai mari de 180.

Pentru a facilita evaluarea performanțelor, s-a implementat un cronometru intern dar și posibilitatea de a face media valorilor PWM trimise motoarelor. Timpul returnat este în zecimi de secundă.

6. Concluzii finale, contribuții și perspective

În urma parcurgerii acestui proiect se desprind următoarele concluzii:

Utilizarea bibliotecilor externe dau un anumit grad de flexibilitatea, însă acestea trebuie adaptate cerințelor;

Împărțirea programului în subprograme ajută la dezvoltarea mai ușoară a logicii sistemului;

Performanțe ridicate se pot obține și fără memorarea traseului, folosind un senzor antemergător.

Printre avantajele sistemului proiectat se evidențiază:

Utilizarea comunicației wireless pentru dezvoltarea software;

Structura hardware flexibilă – prin înlocuirea suporturilor se poate schimba poziționarea senzorilor și tipul motoarelor;

Programul de funcționare a fost împărțit în funcții individuale, ce pot fi ușor adaptate la noi cerințe.

În urma realizării proiectului s-au observat următoarele puncte slabe ce pot fi trate în dezvoltările ulterioare:

Bateriile Ultrafire 18650, deși oferă suficientă putere, sunt destul de voluminoase și greoaie;

Punctele de prindere pentru platforma Arduino, fiind dispuse pe diagonală, tind să încline șasiul;

Senzorii HC-SR04 nu sunt suficient de stabili la nivelul de precizie necesar, de ordinul milimetrilor.

Contribuțiile personale, respectiv originale în cadrul acestui proiect vizează:

Realizarea unei analize a sistemelor similare la nivel internațional;

Modelarea CATIA a întregului sistem;

Proiectarea și realizarea software a filtrelor pentru senzorii periferici;

Implementarea funcției de accelerare/frânare în funcție de senzorul antemergător.

Ca direcții viitoare de cercetare se pot considera:

Poziționarea diferită a componentelor astfel încât sistemul să aibă un centru de greutate coborât;

Folosirea unor roți mai late ce vor îmbunătății aderența;

Utilizarea encoder-elor pentru a determina vitezei de deplasare în scopul unei frânări eficiente;

Folosirea mai multor senzori periferici ce va reduce considerabil erorile de interpretare a obstacolelor.

Bibliografie

REZUMAT

Sistemele mobile autonome sunt sisteme mecatronice inteligente cu rolul de a ușura munca omului, capabile să ia singure decizii în scopul efectuării unor anumite sarcini. Acestea sunt folosite pentru explorare, transport, cercetare, mentenanță, reparații sau chiar divertisment.

Sistemele mobile tip line follower sunt sisteme autonome create special pentru a urmării trasee marcate cu o culoare diferită față de teren. Acestea au o importantă aplicabilitate în industrie, pentru transportarea diferitelor componente. La nivel academic se organizează competiții ce au în vedere proiectarea și realizarea sistemelor performante de acest tip.

Prezentul proiect de diplomă își propune realizarea unui sistem mobil autonom de tip line follower, în vederea selecției pentru participarea la Olimpiada Națională de Mecatronica 2013, secțiunea Roboți Mobili. În proiectarea acestuia s-a urmărit stabilirea unei baze hardware flexibile și obținerea performanțelor printr-un software specializat.

SUMMARY

Autonomous mobile systems are intelligent mechatronic systems used for easing human labor, capable of making their own decisions in order to accomplishment certain tasks. They are used for exploration, transportation, research, maintenance or even entertainment.

Line follower mobile systems are autonomous systems especially designed for following a different colored line than the floor. They are used in industrial environments for transporting different components. At an academic level, competitions are organized for designing and creating such systems.

The purpose of the current degree thesis is to create an autonomous line follower system that can take part in the National Mechatronics Olympiad 2013, Mobile Robots section. In the development process, the main objective was to establish a flexible hardware architecture and obtain performance by implanting specialized software.

ANEXE

ANEXA 1

PROGRAMUL DE FUNCȚIONARE PENTRU TRASEUL DE OBSTACOLE

#include <QTRSensors.h>

#include <NewPing.h>

int const NUM_SENSORS = 8;

//Senzorii in infrarosu periferici

QTRSensorsAnalog xtra((unsigned char[]) {7, 6 ,5}, 3, 8, 25);

unsigned int xtraValues[3];

//Senzorii in infrarosu in linie {pini}, nr senzori, samples[average]/reading, emitor pin

QTRSensorsAnalog qtra((unsigned char[]) {15, 14, 13, 12, 11, 10, 9, 8}, 8, 8, 25);

unsigned int sensorValues[NUM_SENSORS];

int M1, M2;

float Kp, Kd, Ki;

int m1Speed;

int m2Speed;

int speed;

float normalM = 80;

float normalP = 34;

float normalI = 0;

float normalD = 0.4;

unsigned long slowTime;

float slowM = 60;

float slowP = 45;

float slowI = 0;

float slowD = 0.25;

void normalParam() {

M2 = normalM;

M1 = M2 -2;

Kp = normalP;

Ki = normalI;

Kd = normalD;

Serial3.println("Normal Param");

speed = 1;

}

void slowParam() {

M2 = slowM;

M1 = M2 -2;

Kp = slowP;

Ki = slowI;

Kd = slowD;

Serial3.println("Slow Param");

speed = 0;

slowTime = millis();

}

int P = 0;

int I = 0;

int D = 0;

int last_P = 0;

int power_difference = 0;

int ideal = (NUM_SENSORS – 1)*500;

boolean slow = false;

boolean left90, right90, front90, front91;

int in_gyro;

//Driver motoare L298

int in1 = 7; //input 1

int in2 = 4; //input 2

int in3 = 3; //input 3

int in4 = 2; //input 4

int enable1 = 5; //stanga PWM

int enable2 = 6; //dreapta PWM

//Shield interfatare

int led_v = 52; //led verde

int led_r = 50; //led rosu

int btn_1 = 44; //buton 1

int btn_2 = 48; //buton 2

int btn_3 = 46; //buton 3

boolean paused = 0;

byte byteRead;

/////////////////////////////////////

////////Senzori ultrasonici//////////

/////////////////////////////////////

boolean sonar_on = true; //aici se opresc senzorii

int perete = 0;

unsigned int sonic_stg_distance = 100;

unsigned int sonic_drp_distance = 100;

unsigned int sonic_fata_distance = 300;

unsigned long sonic_lastCheck;

NewPing sonar_stg(10, 11, 10); //stanga (trig, echo, max_distance(cm))

NewPing sonar_drp(9, 8, 10); //dreapta (trig, echo, max_distance)

NewPing sonar_fata(35, 37, 30);

/////////////////////////////////////

////////////Detector linie///////////

/////////////////////////////////////

unsigned long last_line = 0;

unsigned long first_line = 0;

boolean line_in;

int detect_line() {

if (qtra.on_line) {

if (!line_in) {

line_in = true;

first_line = millis();//moment activare

}

last_line = millis();//ultimul moment activ

} else {

line_in = false;

}

if (millis() – last_line > 500) {//intarziere dezactiare

return 0; //nu exista linie

}

if (line_in) {

if (last_line – first_line > 70) {//intarziere activare

return 2; //e stabil pe linie

} else return 1; //acum a intrat pe linie

}

}

/////////////////////////////////////

///////Cronometru+Viteza medie///////

/////////////////////////////////////

unsigned long time;

unsigned long nr_sampl = 0;

boolean timer_on = false;

unsigned long avg_speed = 0;

void timer(boolean new_lap) {

unsigned int lap;

if (timer_on) {

lap = millis() – time;

lap = lap/100;

Serial3.print("Lap Time: ");

Serial3.println(lap);

Serial3.print("Average Speed: ");

Serial3.println(avg_speed/nr_sampl);

avg_speed = 0;

nr_sampl = 0;

if (new_lap) {

time = millis();

Serial3.println("- – – – – – – – – – – -");

Serial3.println("Lap Time Started");

Serial3.println("- – – – – – – – – – – -");

}

else {

Serial3.println("- – – – – – – – – – – -");

Serial3.println("Lap Timing off");

Serial3.println("- – – – – – – – – – – -");

timer_on = false;

}

}

else {

if (new_lap) {

timer_on = true;

time = millis();

avg_speed = 0;

nr_sampl = 0;

Serial3.println("- – – – – – – – – – – -");

Serial3.println("Lap Time Started");

Serial3.println("- – – – – – – – – – – -");

}

else {

avg_speed = 0;

nr_sampl = 0;

}

}

}

boolean serial_sens_on = false;

int mc = 0;

void print_sensors() {

if (serial_sens_on) {

mc++;

if (mc == 100) {

mc = 0;

}

if (mc == 0) {

unsigned int i;

Serial3.println("- – – Date senzori- – -");

xtra.readCalibrated(xtraValues);

for (i = 0; i < 3; i++) {

Serial3.print(xtraValues[i]);

Serial3.print(' ');

}

Serial3.println("");

for (i = 0; i < NUM_SENSORS; i++)

{

Serial3.print(sensorValues[i]);

Serial3.print(' ');

}

Serial3.println("");

Serial3.print("position=");

Serial3.println(P);

Serial3.print("left/right/front90/front91: ");

Serial3.print(left90);

Serial3.print(right90);

Serial3.print(front90);

Serial3.println(front91);

if (sonar_on) {

Serial3.print("Us fata: ");

Serial3.println(sonic_fata_distance);

Serial3.print("Us stanga: ");

Serial3.println(sonic_stg_distance);

Serial3.print("Us dreapta: ");

Serial3.println(sonic_drp_distance);

}

Serial3.println("- – – – – – – – – – – -");

}

}

}

void motoare(int v1,int v2)

{

if (v1 < 0) {

digitalWrite(in1,LOW);

digitalWrite(in2,HIGH);

v1= (-1)*v1;

analogWrite(enable1, v1);

}

else {

if(v1 >0) {

digitalWrite(in1,HIGH);

digitalWrite(in2,LOW);

analogWrite(enable1, v1);

}

else {

digitalWrite(in1,LOW);

digitalWrite(in2,LOW);

analogWrite(enable1, 255);

}

}

if (v2 < 0) {

digitalWrite(in3,LOW);

digitalWrite(in4,HIGH);

v2= (-1)*v2;

analogWrite(enable2, v2);

}

else {

if(v2 >0) {

digitalWrite(in3,HIGH);

digitalWrite(in4,LOW);

analogWrite(enable2, v2);

}

else {

digitalWrite(in3,LOW);

digitalWrite(in4,LOW);

analogWrite(enable2, 255);

}

}

if (millis()%100 == 0) {

nr_sampl++;

avg_speed += (v1+v2)/2;

}

}

void calibrare() {

Serial3.println("Calibrez");

qtra.resetCalibration(); //resetaza calibrarile vechi

xtra.resetCalibration();

digitalWrite(led_v, 1); //aprinde led-ul verde

int i;

for (i = 0; i < 150; i++) //executa calibrarea un interval de timp

{

qtra.calibrate();

xtra.calibrate();

if (i%10==0)

digitalWrite(led_r, !digitalRead(led_r)); //aprinde intermitent led-ul rosu

}

digitalWrite(led_r, 0); //stinge led-ul verde

digitalWrite(led_v, 1); //stinge led-ul rosu

Serial3.println("Terminat calibrarea");

}

void setup() {

//Grabeste convertor ADC–mai putin precis

const unsigned char PS_64 = (1 << ADPS2) | (1 << ADPS1);

const unsigned char PS_128 = (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);

ADCSRA &= ~PS_128; // remove bits set by Arduino library

ADCSRA |= PS_64; // set our own prescaler to 64

//SHIELD

Serial3.begin(9600);

pinMode(led_v, OUTPUT);

pinMode(led_r, OUTPUT);

pinMode(btn_1, INPUT);

pinMode(btn_2, INPUT);

pinMode(btn_3, INPUT);

//Pull-up resistors

digitalWrite(btn_1, HIGH);

digitalWrite(btn_2, HIGH);

digitalWrite(btn_3, HIGH);

//MOTOARE

pinMode(in1, OUTPUT);

pinMode(in2, OUTPUT);

pinMode(in3, OUTPUT);

pinMode(in4, OUTPUT);

pinMode(enable2, OUTPUT);

pinMode(enable1, OUTPUT);

//Calibrare

digitalWrite(led_v, 1);

digitalWrite(led_r, 1);

//slowParam();

normalParam();

Serial3.println("Astept inceperea calibrarii");

while (digitalRead(btn_2) == HIGH) {

}

calibrare();

Serial3.println("Astept start");

paused = true;

}

void loop() {

read_sonar();

read_xtra();

read_line();

if (perete <= 0) {

print_sensors();

set_speed();

computePID();

}

m1Speed = M1 + power_difference;

m2Speed = M2 – power_difference;

if (m1Speed > 255) m1Speed = 255;

if (m2Speed > 255) m2Speed = 255;

if (m1Speed < -255) m1Speed = -255;

if (m2Speed < -255) m2Speed = -255;

if (!paused) no_pause();

else pause();

//comm

if (Serial3.available()) {

serial();

}

}

void read_sonar() {

if (sonar_on) {

if (millis() – sonic_lastCheck >= 50) {

unsigned int uS = sonar_stg.ping();

sonic_stg_distance = uS / 5.7;

if (sonic_stg_distance < 10) {

sonic_stg_distance = 100;

}

uS = sonar_drp.ping();

sonic_drp_distance = uS / 5.7;

if (sonic_drp_distance < 10) {

sonic_drp_distance = 100;

}

uS = sonar_fata.ping();

sonic_fata_distance = uS / 5.7;

if (sonic_fata_distance < 10) {

sonic_fata_distance = 300;

}

sonic_lastCheck = millis();

if (perete == 1) {

do_perete(sonic_stg_distance);

}

if (perete == 2) {

do_perete(sonic_drp_distance);

}

}

}

}

void set_speed() {

//frana inainte de curba daca mergeam drept(aveam viteza)

if (!front90 && in_gyro == 0 && speed != 0) {

brake();

slowParam();

}

//revenire pe linie dreapta

if (speed == 0 && abs(power_difference) < 20 && front90 && in_gyro == 0 ) {

normalParam();

}

}

unsigned long first_gyro;

void read_line() {

P = qtra.readLine(sensorValues) – ideal;

//in cazul in care nu avem linie

//if (!detect_line() && !paused && perete <= 0) {

// if (!front90 && !left90 && !right90) {

// Serial3.println("Line not detected");

// paused = true;

// stop_count = M2 * 1.2;

// fullstop();

// }

//}

if (perete <= 0 && left90 && right90 && sensorValues[3] < 400 && sensorValues[4] < 400 && in_gyro == 0 && !paused && millis() – first_gyro > 2000) {

in_gyro = 1;

Serial3.print("Gyro:");

Serial3.println(in_gyro);

}

if (in_gyro == 1 && xtraValues[1] > 600) {

in_gyro = 2;

Serial3.print("Gyro:");

Serial3.println(in_gyro);

}

if (in_gyro == 2 && xtraValues[1] < 200) {

in_gyro = 3;

Serial3.print("Gyro:");

Serial3.println(in_gyro);

}

if (in_gyro == 3 && xtraValues[1] > 600) {

in_gyro = 0;

motoare(m2Speed,m1Speed);

delay(100);

first_gyro = millis();

Serial3.print("Gyro:");

Serial3.println(in_gyro);

}

//linie intrerupta (deci a iesit de pe ea dar nu pe lateral – prin fata)&& sonic_fata_distance < 280

if (P<-ideal && perete <= 0) {

if (!left90 && !right90 && !front90 && sonar_on && (sonic_drp_distance < 100 || sonic_stg_distance < 100)) {

slowParam();

Serial3.println("Perete");

brake();

if(sonic_drp_distance < 100) {

perete = 2;//drp

Serial3.println("Dreapta");

}

if(sonic_stg_distance < 100) {

perete = 1;//stg

Serial3.println("Stanga");

}

}

if (right90 && !left90 && !front90) {

P = +ideal;

last_P = P;

brake();

//Serial3.println("right turn");

}

if (left90 && !front90) { //preferam stanga in cazul in care avem ambii senzori activi

P = -ideal;

last_P = P;

brake();

//Serial3.println("left turn");

}

if (front90) {

P = 0;

last_P = P;

//Serial3.println("go ahead");

}

}

}

void computePID() {

if (P >= -ideal) {

D = P – last_P;

last_P = P;

I += P/Kp;

if (I > 255) I = 255;

if (I < -255) I = -255;

if(Ki > 0) {

power_difference = P/Kp + D*Kd + I/Ki;

} else {

power_difference = P/Kp + D*Kd;

}

}

}

unsigned long last_left;

unsigned long last_right;

unsigned long last_front;

unsigned long timein_front;

boolean front_delay;

void read_xtra() {

xtra.readCalibrated(xtraValues); //citeste valori analogice

static int prag = 400; //pragul de digitizare

static int timeout_delay = 170; //intarziere la dezactivare

//front 90 – intarziere la activare, senzor fata

if (xtraValues[1] > prag && !front_delay) {//trecerea din 0 in 1

timein_front = millis();// momentul trecerii

front_delay = true;

}

if (front_delay) {

if (xtraValues[1] < prag) {// trecerea in 0

front_delay = false;

front90 = false;

} else {

if (millis() – timein_front > 200) {// trecerea in 1 dupa 200ms

front90 = true;

} } }

//front 91 – intarziere la dezactivare, senzor fata

if (xtraValues[1] > prag) {

last_front = millis();

front91 = true;

}

if (millis() – last_front > 200){

front91 = false;

}

//left 90 – intarziere la dezactivare, senzor stanga

if (millis() – last_left > timeout_delay) {

left90 = 0;

}

if (xtraValues[0] > prag ){

left90 = 1;

last_left = millis();

}

//right 90 – intarziere la dezactivare, senzor dreapta

if (millis() – last_right > timeout_delay) {

right90 = 0;

}

if (xtraValues[2] > prag){

right90 = 1;

last_right = millis();

}

}

void no_pause() {

digitalWrite(led_r, 1);

digitalWrite(led_v, 0);

motoare(m1Speed, m2Speed);

if (digitalRead(btn_3) == LOW) //urmarim apasarea butonului 3

paused = true;

}

void pause()

{

digitalWrite(led_r, 0);

digitalWrite(led_v, 1);

motoare(0,0);

if(digitalRead(btn_1) == LOW) { //Start

Serial3.println("Wait!");

unsigned int i;

for (i = 0; i < 3; i++) {

digitalWrite(led_v, 0);

delay(250);

digitalWrite(led_v, 1);

delay(250);

}

Serial3.println("Go!");

in_gyro = 0;

perete = 0;

paused = false;

}

if(digitalRead(btn_2) == LOW) //Calibrare

calibrare();

}

int last_wall;

void do_perete(int dist) {

int d2, p2;

p2 = dist – 50;

d2 = p2 – last_wall;

last_wall = p2;

power_difference = p2/1.7 + d2*4.6;

if (sonic_fata_distance < 130) {

power_difference = -170;//-170

Serial3.println("Perete in fata");

}

if (perete == 1) //peretele este pe stanga

power_difference = -1 * power_difference;

if (detect_line() == 2 && dist == 100) {//daca vede linie si nu mai vede perete

Serial3.println("Exit perete");

perete = -1;

}

}

unsigned long last_brake;

void brake() {

if(millis() – last_brake > 500 ) { //sa nu franzeze de mai multe ori

if (!paused )

motoare(-255, -255);

Serial3.println("Trebuie frana!");

last_brake = millis();

while (millis() – last_brake < M2) {

read_xtra();

read_line();

//Serial3.println("Frana");

}

}

}

void fullstop() {

if (!paused) {

motoare(-255,-255);

delay(70);

Serial3.println("Trebuie frana!");

motoare(0,0);

}

}

void serial()

{

byteRead = Serial3.read();

switch (byteRead)

{

case 'r':

{

if (!paused)

{

fullstop();

}

paused = !paused;

if (paused)

timer(false);

//slow = false;

break;

}

case 't':

{

Serial3.println("- -Variabile sistem – -");

Serial3.print("left/right/front90/front91: ");

Serial3.print(left90);

Serial3.print(right90);

Serial3.print(front90);

Serial3.println(front91);

Serial3.print("in_gyro: ");

Serial3.println(in_gyro);

Serial3.print("sonar_on: ");

Serial3.println(sonar_on);

Serial3.print("perete: ");

Serial3.println(perete);

Serial3.print("speed: ");

Serial3.println(speed);

//Normal var

Serial3.println("–Normal Param–");

Serial3.print("normalM: ");

Serial3.println(normalM);

Serial3.print("normalP: ");

Serial3.println(normalP);

Serial3.print("normalD: ");

Serial3.println(normalD);

//Slow var

Serial3.println("–Slow Param–");

Serial3.print("slowM: ");

Serial3.println(slowM);

Serial3.print("slowP: ");

Serial3.println(slowP);

Serial3.print("slowD: ");

Serial3.println(slowD);

Serial3.println("- – – – – – – – – – – -");

break;

}

case 'L':

{ //start time/new lap

timer(true);

break;

}

case 'l':

{ //stop lap times

timer(false);

break;

}

case 'y':

{

serial_sens_on = !serial_sens_on;

Serial3.print("Send sensor data(1/0): ");

Serial3.println(serial_sens_on);

break;

}

case 'u':

{

sonar_on = !sonar_on;

Serial3.print("Ultrasonic sensors(1/0): ");

Serial3.println(sonar_on);

break;

}

case 'w':

{

motoare(78,80);

delay(150);

motoare(0,0);

Serial3.println("Fata");

break;

}

case 'a':

{

motoare(20,80);

delay(150);

motoare(0,0);

Serial3.println("Stanga");

break;

}

case 's':

{

motoare(80,20);

delay(150);

motoare(0,0);

Serial3.println("Dreapta");

break;

}

case 'z':

{

motoare(-82,-80);

delay(150);

motoare(0,0);

Serial3.println("Spate");

break;

}

case 'M':

{

normalM = Serial3.parseInt();

Serial3.println(normalM);

normalParam();

break;

}

case 'P':

{

normalP = Serial3.parseFloat();

Serial3.println(normalP);

normalParam();

break;

}

case 'I':

{

normalI = Serial3.parseFloat();

Serial3.println(normalI);

normalParam();

break;

}

case 'D':

{

normalD = Serial3.parseFloat();

Serial3.println(normalD);

normalParam();

break;

}

case 'm':

{

slowM = Serial3.parseInt();

Serial3.println(slowM);

slowParam();

break;

}

case 'p':

{

slowP = Serial3.parseFloat();

Serial3.println(slowP);

slowParam();

break;

}

case 'i':

{

slowI = Serial3.parseFloat();

Serial3.println(slowI);

slowParam();

break;

}

case 'd':

{

slowD = Serial3.parseFloat();

Serial3.println(slowD);

slowParam();

break;

}

}

}

Bibliografie

ANEXE

ANEXA 1

PROGRAMUL DE FUNCȚIONARE PENTRU TRASEUL DE OBSTACOLE

#include <QTRSensors.h>

#include <NewPing.h>

int const NUM_SENSORS = 8;

//Senzorii in infrarosu periferici

QTRSensorsAnalog xtra((unsigned char[]) {7, 6 ,5}, 3, 8, 25);

unsigned int xtraValues[3];

//Senzorii in infrarosu in linie {pini}, nr senzori, samples[average]/reading, emitor pin

QTRSensorsAnalog qtra((unsigned char[]) {15, 14, 13, 12, 11, 10, 9, 8}, 8, 8, 25);

unsigned int sensorValues[NUM_SENSORS];

int M1, M2;

float Kp, Kd, Ki;

int m1Speed;

int m2Speed;

int speed;

float normalM = 80;

float normalP = 34;

float normalI = 0;

float normalD = 0.4;

unsigned long slowTime;

float slowM = 60;

float slowP = 45;

float slowI = 0;

float slowD = 0.25;

void normalParam() {

M2 = normalM;

M1 = M2 -2;

Kp = normalP;

Ki = normalI;

Kd = normalD;

Serial3.println("Normal Param");

speed = 1;

}

void slowParam() {

M2 = slowM;

M1 = M2 -2;

Kp = slowP;

Ki = slowI;

Kd = slowD;

Serial3.println("Slow Param");

speed = 0;

slowTime = millis();

}

int P = 0;

int I = 0;

int D = 0;

int last_P = 0;

int power_difference = 0;

int ideal = (NUM_SENSORS – 1)*500;

boolean slow = false;

boolean left90, right90, front90, front91;

int in_gyro;

//Driver motoare L298

int in1 = 7; //input 1

int in2 = 4; //input 2

int in3 = 3; //input 3

int in4 = 2; //input 4

int enable1 = 5; //stanga PWM

int enable2 = 6; //dreapta PWM

//Shield interfatare

int led_v = 52; //led verde

int led_r = 50; //led rosu

int btn_1 = 44; //buton 1

int btn_2 = 48; //buton 2

int btn_3 = 46; //buton 3

boolean paused = 0;

byte byteRead;

/////////////////////////////////////

////////Senzori ultrasonici//////////

/////////////////////////////////////

boolean sonar_on = true; //aici se opresc senzorii

int perete = 0;

unsigned int sonic_stg_distance = 100;

unsigned int sonic_drp_distance = 100;

unsigned int sonic_fata_distance = 300;

unsigned long sonic_lastCheck;

NewPing sonar_stg(10, 11, 10); //stanga (trig, echo, max_distance(cm))

NewPing sonar_drp(9, 8, 10); //dreapta (trig, echo, max_distance)

NewPing sonar_fata(35, 37, 30);

/////////////////////////////////////

////////////Detector linie///////////

/////////////////////////////////////

unsigned long last_line = 0;

unsigned long first_line = 0;

boolean line_in;

int detect_line() {

if (qtra.on_line) {

if (!line_in) {

line_in = true;

first_line = millis();//moment activare

}

last_line = millis();//ultimul moment activ

} else {

line_in = false;

}

if (millis() – last_line > 500) {//intarziere dezactiare

return 0; //nu exista linie

}

if (line_in) {

if (last_line – first_line > 70) {//intarziere activare

return 2; //e stabil pe linie

} else return 1; //acum a intrat pe linie

}

}

/////////////////////////////////////

///////Cronometru+Viteza medie///////

/////////////////////////////////////

unsigned long time;

unsigned long nr_sampl = 0;

boolean timer_on = false;

unsigned long avg_speed = 0;

void timer(boolean new_lap) {

unsigned int lap;

if (timer_on) {

lap = millis() – time;

lap = lap/100;

Serial3.print("Lap Time: ");

Serial3.println(lap);

Serial3.print("Average Speed: ");

Serial3.println(avg_speed/nr_sampl);

avg_speed = 0;

nr_sampl = 0;

if (new_lap) {

time = millis();

Serial3.println("- – – – – – – – – – – -");

Serial3.println("Lap Time Started");

Serial3.println("- – – – – – – – – – – -");

}

else {

Serial3.println("- – – – – – – – – – – -");

Serial3.println("Lap Timing off");

Serial3.println("- – – – – – – – – – – -");

timer_on = false;

}

}

else {

if (new_lap) {

timer_on = true;

time = millis();

avg_speed = 0;

nr_sampl = 0;

Serial3.println("- – – – – – – – – – – -");

Serial3.println("Lap Time Started");

Serial3.println("- – – – – – – – – – – -");

}

else {

avg_speed = 0;

nr_sampl = 0;

}

}

}

boolean serial_sens_on = false;

int mc = 0;

void print_sensors() {

if (serial_sens_on) {

mc++;

if (mc == 100) {

mc = 0;

}

if (mc == 0) {

unsigned int i;

Serial3.println("- – – Date senzori- – -");

xtra.readCalibrated(xtraValues);

for (i = 0; i < 3; i++) {

Serial3.print(xtraValues[i]);

Serial3.print(' ');

}

Serial3.println("");

for (i = 0; i < NUM_SENSORS; i++)

{

Serial3.print(sensorValues[i]);

Serial3.print(' ');

}

Serial3.println("");

Serial3.print("position=");

Serial3.println(P);

Serial3.print("left/right/front90/front91: ");

Serial3.print(left90);

Serial3.print(right90);

Serial3.print(front90);

Serial3.println(front91);

if (sonar_on) {

Serial3.print("Us fata: ");

Serial3.println(sonic_fata_distance);

Serial3.print("Us stanga: ");

Serial3.println(sonic_stg_distance);

Serial3.print("Us dreapta: ");

Serial3.println(sonic_drp_distance);

}

Serial3.println("- – – – – – – – – – – -");

}

}

}

void motoare(int v1,int v2)

{

if (v1 < 0) {

digitalWrite(in1,LOW);

digitalWrite(in2,HIGH);

v1= (-1)*v1;

analogWrite(enable1, v1);

}

else {

if(v1 >0) {

digitalWrite(in1,HIGH);

digitalWrite(in2,LOW);

analogWrite(enable1, v1);

}

else {

digitalWrite(in1,LOW);

digitalWrite(in2,LOW);

analogWrite(enable1, 255);

}

}

if (v2 < 0) {

digitalWrite(in3,LOW);

digitalWrite(in4,HIGH);

v2= (-1)*v2;

analogWrite(enable2, v2);

}

else {

if(v2 >0) {

digitalWrite(in3,HIGH);

digitalWrite(in4,LOW);

analogWrite(enable2, v2);

}

else {

digitalWrite(in3,LOW);

digitalWrite(in4,LOW);

analogWrite(enable2, 255);

}

}

if (millis()%100 == 0) {

nr_sampl++;

avg_speed += (v1+v2)/2;

}

}

void calibrare() {

Serial3.println("Calibrez");

qtra.resetCalibration(); //resetaza calibrarile vechi

xtra.resetCalibration();

digitalWrite(led_v, 1); //aprinde led-ul verde

int i;

for (i = 0; i < 150; i++) //executa calibrarea un interval de timp

{

qtra.calibrate();

xtra.calibrate();

if (i%10==0)

digitalWrite(led_r, !digitalRead(led_r)); //aprinde intermitent led-ul rosu

}

digitalWrite(led_r, 0); //stinge led-ul verde

digitalWrite(led_v, 1); //stinge led-ul rosu

Serial3.println("Terminat calibrarea");

}

void setup() {

//Grabeste convertor ADC–mai putin precis

const unsigned char PS_64 = (1 << ADPS2) | (1 << ADPS1);

const unsigned char PS_128 = (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);

ADCSRA &= ~PS_128; // remove bits set by Arduino library

ADCSRA |= PS_64; // set our own prescaler to 64

//SHIELD

Serial3.begin(9600);

pinMode(led_v, OUTPUT);

pinMode(led_r, OUTPUT);

pinMode(btn_1, INPUT);

pinMode(btn_2, INPUT);

pinMode(btn_3, INPUT);

//Pull-up resistors

digitalWrite(btn_1, HIGH);

digitalWrite(btn_2, HIGH);

digitalWrite(btn_3, HIGH);

//MOTOARE

pinMode(in1, OUTPUT);

pinMode(in2, OUTPUT);

pinMode(in3, OUTPUT);

pinMode(in4, OUTPUT);

pinMode(enable2, OUTPUT);

pinMode(enable1, OUTPUT);

//Calibrare

digitalWrite(led_v, 1);

digitalWrite(led_r, 1);

//slowParam();

normalParam();

Serial3.println("Astept inceperea calibrarii");

while (digitalRead(btn_2) == HIGH) {

}

calibrare();

Serial3.println("Astept start");

paused = true;

}

void loop() {

read_sonar();

read_xtra();

read_line();

if (perete <= 0) {

print_sensors();

set_speed();

computePID();

}

m1Speed = M1 + power_difference;

m2Speed = M2 – power_difference;

if (m1Speed > 255) m1Speed = 255;

if (m2Speed > 255) m2Speed = 255;

if (m1Speed < -255) m1Speed = -255;

if (m2Speed < -255) m2Speed = -255;

if (!paused) no_pause();

else pause();

//comm

if (Serial3.available()) {

serial();

}

}

void read_sonar() {

if (sonar_on) {

if (millis() – sonic_lastCheck >= 50) {

unsigned int uS = sonar_stg.ping();

sonic_stg_distance = uS / 5.7;

if (sonic_stg_distance < 10) {

sonic_stg_distance = 100;

}

uS = sonar_drp.ping();

sonic_drp_distance = uS / 5.7;

if (sonic_drp_distance < 10) {

sonic_drp_distance = 100;

}

uS = sonar_fata.ping();

sonic_fata_distance = uS / 5.7;

if (sonic_fata_distance < 10) {

sonic_fata_distance = 300;

}

sonic_lastCheck = millis();

if (perete == 1) {

do_perete(sonic_stg_distance);

}

if (perete == 2) {

do_perete(sonic_drp_distance);

}

}

}

}

void set_speed() {

//frana inainte de curba daca mergeam drept(aveam viteza)

if (!front90 && in_gyro == 0 && speed != 0) {

brake();

slowParam();

}

//revenire pe linie dreapta

if (speed == 0 && abs(power_difference) < 20 && front90 && in_gyro == 0 ) {

normalParam();

}

}

unsigned long first_gyro;

void read_line() {

P = qtra.readLine(sensorValues) – ideal;

//in cazul in care nu avem linie

//if (!detect_line() && !paused && perete <= 0) {

// if (!front90 && !left90 && !right90) {

// Serial3.println("Line not detected");

// paused = true;

// stop_count = M2 * 1.2;

// fullstop();

// }

//}

if (perete <= 0 && left90 && right90 && sensorValues[3] < 400 && sensorValues[4] < 400 && in_gyro == 0 && !paused && millis() – first_gyro > 2000) {

in_gyro = 1;

Serial3.print("Gyro:");

Serial3.println(in_gyro);

}

if (in_gyro == 1 && xtraValues[1] > 600) {

in_gyro = 2;

Serial3.print("Gyro:");

Serial3.println(in_gyro);

}

if (in_gyro == 2 && xtraValues[1] < 200) {

in_gyro = 3;

Serial3.print("Gyro:");

Serial3.println(in_gyro);

}

if (in_gyro == 3 && xtraValues[1] > 600) {

in_gyro = 0;

motoare(m2Speed,m1Speed);

delay(100);

first_gyro = millis();

Serial3.print("Gyro:");

Serial3.println(in_gyro);

}

//linie intrerupta (deci a iesit de pe ea dar nu pe lateral – prin fata)&& sonic_fata_distance < 280

if (P<-ideal && perete <= 0) {

if (!left90 && !right90 && !front90 && sonar_on && (sonic_drp_distance < 100 || sonic_stg_distance < 100)) {

slowParam();

Serial3.println("Perete");

brake();

if(sonic_drp_distance < 100) {

perete = 2;//drp

Serial3.println("Dreapta");

}

if(sonic_stg_distance < 100) {

perete = 1;//stg

Serial3.println("Stanga");

}

}

if (right90 && !left90 && !front90) {

P = +ideal;

last_P = P;

brake();

//Serial3.println("right turn");

}

if (left90 && !front90) { //preferam stanga in cazul in care avem ambii senzori activi

P = -ideal;

last_P = P;

brake();

//Serial3.println("left turn");

}

if (front90) {

P = 0;

last_P = P;

//Serial3.println("go ahead");

}

}

}

void computePID() {

if (P >= -ideal) {

D = P – last_P;

last_P = P;

I += P/Kp;

if (I > 255) I = 255;

if (I < -255) I = -255;

if(Ki > 0) {

power_difference = P/Kp + D*Kd + I/Ki;

} else {

power_difference = P/Kp + D*Kd;

}

}

}

unsigned long last_left;

unsigned long last_right;

unsigned long last_front;

unsigned long timein_front;

boolean front_delay;

void read_xtra() {

xtra.readCalibrated(xtraValues); //citeste valori analogice

static int prag = 400; //pragul de digitizare

static int timeout_delay = 170; //intarziere la dezactivare

//front 90 – intarziere la activare, senzor fata

if (xtraValues[1] > prag && !front_delay) {//trecerea din 0 in 1

timein_front = millis();// momentul trecerii

front_delay = true;

}

if (front_delay) {

if (xtraValues[1] < prag) {// trecerea in 0

front_delay = false;

front90 = false;

} else {

if (millis() – timein_front > 200) {// trecerea in 1 dupa 200ms

front90 = true;

} } }

//front 91 – intarziere la dezactivare, senzor fata

if (xtraValues[1] > prag) {

last_front = millis();

front91 = true;

}

if (millis() – last_front > 200){

front91 = false;

}

//left 90 – intarziere la dezactivare, senzor stanga

if (millis() – last_left > timeout_delay) {

left90 = 0;

}

if (xtraValues[0] > prag ){

left90 = 1;

last_left = millis();

}

//right 90 – intarziere la dezactivare, senzor dreapta

if (millis() – last_right > timeout_delay) {

right90 = 0;

}

if (xtraValues[2] > prag){

right90 = 1;

last_right = millis();

}

}

void no_pause() {

digitalWrite(led_r, 1);

digitalWrite(led_v, 0);

motoare(m1Speed, m2Speed);

if (digitalRead(btn_3) == LOW) //urmarim apasarea butonului 3

paused = true;

}

void pause()

{

digitalWrite(led_r, 0);

digitalWrite(led_v, 1);

motoare(0,0);

if(digitalRead(btn_1) == LOW) { //Start

Serial3.println("Wait!");

unsigned int i;

for (i = 0; i < 3; i++) {

digitalWrite(led_v, 0);

delay(250);

digitalWrite(led_v, 1);

delay(250);

}

Serial3.println("Go!");

in_gyro = 0;

perete = 0;

paused = false;

}

if(digitalRead(btn_2) == LOW) //Calibrare

calibrare();

}

int last_wall;

void do_perete(int dist) {

int d2, p2;

p2 = dist – 50;

d2 = p2 – last_wall;

last_wall = p2;

power_difference = p2/1.7 + d2*4.6;

if (sonic_fata_distance < 130) {

power_difference = -170;//-170

Serial3.println("Perete in fata");

}

if (perete == 1) //peretele este pe stanga

power_difference = -1 * power_difference;

if (detect_line() == 2 && dist == 100) {//daca vede linie si nu mai vede perete

Serial3.println("Exit perete");

perete = -1;

}

}

unsigned long last_brake;

void brake() {

if(millis() – last_brake > 500 ) { //sa nu franzeze de mai multe ori

if (!paused )

motoare(-255, -255);

Serial3.println("Trebuie frana!");

last_brake = millis();

while (millis() – last_brake < M2) {

read_xtra();

read_line();

//Serial3.println("Frana");

}

}

}

void fullstop() {

if (!paused) {

motoare(-255,-255);

delay(70);

Serial3.println("Trebuie frana!");

motoare(0,0);

}

}

void serial()

{

byteRead = Serial3.read();

switch (byteRead)

{

case 'r':

{

if (!paused)

{

fullstop();

}

paused = !paused;

if (paused)

timer(false);

//slow = false;

break;

}

case 't':

{

Serial3.println("- -Variabile sistem – -");

Serial3.print("left/right/front90/front91: ");

Serial3.print(left90);

Serial3.print(right90);

Serial3.print(front90);

Serial3.println(front91);

Serial3.print("in_gyro: ");

Serial3.println(in_gyro);

Serial3.print("sonar_on: ");

Serial3.println(sonar_on);

Serial3.print("perete: ");

Serial3.println(perete);

Serial3.print("speed: ");

Serial3.println(speed);

//Normal var

Serial3.println("–Normal Param–");

Serial3.print("normalM: ");

Serial3.println(normalM);

Serial3.print("normalP: ");

Serial3.println(normalP);

Serial3.print("normalD: ");

Serial3.println(normalD);

//Slow var

Serial3.println("–Slow Param–");

Serial3.print("slowM: ");

Serial3.println(slowM);

Serial3.print("slowP: ");

Serial3.println(slowP);

Serial3.print("slowD: ");

Serial3.println(slowD);

Serial3.println("- – – – – – – – – – – -");

break;

}

case 'L':

{ //start time/new lap

timer(true);

break;

}

case 'l':

{ //stop lap times

timer(false);

break;

}

case 'y':

{

serial_sens_on = !serial_sens_on;

Serial3.print("Send sensor data(1/0): ");

Serial3.println(serial_sens_on);

break;

}

case 'u':

{

sonar_on = !sonar_on;

Serial3.print("Ultrasonic sensors(1/0): ");

Serial3.println(sonar_on);

break;

}

case 'w':

{

motoare(78,80);

delay(150);

motoare(0,0);

Serial3.println("Fata");

break;

}

case 'a':

{

motoare(20,80);

delay(150);

motoare(0,0);

Serial3.println("Stanga");

break;

}

case 's':

{

motoare(80,20);

delay(150);

motoare(0,0);

Serial3.println("Dreapta");

break;

}

case 'z':

{

motoare(-82,-80);

delay(150);

motoare(0,0);

Serial3.println("Spate");

break;

}

case 'M':

{

normalM = Serial3.parseInt();

Serial3.println(normalM);

normalParam();

break;

}

case 'P':

{

normalP = Serial3.parseFloat();

Serial3.println(normalP);

normalParam();

break;

}

case 'I':

{

normalI = Serial3.parseFloat();

Serial3.println(normalI);

normalParam();

break;

}

case 'D':

{

normalD = Serial3.parseFloat();

Serial3.println(normalD);

normalParam();

break;

}

case 'm':

{

slowM = Serial3.parseInt();

Serial3.println(slowM);

slowParam();

break;

}

case 'p':

{

slowP = Serial3.parseFloat();

Serial3.println(slowP);

slowParam();

break;

}

case 'i':

{

slowI = Serial3.parseFloat();

Serial3.println(slowI);

slowParam();

break;

}

case 'd':

{

slowD = Serial3.parseFloat();

Serial3.println(slowD);

slowParam();

break;

}

}

}