1

Istituto Tecnico Industriale Statale “Alessandro Rossi”
Classe V sez. B Ind. Elettronica e Telecomunicazioni Ind. Specializzazione robotica

“ROBOT ESPLORATORE”

Ferrari Marco & Verona Diego

2

INTRODUZIONE: Il nostro progetto d’esame fa parte del vasto e variegato mondo della robotica e dell’automazione 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 l’automazione ricercata nell’ambiente 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 dell’oggetto, 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 dell’ambiente 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 l’oggetto più vicino e si muove in tale direzione. A questo punto, sempre grazie all’uso di due sensori ultrasuoni, il robot inizia a muoversi verso l’oggetto tenendosi sempre il più verticalmente possibile a quest’ultimo in modo che l’oggetto entri nell’apertura 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 è

3

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 un’apertura 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

4

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

5

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

6

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): L’integrato LM7805 è un regolatore di tensione CIRCUITO DEL REGOLATORE DI che fornisce in uscita una tensione stabilizzata TENSIONE di 5 [V] a patto che l’ingresso 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, l’alimentazione 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 dell’integrato LM7805 si rimanda al link: www.datasheetcatalog.org/datasheet/fairchild/LM7805.pdf

7

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. L’operazionale e l’ingresso 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 all’integrato 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 dell’utilizzo 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 l’utilizzo 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 l’ingresso non invertente, quindi la 0 DIDASCALIA: tensione della batteria è sopra al valore VBAT: tensione prelevata dalla batteria. desiderato di 11 [V], l’uscita 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 l’ingresso invertente e l’uscita 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

,

8

+= 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

9

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 25°C è 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 all’ingresso invertente del comparatore sarà maggiore di quella presente all’ingresso 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 all’emettitore 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 all’ingresso 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 l’intero 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 all’emettitore, 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 l’oscuramento, 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. L’emettitore del fototransistor è stato invece collegato all’ingresso invertente dell’operazionale così che l’uscita dell’operazionale sia a livello alto solo in presenza di una tensione, all’ingresso non invertente, maggiore di quella presente all’ingresso invertente. Questo si riscontra quando la corrente che attraversa il fototransistor generi una tensione ai capi del trimmer, quindi all’ingresso invertente, minore della tensione garantita dal partitore.

Figura 10: trasduttore L14G1

13

Tale partitore è necessario per mantenere, all’ingresso non invertente dell’operazionale, una tensione di riferimento di 2,5 [V] e non 0 [V], come si potrebbe pensare; in quanto l’emettitore del fototransistor avrà sempre una, anche se piccola, corrente. Infine abbiamo collegato un led di alimentazione e un led in uscita dell’operazionale 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

6

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 dell’impulso 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 all’interno 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 dell’attrazione 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 l’asse 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 l’una dall’altra. 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 20°C. 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 :

Un’altra 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 un’accelerazione 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à sull’altro (sempre in situazioni ideali) dunque la coppia di ciascun motore dovrà essere:
2 = 2,74 = 1,37 2 ∗

Per sicurezza, tenendo anche conto dell’approssimazione 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 dell’asse 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 un’altro 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: L’l298 è 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 dell’L298:

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

Sign up to vote on this title
UsefulNot useful