1

Istituto Tecnico Industriale Statale Alessandro Rossi

Classe V sez. B Ind. Elettronica e Telecomunicazioni Ind. Specializzazione robotica

ROBOT ESPLORATORE

Ferrari Marco & Verona Diego

INTRODUZIONE: Il nostro progetto desame fa parte del vasto e variegato mondo della robotica e dellautomazione industriale, che sempre con pi vigore trova spazio nella vita quotidiana. Le persone acquisiscono ogni giorno una maggiore familiarit con tali macchine, creando un fertile contesto per lo sviluppo della robotica. Il nostro progetto combina lautomazione ricercata nellambiente produttivo-industriale trasportandolo in un pi vasto contesto di utilizzo. La struttura leggere e compatta e un semplice sistema di locomozione rende il robot capace di muoversi in ambienti casalinghi, differendo dai robot ingombranti e fissi che si trovano in tutte le industrie. Il suo funzionamento generale permette una notevole flessibilit di utilizzo mentre la sua struttura, composta da braccio meccanico e ruote motrici, permette un campo di utilizzo molto ampio. Il robot in grado di raccogliere oggetti e di trasportarli in luoghi diversi. La modalit con cui svolge tale compito facilmente modificabile via software. I sensori del robot permettono allo stesso di individuare oggetti di natura diversa e comune eliminando il bisogno che gli oggetti stessi inviino al robot segnali particolari e rendendo il robot flessibile a diverse situazione e impieghi. I due sensori in questione riprendono, inoltre, il funzionamento della vista umana. La posizione dei sensori permette al robot di determinare, non solo la distanza delloggetto, ma anche la posizione dello stesso nello spazio. Con vari livelli di complessit software possibile programmare il robot dalla semplice individuazione di ostacoli nelle vicinanze fino alla ricostruzione dellambiente stesso che lo circonda. Tutte queste caratteristiche rendono il progetto interessante e durante la sua costruzione stato possibile aggiungere di volta in volta nuovi compiti. Nel progetto finale il robot in grado di ricercare fino a due oggetti differenti ed una volta raccolti inizia la ricerca della base dove depositer tali oggetti prima di cominciare una nuova ricerca. Durante la fase di ricerca il robot analizza i dati provenienti dai due sensori ultrasuoni, mentre ruota su se stesso in modo da ricoprire uno spazio di visuale di 360. Grazie al confronto dei dati raccolti identifica loggetto pi vicino e si muove in tale direzione. A questo punto, sempre grazie alluso di due sensori ultrasuoni, il robot inizia a muoversi verso loggetto tenendosi sempre il pi verticalmente possibile a questultimo in modo che loggetto entri nellapertura dove alloggiata la pinza. Quando l'oggetto entra nella bocca interrompe un fascio luminoso diretto verso un sensore di luce, in modo da avvertire il cervello del robot, che a sua volta far fermare i motori del robot. Mentre il robot immobile il braccio meccanico si aziona e, grazie a sensori di contatto, controlla il suo movimento depositando il primo oggetto nel relativo scomparto. Quando la pinza ritorna nella posizione originale il robot ricomincia la sua ricerca. Quando anche il secondo oggetto nello scompartimento relativo il robot inizia la ricerca della base. Per far riconoscere al robot il luogo preciso si

scelto di utilizzare una sorgente luminosa fissa e esterna al robot, in aggiunta a due sensori di luce posti sul lato frontale del robot. In questi modo il robot in grado di In seguire la sorgente pi luminosa. Raggiunta la base il robot deposita i due oggetti a terra e inizia una nuova ricerca.

DESCRIZIONE STRUTTURA E CORPO DEL ROBOT ROBOT: La struttura assume una forma di cubo con lato di 30 [cm] che ha la possibilit di muoversi grazie a 2 ruote motrici e 2 si ruote sferiche per il sostegno della struttura. Le ruote motrici le abbiamo . scelte di materiale con un alto coefficiente di attrito pe ovviare ai per problemi causati dalla mancanza di oblemi aderenza di alcune superfici Le ruote superfici. sferiche le abbiamo adottate invece per la loro semplicit e per i pochi problemi relativamente all'ingombro e al quasi

Figura 1: struttura robot

inesistente spazio di azione. L'intera struttura del robot e dei suoi componenti stata costruita con materiali che costruita soddisfano un buon rapporto peso/forza su supportata da un prezzo ragionevole. A tale scopo abbiamo usato prevalentemente alluminio per la struttura e plexiglas per una parte della struttura della carrucola. ra La base del robot stata creata con un foglio di plastica sottile, ma abbastanza resistente per sostenere il peso della circuiteria, delle lattine e del braccio. E' stata poi ritagliata unapertura sulla apertura base verso il lato frontale del robot e centrale rispetto la posizione dei sensori di ultrasuono e di luce, per dare la possibilit alla lattina di entrare e, una volta entrata, essere presa e trasportata sa verso la sua locazione predefinita. Il braccio meccanico composto da una pinza (comprata in kit di montaggio che comprata montaggio) viene sollevata da un servomotore grazie ad un filo da pesca che ass assicura affidabilit e resistenza all'usura e una

Figura 2: pinza robot

forza massima portante di 5 Kg. Per dar la possibilit alla pinza e alla carrucola di sibilit ruotare nelle tre posizioni (posizione di "presa", posizione sede 1 e 2 per le lattine) abbiamo utilizzato un ulteriore servomotore che sostiene il peso e permette il movimento. Per mantenere la struttura il pi vert verticale possibile durante la rotazione della base abbiamo agganciato la sua estremit superiore allo scheletro del robot assicurandone la possibilit di rotazione grazie ad un perno. Una parte della struttura del robot che ci ha particolarmente interessati stata quella relativa ai movimenti di presa, sollevamento, spostamento, discesa e rilascio della lattina. Da notare che per abbassare la pinza abbiamo sfruttato la forza di gravit e abbiamo utilizzato a nostro vantaggio lo spostamento del baricentro con e senza lattina della pinza stessa. Infatti la pinza Figura 3: servomotore carrucola attaccata superiormente ad una base quadrata di plexiglas che scorre su una rotaia verticale. frontalmente, I sensori luce, che vengono utilizzati frontalmente sono stati isolati dalla struttura perch il loro rivestimento esterno coincide con la base del fototransitro stesso, che a contatto con la struttura che funge da massa crea una situazione di anomalia dei zione sensori stessi. Sia i sensori di luce che i sensori ultrasuoni sono ubicati nella parte anteriore della struttura del robot e la loro funzione pu essere associata a quella degli occhi umani (percezione della profondit e distanza di un oggetto). Il sensore luce che abbiamo selezionato per creare un interruttore elettronico all'ingresso della bocca del robot illuminato da 5 led ad alta luminosit per escludere in maniera quasi assoluta la possibilit di inteferenza della luce ambient ambientale, che invece deve essere gestita, per i sensori di luce destinati alla ricerca della base, grazie ad una taratura minuziosa ed il pi possibile accurata. Il robot stato ideato e progettato interamente da noi e attribuiamo la sua struttura non perfettamente allineata ad un mente utilizzo di strumentazione di lavoro basilare e molto spesso inad inadeguata ad un simile progetto.

Figura 4: apertura frontale

SCHEMA A BLOCCHI DEL CONTROLLO DEL ROBOT ROBOT:

Figura 5: schema a blocchi generale

Andremo ora a spiegare, descrivere e dimensionare i vari blocchi che compongono il robot.

BATTERIA: Abbiamo scelto i pacchi batteria LiPo Li-BATT eco (Figura 6) perch hanno una capacit elevata, con un peso ridotto e dimensioni contenute. Le caratteristiche della batteria scelta sono riportate nella tabella seguente:
Figura 6: batteria

Caratteristiche Tecniche: Tensione Nominale Numero Elementi Capacita' mAh Valore C (in scarica) Corrente di scarica Dimensioni Peso Connettore di potenza Connettore di bilanciamento

11,1V 3 2000 12 max 24A 110 x 36 x 28 mm 185gr Multiplex M6 MPX/TP/FP

Tabella 1: Caratteristiche tecniche della batteria Lipo Li-BATT eco.

REGOLATORE DI TENSIONE (LM7805): Lintegrato LM7805 un regolatore di tensione CIRCUITO DEL REGOLATORE DI che fornisce in uscita una tensione stabilizzata TENSIONE di 5 [V] a patto che lingresso sia circa il doppio input LM7805 output LINE VREG VOLTAGE di tale valore. I valori dei condestatori e il loro VBATT 5V COMMON collegamento sono dati dalla casa produttrice. C1 C2 Il regolatore di tensione permette di ricavare dai 12 [V] della batteria, necessari per comandare in modo ottimale i motori, lalimentazione TTL compatibile necessaria ai DIDASCALIA: VBAT: tensione prelevata dalla vari crcuiti integrati. batteria. VCC (output): tensione stabilizzata di uscita pari a 5 [V] C1 = 0,33 [F] C2 = 0, 1 [F]

Per le caratteristiche tecniche dellintegrato LM7805 si rimanda al link: www.datasheetcatalog.org/datasheet/fairchild/LM7805.pdf

CIRCUITO DI CONTROLLO LIVELLO BATTERIA (LM7805): Il circuito rappresentato compara, grazie ad un amplificatore operazionale in configurazione comparatore, la tensione della batteria con una tensione di soglia. Loperazionale e lingresso invertente devono essere alimentati con una tensione stabilizzata, che non si abbassi con lo scendere della tensione della batteria che alimenta il robot; altrimenti il confronto di tali tensioni non avrebbe senso perch entrambi dipendenti dal livello della batteria stessa. Grazie allintegrato LM7805 posso avere i 5 [V] costanti desiderati, finch la batteria riamane sopra i 9 [V] circa. Pertanto il valore di soglia, oltre la quale deve accendersi il led per segnalare il basso livello della batteria, stato scelto abbastanza elevato, anche per non sforzare i motori, e vale circa 11 [V]. Non si ha una precisione elevata della tensione di soglia a causa dellutilizzo di resistenze di non alta CIRCUITO CONTROLLO LIVELLO BATTERIA precisione, inoltre, si dovuto adattare il VBAT VCC circuito ed il suo dimensionamento a valori di resistenze poresenti in commercio. La precisione non rappresenta comunque un LED R1 fattore decisivo per lutilizzo di tale circuito, 7 1 5 esso di sola indicazione. La tensione della RL 3 batteria viena abbassata grazie ad un V+ 6 PIC 2 partitore in modo da rendere pi semplice TLO81 R2 il confronto con la tensione stabilizzata. Se 4 prevale lingresso non invertente, quindi la 0 DIDASCALIA: tensione della batteria sopra al valore VBAT: tensione prelevata dalla batteria. desiderato di 11 [V], luscita si porta in VCC: tensione prelevata dal regolatore di tensione. saturazione (Vsat = Vcc = 5 [V]) e non viene R1 = 12 [k] R2 = 10 [k] a crearsi differenza di potenziale ai capi del RL = 330 [] diodo led che quindi rimane spento. Se invece, la batteria scende sotto il livello di tensione prestabilito, prevale lingresso invertente e luscita va a massa (-Vsat = Gnd = 0 [V]); di conseguenza il led si accende per la differenza di potenziale che viene a formarsi ai suoi capi. Dimensionamento componenti:

. . :

11

+= 5 += 1= 2 1+ 2
( , , )

= 11 : 2 = 10 2 5 = = 0,46 1 + 2 11

1 = 12

FUNZIONAMENTO SENSORE ULTRASUONI (SRF05): Il sensore ad ultrasuoni di cui si fatto uso SRF05 del tipo Trig/Echo. SRF05 non altro che l'acronimo di Sonic Range Finder 05 e come dice il suo stesso nome un misuratore di distanze che sfrutta gli ultrasuoni per determinare la distanza dall'ostacolo che ha di fronte, ed in questo caso il quinto della serie (05) . In Figura

Figura 7: sensore ad ultrasuoni SRF05 (modalit 2-pinTrig/Echo)

1 riportata la foto del sensore. I pin sulla destra sono utilizzati solo in fase di programmazione del dispositivo stesso ovvero per l'installazione del firmware all'interno del PIC che controlla il sensore stesso. Caratteristiche tecniche Tensione operativa Corrente operativa tipica Frequenza Portata Impulso di ritorno

Trigger di input Modalit di funzionamento Dimensioni

5 [V] 4 [mA] 40 [KHz] 1 [cm] 4 [m] Segnale TTL positivo, di durata proporzionale alla distanza rilevata. Impulso TTL di durata minima di 10 [uS] Pin singolo per trig/echo o 2 Pin SRF04 compatibile. 43 x 20 x H 17 [mm]

Tabella 2: caratteristiche tecniche dei sensori SRF05

Il principio di funzionamento del sensore ad ultrasuoni molto semplice. Gli ultrasuoni sono frequenze che il nostro orecchio non in grado di percepire. Il sensore in questione lavora alla frequenza di 40 [KHz] mentre un buon orecchio umano percepisce suoni fino a circa 22 [KHz]. Questo significa che il nostro orecchio non in grado di percepire il suono emesso da questo sensore. Quando il segnale, emesso dal sensore, raggiunge un ostacolo, viene riflesso. Dal momento che la velocit con cui il suono viaggia nello spazio libero, nota1, misurando il tempo che percorre tra l'emissione del segnale e il suo ritorno possibile risalire alla distanza dell'oggetto che ha causato la riflessione. Da questo si capisce che se non c' nessuno ostacolo non verr rilevato nessun eco. Il sensore SRF05 possiede due sensori ad ultrasuoni uno utilizzato come sorgente per generare la nota a 40 [KHz] e uno utilizzato come orecchio per rilevare l'eventuale segnale di eco derivante dalla presenza di un ostacolo. La presenza dei due sensori non in generale obbligatoria, sono infatti presenti sistemi con un solo sensore che funziona prima come sorgente e poi come orecchio per captare un eventuale eco. La velocit con cui il suono viaggia nello spazio libero viene a dipendere dall'umidit dell'aria dalla temperatura e anche dalla pressione atmosferica. Da questo si capisce che le misure saranno soggette ad un certo errore. In questo progetto non prevista nessuna autocalibrazione ma possibile comunque ottenere una precisione intorno al cm. Un altro fattore che pu causare un deterioramento della precisione del sensore legata alla forma dell'oggetto che causa la riflessione stessa. Forme complesse o troppo grandi causano riflessioni multiple che degradano la precisione del sensore stesso2. Anche in questi casi la precisione comunque pi che sufficiente in molte applicazioni. Il sensore utilizzabile secondo le specifiche riportate in Tabella 2 in un range compreso tra 1 [cm] e 4 [m] anche se l'accuratezza in questi due estremi non elevata. Vediamo ora in maggior dettaglio come comandare il sensore SRF05. Le linee di controllo sono quelle sulla sinistra di Figura 7. Queste possono essere impostate per due modalit di controllo differenti nominate 2-pin Trig/Echo e 1-pin Trig/Echo. La prima modalit riportata in Figura 7 ed quella da noi adoperata; possibile osservare che partendo dall'alto bisogna collegare il pin 1 a Vcc, il pin 2 per il segnale di output per l'echo, il pin 3 il segnale di input per il Trig, il pin 4 non connesso3 mentre il pin 5 collegato a massa.

1 La velocit del suono nello spazio libero e alla temperatura di 25C circa 340m/s, quindi se gridiamo la nostra voce giunger a 340m di distanza dopo un secondo. 2 Se si mettono troppo vicini un oggetto piccolo e uno grande il sensore tender a rilevare solo l'oggetto grande. 3 Non va collegato n a massa n a Vcc poich sulla scheda del sensore gia' presente un resistore di pull-up.

10

Per comandare il sensore in questa modalit bisogna inviare un impulso sulla linea di Trig di almeno 10 [uS]. Quando il sensore riceve questo impulso trasmette il segnale alla frequenza di 40 [KHz] in 8 piccoli colpetti, e si mette poi in attesa del segnale di eco. Il tempo che impiega il segnale d'eco a tornare al sensore misurabile dalla durata dell'impulso che presente in uscita alla linea Echo. Se la durata pi' di 30ms vuol dire che l'oggetto oltre i 4 metri della portata del sensore. Quanto appena descritto riassunto in Figura 8.

Figura 8: modalit 2-pin Trig/Echo e range di funzionamento

Il range di localizzazione di questo tipo di sensori quello che vedete nella Figura 8.

MICROINTERRUTTORE A LEVETTA (FINECORSA): Il funzionamento del finecorsa, usato come sensore di contatto, molto semplice. come VCC evidenzia il circuito, il deviatore, a riposo, FINECORSA collega il pin di ingresso del PIC a massa. Se PIC premuta la levetta viene commutato il R1 collegamento del PIC a Vcc in modo che il microcontrollore rilevi un impulso a livello logico DIDASCALIA: alto 5[v]. Grazie a tale VCC: tensione data dal regolatore di tensione. R1 = 10 [k] impulso, ricevuto dal microcontrollore, viene gestito il movimento della carrucola del robot. La resistenza necessaria durante il test che il PIC esegue quando lo si programma. Il microcontrollore infatti, durante tale test,
CIRCUITO SENSORI DI CONTATTO
Figura 9: interuttore finecorsa

11

abilita tutti le uscite portandole a livello logico alto, di conseguenza, senza la resistenza verrebbe a crearsi un cortociucuito e il programmatore (pikit2) non consentirebbe lo scaricamento del nuovo software nel PIC.

SENSORI SEGUI LUCE (L14G1): Il principio di funzionamento molto efficace e semplice: se il sensore di destra vede pi luce di quello di sinistra, la tensione allingresso invertente del comparatore sar maggiore di quella presente allingresso non invertente, quindi il segnale che arriver al CIRCUITO SENSORI SEGUI LUCE pic sar un livello logico VCC basso e, grazie ad un opportuno programma, il robot proceder girando a destra L14G1 L14G1_2 (quindi verso la parte 7 1 5 pi luminosa); nel caso 3 PIC 6 in cui la tensione 2 R1 TL081 allemettitore del 4 fototransistor sinistro R2 R3 R4 sia maggiore di quello destro allora il segnale di uscita del DIDASCALIA: comparatore sar a VCC: tensione data dal regolatore di tensione di 5 [V]. livello logico alto e R1 = 100 [K] R2 = 4,1 [K] quindi il robot girer a R3 = 41[K] sinistra. A differenza R4 = 330 [] del sistema utilizzato per identificare la presenza di luce o meno allingresso della bocca del robot, in questo sistema serve una calibrazione pi precisa e le possibilit di errore sono pi alte. Questo causato dal fatto che le distanze sono maggiori e che la luce ambientale influenza lintero funzionamento del sistema.

12

SENSORE LUCE BOCCA (L14G1): Per la rilevazione degli oggetti che entrano nella bocca del robot abbiamo utilizzato, in modo semplice ma efficace, un fototransistor e un operazionale in modalit comparatore. Collegando il fototransistor in modo che la base sia funzione della luce, si riscontra che pi luce c, pi alta la corrente sulla base e pi corrente passa dal collettore allemettitore, che collegato a massa tramite un trimmer che utilizzeremo per tarare il sensore. Il trimmer non di precisione perch non serve una calibrazione minuziosa dato che il sensore deve solo rilevare la presenza o loscuramento, da parte dell eventuale oggetto, di una fascio luminoso di 5 led rossi ad alta luminosit. I led ad alta luminosit garantiscono anche una efficace immunit del sensore alla luce ambientale che risulata molto pi debole di quella emanata da tali led. Lemettitore del fototransistor stato invece collegato allingresso invertente delloperazionale cos che luscita delloperazionale sia a livello alto solo in presenza di una tensione, allingresso non invertente, maggiore di quella presente allingresso invertente. Questo si riscontra quando la corrente che attraversa il fototransistor generi una tensione ai capi del trimmer, quindi allingresso invertente, minore della tensione garantita dal partitore.

Figura 10: trasduttore L14G1

13

Tale partitore necessario per mantenere, allingresso non invertente delloperazionale, una tensione di riferimento di 2,5 [V] e non 0 [V], come si potrebbe pensare; in quanto lemettitore del fototransistor avr sempre una, anche se piccola, corrente. Infine abbiamo collegato un led di alimentazione e un led in uscita delloperazionale per una pi facile lettura dei segnali e delle tensioni presenti nel circuito.

CIRCUITO SENSORE DI LUCE BOCCA

VCC

Led rossi ad alta luminosit

R1

Led alimentazione attiva

R3
7 3 1 5

R6

L14G1
2

PIC

TL081
4

R2

R4

R5

DIDASCALIA:
VCC: tensione data dal regolatore di tensione di 5 [V]. R1 = 100 [] R2 = 100 [K] R3 = 330 [] R4 = 330 [] R5 = 330 [] R6 = 330 []

Figura 11: trasduttore L14G1

14

SERVOMOTORI: Per i movimenti del braccio meccanico abbiamo deciso di adoperare tre servi, per alcune loro caratteristiche preferibili ai normali motori in corrente conitua: 1. 2. 3. 4. Coppia meccanica elevata a prezzi accessibili. Possibilit di controllo di precisione del movimento. Dimensioni e peso ridotti. Assenza di circuiti di controllo e potenza esterni al servo.

I servmotori scelti sono di tipo analogico, in quanto il costo dei servi digitali usciva costo dalle nostre possibilit: L escursione massima di rotazione tipica di 180 per due dei tre servo adoperati mentre il terzo servo multigiri e sar utilizzato per il funzionamento della carrucola che solleva ed abbassa la pinza. la Funzionamento generale del servo: In un servomotore l' energia meccanica viene fornita da un micro-motore a corrente microcontinua, il quale, qualora se ne inverta la polarit di alimentazione (scambiando, cio, il positivo con il negativo), in grado di invertire il verso di rota rotazione; al suo alberino, tramite una riduzione effettuata da un treno di piccoli ingranaggi, connesso il perno millerighe, che fuoriesce dal corpo del servo, sul quale siamo soliti avvitare la squadretta di comando. A questo perno anche collegato l'albe l'alberino di un minuscolo potenziometro, che quindi varia la propria resistenza in funzione della posizione assunta dalla squadretta. Il servo viene connesso al circuito di controllo tramite un cavetto tripolare: il conduttori rosso porta la tensione di alimentazione proveniente dal azione regolatore di tensione LM7805, su quello giallo invece ciclicamente presente il segnale di comando (di durata compresa tra 1 e 2 msec). Il piccolo potenziometro, che sappiamo collegato meccanicamente alla squadretta, invece elettricamente connesso ricamente Figura 12: interno servomotore ad un circuito monostabile e tarato per generare, quando la squadretta raggiunge gli estremi della sua rotazione, impulsi di durata variabile da 1 a 2 msec. Questo segnale, e quello proveniente dal circuito di controllo (PIC nel nostro caso), vengono inviati ad un circuito caso),

15

"comparatore", che ne raffronta la durata temporale; se l'impulso, chiamiamolo "di posizione" differisce da quello "di controllo", il comparatore aziona il motore, fornendo allo stesso la polarit idonea a farlo girare nel giusto verso, finch non verificher che la durata dei due impulsi divenuta uguale. In altre parole, la logica di controllo del servo "insegue" il segnale proveniente dal radiocomando, finch non riesce a generarne uno uguale; e, nel realizzare questa condizione, ottiene proprio il risultato ricercato. Caratteristiche del servo: Oltre alle dimensioni e al peso, vengono invariabilmente fornite due sole caratteristiche operative: velocit e coppia. Ma questi valori, pur di fondamentale importanza e facile interpretazione, non ne caratterizzano completamente la qualit; come in ogni servomeccanismo, bisogna prendere in esame anche due altri parametri fondamentali: la sensibilit e la precisione. Definiamo la sensibilit (in modo molto generico) come la capacit di reagire ad una minima variazione del segnale di comando: quanto pi la variazione percepita sar piccola, tanto pi elevata la sensibilit del sistema. La precisione, invece, pu essere definita come la capacit del servo di riportarsi sempre nella stessa posizione, qualora riceva un identico segnale di comando; e ci indipendentemente dal variare delle condizioni ambientali (temperatura, vibrazioni...), dal mutare del carico meccanico (resistenza opposta dalle superfici mobili comandate) e dalle modalit di operazione (ad esempio, se il ritorno in posizione del servo avvenga con movimento orario, oppure antiorario): tanto pi piccolo sar l'errore commesso, tanto migliore la precisione. Queste due caratteristiche rappresentano in realt gli incubi ricorrenti per tutti i progettisti di servomeccanismi in genere; si dimostra infatti matematicamente (si chiamavano Bode e Nyquist gli studiosi che hanno gettato le basi per effettuare calcoli in materia) che l'incremento della sensibilit e/o della precisione avviene sempre a scapito della stabilit del sistema. Ci significa che, cercando di migliorare oltre un dato valore questi parametri, si va inevitabilmente incontro ad un grave degrado nelle prestazioni; nel caso del servomotore, la squadretta inizierebbe ad oscillare avanti e indietro intorno alla posizione da raggiungere o, nel caso peggiore, finirebbe a fondo corsa, senza rispondere pi ai comandi. Come pu allora il costruttore di un servo migliorare la qualit del suo prodotto? I fattori determinanti sono: 1) l'impiego d'ingranaggi metallici lavorati con precisione, in modo da ridurre al minimo i giochi e l'usura meccanica;

16

2) l'adozione di un potenziometro di alta precisione, compensato rispetto alle variazioni di temperatura ed efficacemente protetto dalle influenze ambientali; 3) la realizzazione di circuiti elettronici aventi stabilit elevata e deriva termica minima. Comandare il servo: Nonostante ci sia una notevole somiglianza, il segnale di controllo dei Servo non assolutamente un segnale PWM. La notazione corretta PCM ovvero Pulse-Code Modulation che, letteralmente, si pu tradurre come Modulazione Codificata di Impulsi. Per convincersi sufficiente confrontare i due segnali con un normale oscilloscopio. Il Figura 13: interno servomotore funzionamento del Servo molto semplice. Come si pu osservare, dallo schema a blocchi, il segnale di Controllo arriva all'Elettronica. Questa lo confronta con il valore presente sul Potenziometro e decide se deve fare ruotare il motore ed in che direzione.Per comandare un Servo occorre, tramite il filo di controllo, inviare allo stesso una serie di impulsi TTL. La durata del singolo impulso determina la posizione dell'asse di uscita. Il tempo di pausa, tra un impulso ed il successivo, pu variare entro ampi limiti senza che si abbia una perdita di controllo del Servo. Dopo avere data una descrizione qualitativa cominciamo a dare delle indicazioni quantitative stabilendo dei precisi valori numerici.La durata dell'impulso pu variare tra un minimo di 1mS ed un massimo di 2mS. In funzione della durata di questo impulso il Servo far ruotare il perno di uscita, solidale con il potenziometro, fino al raggiungimento del completo equilibrio.

Relazione esistente tr larghezza degli impulsi e rotazione: La durata minima e massima dell'impulso corrisponde ai due estremi dell'escursione del Servo. L'impulso della durata di 1,5mS corrisponde alla posizione centrale, o mediana, del Servo. Bisogna ricordare che questo

Figura 14: rotazione albero servomotore

17

impulso deve essere continuamente rinfrescato (ripetuto) altrimenti il Servo, non essendo pi governato, si porter a riposo. Questo significa che non potr pi garantire di mantenere la posizione e l'albero potr essere mosso dalle forze casuali presenti sul carico applicato. La durata della Pausa, tra un impulso ed il successivo, deve essere compresa tra un minimo di 10mS ed un massimo di 40mS. La durata tipica della Pausa tra gli impulsi di 20mS che sono pari a 50Hz. Per il servo multigiri, invece, la variazione della durata dellimpulso di comando determina, oltre alla direzione di rotazione, la velocit con cui il servo ruota.

Caratteristiche dei servi usati: Caratteristiche HS 475 HB Tipo motore 3 poli ferrite Cuscinetti a sfera 2 Ingranaggi Resina Karbonite Coppia [Kg*cm] 5,5 (6 Volts) Velocit [Sec./40] 0,12 (6 Volts) Velocit [Sec/60] 0,18 (6 Volts) Dimensioni [mm] 41 x 20 x 37 Peso [g] 39
Tabella 3: caratteristiche HS - 475 HB e foto del servo

Caratteristiche HSR 1422CR Tipo di controllo Standard RC 1,5 [ms] servo fermo Coppia [Kg*cm] 2,8 Velocit 43.4 rpm (giri al minuto) variabile Senso di rotazione Orario / Antiorario Dimensioni [mm] 40.6 x 19.8 x 36.6 Peso [g] 41,7
Tabella 4: caratteristiche HSR - 1422 CR e foto del servo

18

MOTORI IN CORRENTE CONTINUA E DRIVER DI POTENZA (L298) (L298): I motori in corrente continua (dc) sono costituiti da uno statore formato da una carcassa metallica contenente uno o pi magneti che creano un campo allinterno dalla stessa e da un rotore,anche esso costituito da una carcassa metallica , il quale sostiene delle bobine collegate fra loro a livello del collettore che una volta alimentate generano un campo magnetico. Per effetto dellattrazione dei poli contrari e della repulsione dei poli di stesso segno, sul rotore si genera una coppia che lo mette in moto. Non appena il rotore si mette in moto alcune spazzole, operano su lame diverse del collettore, alimentando le bobine in maniera che lasse dei nuovi poli del rotore sia comunque perpendicolare a quello dello statore, causando Figura 15: motore in corrente continua quindi la continua rotazione del motore. La fluttuazione della coppia risultante diminuisce aumentando il numero di lame del collettore. La coppia che fornisce il motore e la sua velocit di rotazione sono dipendenti luna dallaltra. Si tratta di una caratteristica essenziale per questo tipo di motore. Tale relazione lineare e permette di conoscere sia la velocit a vuoto che la coppia di spunto del motore.

19

Da questo grafico si deduce la curva della potenza utile:

Le curve coppia-velocit e potenza utile dipendono dalla tensione di alimentazione velocit del motore. La tensione di alimentazione fornita per un certo motore corrisponde a per un impiego continuo di tale motore in funzionamento nominale alla temperatura ambiente di 20C. E peraltro possibile alimentare il motore con una tensione diversa, in generale compresa tra -50% e +100% della tensione previs ovviamente 50% prevista: se lo si sottoalimenta il motore sar meno potente mentre se lo si sovralimenta sar pi potente ma si riscalder maggiormente . Per variazioni della tensione di alimentazione comprese tra -25% e +50%, la nuova curva coppia 25% coppia-velocit sar parallela a quella nominale. Di conseguenza, sia la coppia di spunto che la velocit a allela vuoto varieranno dello stesso valore percentuale n%. Per quanto riguarda la potenza utile massima del motore, questa si ottiene :

Unaltra caratteristica importante data da grafico coppia-corrente,che permette di corrente,che conoscere la corrente assorbita a vuoto, oppure a motore bloccato bloccato.

20

Il rapporto tra la potenza meccanica utile che pu essere fornita e la potenza assorbita detto rendimento. I motori in corrente continua sono costruiti per funzionare permanentemente a velocit prossime alla propria velocit a vuoto, che essendo nella maggior parte dei casi troppo elevata si applica un motoriduttore. MOTORIDUTTORE: Le nozioni da sapere per il nostro scopo sono abbastanza banali e approssimabili senza problemi. I motori debbono fornire una forza maggiore del prodotto massa per accelerazione, pi altre forze che si oppongono al moto come quelle di attrito al rotolamento delle ruote (attrito volvente) .

Supponendo che il robot non debba pesare complessivamente pi di 2[kg], si voglia unaccelerazione a = 0.38[m/s^2] , il raggio delle ruote 3,65[cm] e approssimando a zero le forze che si oppongono al moto : = = 0,38 2 3,65 = 2,74

21

Essendo due i motori che muoveranno il robot, il peso si ripartir met su un motore e met sullaltro (sempre in situazioni ideali) dunque la coppia di ciascun motore dovr essere:
2 = 2,74 = 1,37 2

Per sicurezza, tenendo anche conto dellapprossimazione a zero delle forze resistenti, si scelto un motoriduttore con una coppia di 1,5[N*cm]. La seguente tabella riporta le caratteristiche del motoriduttore in questione:

Tabella 5: caratteristiche motori DC

A questo punto, conoscendo anche il numero di RPM, possibile calcolare la velocit di punta moltiplicando il numero di giri al secondo dellasse per la

22

circonferenza della ruota. Dividendo il numero di RPM (giri al minuto) per 60 si ottengono il numero di giri al secondo. 165 = 2,75 60 =2 = 2,75 = 0,73 0,73 = 2,0 /

PILOTAGGIO MOTORI DC: Il pilotaggio pi semplice per un motore quello ON-OFF che permette di mandare il motore alla massima velocit oppure fermarlo. Il circuito risulta abbastanza semplice e consiste in un interruttore ( Transistor,rel ecc) in serie al motore e un diodo di ricircolo sul motorino (si ricorda che il motore in DC un carico induttivo) per evitare danni sul resto del circuito. Se si vuole invertire anche il verso di rotazione si ricorre ad un ponte H, che consente di invertire il verso della corrente che attraversa il motore stesso. In seguito riportato lo schema semplificato di un ponte H: In questo caso chiusa la coppia di interruttori A2 e B1 consentendo alla corrente di circolare in un verso,viceversa interruttori A1 e B2 la corrente circolerebbe in unaltro verso. In questo caso se fosse chiusa la coppia di interruttore A1 e B1 la corrente circolerebbe in un altro verso.
Figura 16: schema semplificato di un ponte H

L298: Ll298 un driver per motori DC o stepper,costituito da due ponti H integrati,che supportano un elevato voltaggio (46V) ed elevate correnti (2A per ponte) e che possono essere pilotati con livelli in logica TTL. Ciascun ponte pu essere disabilitato

23

o abilitato tramite il relativo piedino di enable. Per il comando di tale integrato inviamo attraverso il pic due pin per motore: se hanno valore uguale il motore sta fermo; se hanno valore differente il motore si muove in avanti o indietro a seconda che il bit alto sia il primo o il secondo. La velocit dei motori invece comandata mediante segnale di tipo pwm proporzionale ai dati rilevati dai sensori ultrasuoni, senza doverne indicare il valore via software. Semplicemente: se rileva una distanza maggiore il sensore ad ultrasuoni destro, il motore relativo rimarr fermo, viceversa se rileva una distanza maggiore il sensore ultrasuoni sinistro. Il continuo alternarsi di acceso/spento di comando ai motori, in funzione dei dati rilevati dai due sensori, crea un segnale pwm che regola la velocit dei motori. Grazie al motoriduttore incorporato nel motore, non stato necessario modificare tale parametro a livello software, in quanto le velocit in gioco sono molto basse e il robot ha il tempo di girare nella giusta direzione senza una risposta immediata.

Figura 17: circuito interno L298

24

Caratteristiche tecniche dellL298:

Tabella 6: caratteristiche L298

Figura 18: scheda di potenza dei motori

25

PIC 18F458:

Figura 19: pin PIC 18F458

Figura 20: circuito di controllo (PIC 18F458)

Figura 21: schema a blocchi interno del PIC 18F458

Tabella 7: pin utilizzati

26

/*************************************************************************************************************** pic usato: 18f458 quarzo: 40 [MHz] motori cc: collegamenti: motore destro RB0 e RB1 pin 33 e 34 motore sinistro RB2 e RB3 pin 35 e 36 servomotore pinza: Vcc (rosso) Gnd (nero) segnale (giallo) RC2 pin 17 servomotore base: Vcc (rosso) Gnd (nero) segnale (giallo) RB4 pin 37 servomotore carrucola: Vcc (rosso) Gnd (nero) segnale (giallo) RC3 pin 18 sensori ultrasuoni: SRF05 collegamenti: Vcc (rosso) Echo (verde) RC1 e RC5 pin 16 e 24 Trigger (bianco/verde) RC0 e RC4 pin 15 e 23 Gnd (bianco) sensore bocca: / collegamenti: Vcc (rosso) Gnd (bianco) input (blu) output (bianco/blu) RC7 pin 26 sensori luce: / collegamenti: Vcc (rosso) Gnd (bianco) input (blu) output (bianco/blu) RB7 pin 40 fine corsa: / collegamenti: Vcc (rosso) Gnd (bianco) alto (bianco/giallo) RB5 pin 38 basso (bianco/giallo) RB6 pin 39 *************************************************************************************************************/ #include <htc.h> //include la libreria per i comandi

//VALORI DEFINE ******************** #define TEMP_i 10 //numero cicli for (variabile i) #define PAUSA 320 //pausa di circa 20 [ms] tra un impulso al servomotore e un altro #define TRIGGER 25 //impulso trigger sensori ultrasuoni di 10 [us] #define ECHO 50 //controlla il valore di echo ogni 20 [us] // FUNZIONI ******************** void apri_pinza(); void stringi_pinza(); void solleva_pinza(); void abbassa_pinza(); void ruota_pinza_o(); void ruota_pinza_a(); void avanti(); void indietro(); void sinistra(); void destra(); void fermo();

//apre la pinza //chiude la pinza //solleva la pinza //abbassa la pinza //ruota la pinza in senso orario //ruota la pinza in senso antiorario //robot avanti dritto //robot indietro dritto //robot gira sinistra //robot gira destra //robot fermo

//DICHIARAZIONE VARIABILI ******************** int remember; //serve per indicare se l'oggetto preso il primo o il secondo int d; //conta gli impulsi di us dall'invio del segnale di trigger del sensore ultrasuoni sinistro int z; //durata impulso di echo da sensore ultrasuoni destro int s; //conta gli impulsi di us dall'invio del segnale di trigger del sensore ultrasuoni sinistro int x; //durata impulso di echo da sensore ultrasuoni sinistro int i; //variabile di conteggio cicli for int pinza; //variabile di conteggio cicli for int robot; //variabile di conteggio cicli for(utilizzo di for di secondo livello) int control; //serve per verificare che sia passato sufficiente tempo per gli ultrasuoni int remo; //serve per verificare che siano inviati sufficienti impulsi per gli ultrasuoni int minore=100; //serve per trovare la lattina pi vicina int remember=0; //serve per verificare che siano inviati sufficienti impulsi per gli ultrasuoni int ostia=0; //serve ad accedere alla parte di programma specifica //a,b,c,e sono usate come label quindi non disponibili per evetuali variabili

27

//PROGRAMMA PRINCIPALE ******************** main(){ //SETTAGGIO REGISTRI ******************** TRISA=0; TRISB=0b11100000; TRISC=0b10100010; TRISD=0b00000000; TRISE=0; T0CON=0b11000001; //POSIZIONE INIZIALE PINZA ******************** inizio: fermo(); for(robot=0;robot<45;robot++){ stringi_pinza(); ruota_pinza_a(); } while(PORTBbits.RB6==0){ abbassa_pinza(); } for(robot=0;robot<30;robot++){ apri_pinza(); } pinza=0;

//PORTA in uscita //RB5,6,7 ingresso (FINE CORSA UP, DOWN, RITORNO) //RC1 e RC5 in ingresso (ECHO DX e ECHO SX) //PORTD in uscita //PORTE in uscita //prescaler=4 : frequenza=fosc/(4*prescaler)

//ferma i movimenti delle ruote del robot //ruota il braccio e stringi pinza

//abbassa finch non viene premuto il finecorsa inferiore

//GIRA IL ROBOT E RILEVA LA LATTINA PIU' VICINA ******************** if(ostia==0){ while(pinza<=150){ //aspetta che il robot faccia 360 for(i=0;i<10000;i++){ //gira con piccoli scatti e ogni scatto controlla la vista PORTBbits.RB0=1; //motore sinistro avanti PORTBbits.RB1=0; //motore sinistro avanti PORTBbits.RB2=0; //motore destro indietro PORTBbits.RB3=1; //motore destro indietro } pinza++; //rilevazione distanza d=0; //azzera la variabile di conteggio control=0; //azzera control remo=0; //azzera remember //GENERAZIONE IMPULSO DI TRIGGER(utilizza un solo sensore ultrasuoni) PORTCbits.RC0=1; //invia impulso di trigger TMR0L=0; //azzera timer while(TMR0L!=125); //durata impulso di trigger 10us PORTCbits.RC0=0; //spegni impulso di trigger //ALGORITMO RICEZIONE SEGNALE DI ECHO if(PORTCbits.RC1==1){ //se il segnale di echo dal sensore arrivato al pic while(TMR0L!=125); //delay di 50us - 125 d++; //conta quanti impulsi da 50us ci stanno finch l'echo 1 remo++; // incremento remember control=1; //quando control a 1 allora l'echo andato a livello alto goto p; } else{ //se il segnale di echo non arrivato al pic remo++; //incremento remember if(control==0){ //l'echo ancora a livello basso goto p; } else{ if(remo<700){ //se ho fatto meno di 700 controlli continuo goto p; } else{ //MEMORIZZA L'OGGETTO PIU' VICINO if(d<(minore+1)){ minore=d; } } } } //chiudi else } //chiudi while } //chiudi ostia=0 //ferma il robot per evitare eventuali errori for(i=0;i<60;i++){ for(pinza=0;pinza<3000;pinza++){

P:

28

fermo(); } } //GIRA SU SE STESSO FINCH NON RITROVA L'OGGETTO PI VICINO GI MEMORIZZATO ******************** while(1){ //rilevazione distanza d=0; //azzera la variabile di conteggio control=0; //azzera control remo=0; //azzera remember //GENERAZIONE IMPULSO DI TRIGGER(utilizza un solo sensore ultrasuoni) PORTCbits.RC0=1; //invia impulso di trigger TMR0L=0; //azzera timer while(TMR0L!=125); //durata impulso di trigger 10us PORTCbits.RC0=0; //spegni impulso di trigger //ALGORITMO RICEZIONE SEGNALE DI ECHO if(PORTCbits.RC1==1){ //segnale di echo dal sensore arrivato al pic while(TMR0L!=125); //delay di 50us - 125 d++; //a conta quanti impulsi da 50us ci stanno finch l'echo a 1 remo++; //incrementto remember control=1; //quando control a 1 significa che echo andato a 1 goto y; } else{ //se il segnale di echo non arrivato al pic remo++; //incremento remo if(control==0){ //l'echo ancora a livello basso goto y; } else{ if(remo<700){ //se ho fatto meno di 700 controlli continuo goto y; } else{ if(d<=minore){ //se vede l'oggetto pi vicino smette di girare ostia=1; goto vai; } else{ //continua a girare for(i=0;i<10000;i++){ PORTBbits.RB0=1; //motore sinistro avanti PORTBbits.RB1=0; //motore sinistro avanti PORTBbits.RB2=0; //motore destro indietro PORTBbits.RB3=1; //motore destro indietro } } } } //chiudi else } //chiudi else(se il segnale non ritornato al pic) //chiudi while

y:

} vai:

//INIZIO RICERCA E PRELIEVO OGGETTI ******************** if(ostia==1){ while(1){ //ciclo infinito if(PORTCbits.RC7==0){ //se la bocca del robot libera //CERCA OGGETTO ******************** //rilevazione distanza d=0; //azzera la distanza destra s=0; //azzera la distanza sinistra //GENERAZIONE IMPULSO DI TRIGGER (SENSORE DESTRO) PORTCbits.RC4=1; //invia impulso di trigger sensore destro TMR0L=0; //azzera timer while(TMR0L!=TRIGGER); //durata impulso di trigger 10us PORTCbits.RC4=0; //spegni impulso di trigger sensore destro //ALGORITMO RICEZIONE SEGNALE DI ECHO (SENSORE SINISTRO) if(PORTCbits.RC1==1){ //segnale di echo dal sensore arrivato al pic while(TMR0L!=ECHO); //delay di 6us corrispondenti ad 1mm s++; //a conta quanti impulsi ci stanno finch l'echo a livello alto x=s; //x contiene la distanza massima rilevata dal sensore sinistro goto a; }

a:

29

//GENERAZIONE IMPULSO DI TRIGGER (SENSORE SINISTRO) PORTCbits.RC0=1; //invia impulso di trigger sensore sinistro TMR0L=0; //azzera timer while(TMR0L!=TRIGGER); //durata impulso di trigger 10us PORTCbits.RC0=0; //spegni impulso di trigger sensore sinistro //ALGORITMO RICEZIONE SEGNALE DI ECHO (SENSORE DESTRO) if(PORTCbits.RC5==1){ //segnale di echo dal sensore destro arrivato al pic while(TMR0L!=ECHO); //delay di 6us corrispondenti ad 1mm d++; // conta quanti impulsi l'echo a livello alto z=d; //z contiene la distanza massima rilevata dal sensore destro goto b; } //CONFRONTO DISTANZE RILEVATE if((d<600)&&(s<600)){ if(x<z){ sinistra(); } if(x>z){ destra(); } if(x==z){ avanti(); } } else{ avanti(); } } else{ //se l'oggetto nella bocca avanti un secondo e fermati //RECUPERO OGGETTO ******************** for(i=0;i<TEMP_i;i++){ for(pinza=0;pinza<3000;pinza++){ fermo(); } } for(i=0;i<TEMP_i;i++){ for(pinza=0;pinza<3000;pinza++){ avanti(); } } for(i=0;i<TEMP_i;i++){ for(pinza=0;pinza<2000;pinza++){ fermo(); } } //STRINGI PINZA E SOLLEVALA while(PORTBbits.RB5==0){ stringi_pinza(); solleva_pinza(); } //RUOTA BRACCIO(in funzione di remember) E STRINGI PINZA for(robot=0;robot<45;robot++){ stringi_pinza(); //POSIZIONE OGGETTO 1 (DIETRO) if(remember==0){ //se il primo oggetto che trovi mettilo dietro //RUOTA BASE (ORARIO) for(i=0;i<TEMP_i;i++){ //invia al servomotore un impulso di circa 1,9 [ms] for(pinza=0;pinza<50;pinza++){ PORTBbits.RB4=1; } } for(i=0;i<TEMP_i;i++){ //pausa tra gli impulsi inviati di circa 20ms for(pinza=0;pinza<PAUSA;pinza++){ PORTBbits.RB4=0; } } } //POSIZIONE OGGETTO 2 (DESTRA) if(remember==1){ //se il secondo oggetto che trovi mettilo a destra //RUOTA BASE (ORARIO) for(i=0;i<TEMP_i;i++){ //invia al servomotore un impulso for(pinza=0;pinza<29;pinza++){ PORTBbits.RB4=1;

b:

//se entrambi i sensori oggetto < 600 //se la distanza dal sensore sx < di quella del sensore dx

//se la distanza dal sensore dx < di quella del sensore sx

//se la distanza rilevata dai sensore uguale

//se non vedo oggetti nelle vicinanze vado dritto

30

} }

} } for(i=0;i<TEMP_i;i++){ //pausa tra gli impulsi inviati di circa 20ms for(pinza=0;pinza<PAUSA;pinza++){ PORTBbits.RB4=0; } } //chiudi remember //chiudi ciclo per far ruotare la pinza

//ABBASSA E STRINGI PINZA while(PORTBbits.RB6==0){ stringi_pinza(); abbassa_pinza(); } //INIZIO RICERCA BASE ******************** if (remember>=1){ //se ha due lattine ritorna alla base con la pinza sulla seconda destra ostia=2; goto e; } //SOLLEVA E APRI PINZA while(PORTBbits.RB5==0){ apri_pinza(); solleva_pinza(); } //RUOTA (ANTIORARIO) E STRINGI PINZA for(robot=0;robot<45;robot++){ stringi_pinza(); ruota_pinza_a(); } //POSIZIONE ORIGINARIA while(PORTBbits.RB6==0){ abbassa_pinza(); } for(robot=0;robot<45;robot++){ apri_pinza(); } remember++; ostia=0; goto inizio; } } } //ALGORITMO RICERCA BASE ******************** e: if(ostia==2){ while(1){ while(1){ if(PORTBbits.RB7==0){ for(i=0;i<1000;i++){ destra(); } } else{ for(i=0;i<1000;i++){ sinistra(); } } //rilevazione distanza d=0; control=0; remo=0; //prima incontra base poi esce dal while e deposita lattine //in base ai sensori luce gli arriva un segnale //chiudi whlie ostia=1 //chiudi ostia=1

//finch il fine corsa basso non viene attivato

//incrementa remember //serve per far ripartire dall'inizio //il robot comincer il programma dall'inizio

//azzera la variabile di conteggio //azzera control //azzera remember

//GENERAZIONE IMPULSO DI TRIGGER PORTCbits.RC0=1; //invia impulso di trigger TMR0L=0; //azzera timer while(TMR0L!=125); //durata impulso di trigger 10us PORTCbits.RC0=0; //spegni impulso di trigger

31

q:

} mucca:

//ALGORITMO RICEZIONE SEGNALE DI ECHO if(PORTCbits.RC1==1){ //segnale di echo dal sensore arrivato al pic while(TMR0L!=125); //delay di 50us - 125 d++; //a conta quanti impulsi ci stanno finch l'echo a livello alto remo++; //ogni volta che controllo incrementto remo control=1; //quando control a 1 significa che echo a livello alto goto q; } else{ //se il segnale di echo non arrivato al pic remo++; //incremento remember if(control==0){ //se control significa che l'echo ancora andato a livello bass goto q; } else{ //altrimenti if(remo<700){ //se ho fatto meno di 700 controlli continuo goto q; } else{ if(d<=2) { //se vedi la base a pochi centimetri goto mucca; } } } } //chiudi while sensore

//BASE TROVATA ******************** fermo(); //SOLLEVA STRINGENDO PINZA(la pinza si trova ancora nella 2 posizione(a destra)) while(PORTBbits.RB5==0){ //tira su tutto e stringi(la pinza gi nella posizione destra) stringi_pinza(); solleva_pinza(); } //RUOTA (ANTIORARIO) STRINGENDO PINZA for(robot=0;robot<45;robot++){ stringi_pinza(); ruota_pinza_a(); } //ABBASSA STRINGENDO PINZA while(PORTBbits.RB6==0){ stringi_pinza(); abbassa_pinza(); } //APRI PINZA for(robot=0;robot<45;robot++){ apri_pinza(); } //DEPOSITA SECONDA LATTINA SPOSTANDOSI INDIETRO for(i=0;i<10;i++){ for(pinza=0;pinza<32000;pinza++){ indietro(); } } //si sposta pi indietro e deposita l'altra lattina fermo(); //SOLLEVA PINZA while(PORTBbits.RB5==0){ solleva_pinza(); } //RUOTA PINZA (ORARIO) for(robot=0;robot<45;robot++){ for(i=0;i<TEMP_i;i++){ //invia al "servomotore base" un impulso di circa 1,9 [ms] for(pinza=0;pinza<50;pinza++){ PORTBbits.RB4=1; } } for(i=0;i<TEMP_i;i++){ //pausa tra gli impulsi inviati al "servomotore base" di circa 20ms

32

for(pinza=0;pinza<PAUSA;pinza++){ PORTBbits.RB4=0; } } } //ABBASSA PINZA(GIA' APERTA) while(PORTBbits.RB6==0){ abbassa_pinza(); } //SOLLEVA STRINGENDO PINZA while(PORTBbits.RB5==0){ stringi_pinza(); solleva_pinza(); } //RUOTA (ANTIORARIO) STRNGENDO PINZA for(robot=0;robot<45;robot++){ stringi_pinza(); ruota_pinza_a(); } //ABBASSA STRINGENDOLA PINZA while(PORTBbits.RB6==0){ stringi_pinza(); abbassa_pinza(); } for(robot=0;robot<45;robot++){ apri_pinza(); } for(pinza=0;pinza<100;pinza++){ for(i=0;i<10000;i++){ indietro(); } } //SI GIRA E CONTINUA LA RICERCA DELLE LATTINE for(pinza=0;pinza<80;pinza++){ for(i=0;i<10000;i++){ PORTBbits.RB0=1; //motore sinistro avanti PORTBbits.RB1=0; //motore sinistro avanti PORTBbits.RB2=0; //motore destro indietro PORTBbits.RB3=1; //motore destro indietro } } ostia=0; //necessario per ripartire il programma dall'inizio remember=0; //settaggio necessario per prendere 2 nuove lattine goto inizio; //riparte dall'inizio //chiudi while(ostia=2) //chiudi ostia=2 //chiudi int

} } }

//FUNZIONI ******************** //MOVIMENTI PINZA ******************************* //APRI PINZA void apri_pinza(){ for(i=0;i<TEMP_i;i++){ for(pinza=0;pinza<1;pinza++){ PORTCbits.RC2=1; } } for(i=0;i<TEMP_i;i++){ for(pinza=0;pinza<PAUSA;pinza++){ PORTCbits.RC2=0; } } } //STRINGI PINZA void stringi_pinza(){ for(i=0;i<TEMP_i;i++){

//invia al "servomotore pinza" un impulso di circa 1,9 [ms]

//pausa tra gli impulsi inviati al "servomotore pinza" di circa 20ms

//invia al "servomotore pinza" un impulso di circa 1,9 [ms]

33

for(pinza=0;pinza<29;pinza++){ PORTCbits.RC2=1; } } for(i=0;i<TEMP_i;i++){ for(pinza=0;pinza<PAUSA;pinza++){ PORTCbits.RC2=0; } } } //SOLLEVA PINZA void solleva_pinza(){ for(i=0;i<TEMP_i;i++){ for(pinza=0;pinza<25;pinza++){ PORTCbits.RC3=1; } } for(i=0;i<TEMP_i;i++){ for(pinza=0;pinza<PAUSA;pinza++){ PORTCbits.RC3=0; } } } //ABBASSA PINZA void abbassa_pinza(){ for(i=0;i<TEMP_i;i++){ for(pinza=0;pinza<40;pinza++){ PORTCbits.RC3=1; } } for(i=0;i<TEMP_i;i++){ for(pinza=0;pinza<200;pinza++){ PORTCbits.RC3=0; } } } //RUOTA PINZA (ORARIO) void ruota_pinza_o(){ for(i=0;i<TEMP_i;i++){ for(pinza=0;pinza<29;pinza++){ PORTBbits.RB4=1; } } for(i=0;i<TEMP_i;i++){ for(pinza=0;pinza<PAUSA;pinza++){ PORTBbits.RB4=0; } } } //RUOTA PINZA (ANTIORARIO) void ruota_pinza_a(){ for(i=0;i<TEMP_i;i++){ for(pinza=0;pinza<1;pinza++){ PORTBbits.RB4=1; } } for(i=0;i<TEMP_i;i++){ for(pinza=0;pinza<PAUSA;pinza++){ PORTBbits.RB4=0; } } } //MOVIMENTI ROBOT ******************** //AVANTI void avanti(){ PORTBbits.RB0=1; PORTBbits.RB1=0; PORTBbits.RB2=1; PORTBbits.RB3=0; } //pausa tra gli impulsi inviati al "servomotore pinza" di circa 20ms

//invia al "servomotore carrucola" un impulso di circa 1,9 [ms]

//pausa tra gli impulsi inviati al "servomotore carrucola" di circa 20ms

//invia al "servomotore carrucola" un impulso di circa 1,9 [ms]

//pausa tra gli impulsi inviati al "servomotore carrucola" di circa 20ms

//invia al "servomotore base" un impulso di circa 1,9 [ms]

//pausa tra gli impulsi inviati al "servomotore base" di circa 20ms

//invia al "servomotore base" un impulso di circa 1,9 [ms]

//pausa tra gli impulsi inviati al "servomotore base" di circa 20ms

//motore sinistro avanti //motore sinistro avanti //motore destro avanti //motore destro avanti

34

//INDIETRO void indietro(){ PORTBbits.RB0=0; PORTBbits.RB1=1; PORTBbits.RB2=0; PORTBbits.RB3=1; } //SINISTRA void sinistra(){ PORTBbits.RB0=1; PORTBbits.RB1=1; PORTBbits.RB2=1; PORTBbits.RB3=0; } //DESTRA void destra(){ PORTBbits.RB0=1; PORTBbits.RB1=0; PORTBbits.RB2=1; PORTBbits.RB3=1; } //FERMO void fermo(){ PORTBbits.RB0=0; PORTBbits.RB1=0; PORTBbits.RB2=0; PORTBbits.RB3=0; }

//motore sinistro indietro //motore sinistro indietro //motore destro indietro //motore destro indietro

//motore sinistro fermo //motore sinistro fermo //motore destro avanti //motore destro avanti

//motore sinistro avanti //motore sinistro avanti //motore destro fermo //motore destro fermo

//motore sinistro fermo //motore sinistro fermo //motore destro fermo //motore destro fermo