Realizzazione e controllo di un robot mobile differenziale

Transcript

UNIVERSITÀ DEGLI STUDI DI ROMA
TOR VERGATA
FACOLTÀ DI INGEGNERIA
CORSO DI LAUREA IN INGEGNERIA
DELL'AUTOMAZIONE
A.A. 2009/2010
Tesi di Laurea
REALIZZAZIONE E CONTROLLO DI UN ROBOT
MOBILE DIFFERENZIALE
RELATORE
CANDIDATO
Ing. Daniele Carnevale
Moreno Mattia
0121909
...alla mia famiglia
e ad Alessandra.
Indice
Ringraziamenti
1
Introduzione
2
1 Descrizione Hardware
5
1.1
Sensori di sistanza
. . . . . . . . . . . . . . . . . . . . . . . . . . . .
5
1.2
Controller Motori . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7
1.2.1
Led di segnalazione . . . . . . . . . . . . . . . . . . . . . . . .
9
1.2.2
Comandi principali del M.M.B.e.
. . . . . . . . . . . . . . . .
10
1.3
Motori DC . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
12
1.4
Encoder
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
13
1.5
Motorizzazione della torretta . . . . . . . . . . . . . . . . . . . . . . .
14
1.6
Modulo wireless . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
15
1.7
Scheda ARDUINO
. . . . . . . . . . . . . . . . . . . . . . . . . . . .
18
1.7.1
Alimentazione e memorie Arduino . . . . . . . . . . . . . . . .
19
1.7.2
Input e Output
. . . . . . . . . . . . . . . . . . . . . . . . . .
20
Circuito di alimentazione . . . . . . . . . . . . . . . . . . . . . . . . .
21
1.8
2 Gestione dell'hardware
2.1
INDICE
Gestione dei motori . . . . . . . . . . . . . . . . . . . . . . . . . . . .
23
23
I
INDICE
2.2
2.1.1
Errore di compilazione
. . . . . . . . . . . . . . . . . . . . . .
2.1.2
Librerie SoftSerial e NewSoftSerial
. . . . . . . . . . . . . . .
24
2.1.3
Software gestione motori . . . . . . . . . . . . . . . . . . . . .
27
Gestione della comunicazione Arduino Matlab
24
. . . . . . . . . . . . .
28
2.2.1
Trasmissione . . . . . . . . . . . . . . . . . . . . . . . . . . . .
29
2.2.2
Ricezione
. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
30
2.3
Gestione Sensori
. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
33
2.4
Gestione Servomotore . . . . . . . . . . . . . . . . . . . . . . . . . . .
37
3 Applicazioni
3.1
38
Interfaccia graca di comando . . . . . . . . . . . . . . . . . . . . . .
38
3.1.1
Creazione dell'applicazione GUI . . . . . . . . . . . . . . . . .
38
3.1.2
Elaborazione dati . . . . . . . . . . . . . . . . . . . . . . . . .
42
3.2
Modellizzazione del sistema
. . . . . . . . . . . . . . . . . . . . . . .
45
3.3
Controllo distanza
. . . . . . . . . . . . . . . . . . . . . . . . . . . .
46
3.4
Controllo di posizione . . . . . . . . . . . . . . . . . . . . . . . . . . .
49
Conclusioni e sviluppi futuri
54
Appendice
56
Elenco delle gure
79
Bibliograa
80
INDICE
II
Ringraziamenti
Grazie al mio relatore per i consigli, il supporto e la partecipazione che ha dimostrato
nel guidarmi in questo mio lavoro, per la sua esperienza e la sua professionalità messe a
mia disposizione. Un grazie speciale anche ai miei compagni di corso, che dall'inizio di
questi tre anni non hanno mai lesinato su un aiuto o un consiglio. Alla mia famiglia
per aver creduto in me e ad Alessandra che ha avuto il coraggio e la pazienza di
aspettare che i miei sforzi e i miei tentativi si trasformassero in Octagon.
Introduzione
1
Introduzione
Il termine ROBOT deriva dalla parola ceca ROBOTA che signica lavoro pesante o
lavoro forzato ed identica una qualsiasi macchina, di forma più o meno antropomorfa, in grado di sostituire l'uomo nei lavori più pesanti, noiosi, faticosi o pericolosi.
La robotica è la scienza che cerca di sviluppare, appunto, metodologie che permattano ad una macchina (robot) dotata di opportuni sensori ed attuattori di percepire ed
interagire con l'ambiente esterno e di svolgere funzioni che sarebbero, altrimenti, ad
esclusivo appannaggio dell'uomo.
Numerosi sono i robot progettati e realizzati no ad oggi in campo industriale, medico, militare e nell'ambito della ricerca scientica:
quelli controllati in remoto, i
robot autonomi (droni), i robot sequenziali, quelli di formazione ed investigazione e i
microrobot, solo per elencarne alcuni.
Lo scopo del presente lavoro è quello di riprogettare un precedente robot mobile, l'Octagon, per realizzare un gruppo di Robot a basso costo e che siano versatili,
espandibili, ad elevata autonomia e semplici da riprodurre e mantenere. Gli Octagon,
sono stati creati con lo scopo di fornire uno strumento di grande valore didattico e di
rappresentare, in tal modo, un valido canale di comunicazione interdisciplinare.
Gli Octagon sono in grado di comunicare tramite un modulo radio che ha una portata
di 250 metri in linea d'aria ed un consumo molto ridotto. La capacità di comunicazione
è un requisito essenziale per l'implementazione di tutti quegli algoritmi di intelligenza
Introduzione
2
Introduzione
distribuita multi agente, in cui gli agenti comunicano tra di loro per scambiarsi diverse
informazioni (odometriche, stato, ecc..) e coordinarsi.
Il basso costo dell'hardware scelto per la realizzazione dei robot inuisce in modo molto
positivo sulla riproducibilità e li qualica come un'ottima piattaforma per l'immissione sul mercato di agenti mobili che potranno poi essere specializzati per speciche
applicazioni.
L'estrema modularità con cui gli Octagon sono stati progettati li rende totalmente
aperti ad eventuali modiche e, anche se nella loro versione di base dispongono solo
di quattro sensori ad infrarosso, essi sono in grado di utilizzare una vasta gamma di
sensori: pirometrici per il rilevamento di fonti di calore, periferiche per il controllo di
servocomandi, sensori ad ultrasuoni...
Gli Octagon una volta equipaggiati di un sensore di visione, costituiranno un ottimo
strumento di ricerca di elevata complessità, come ad esempio per: la Swarm Robotics,
ossia la robotica degli sciami . Si tratta di una recente disciplina che trae le sue origini dall'osservazione dei comportamenti di alcuni insetti sociali, organizzati intorno
ad un individuo, la regina, i quali manifestano quella che spesso viene denita intelligenza della colonia che dà luogo ad un comportamento collettivo auto-organizzante.
L'aspetto più interessante di questi sistemi sta nell'emergere di un comportamento
globale grazie allo scambio di messaggi tra i singoli individui e senza alcuna azione di
supervisione.
La presente tesi è divisa in tre capitoli.
Nel primo capitolo vengono descritti tutti i moduli ed i componenti elettrici e meccanici utilizzati per la costruzione del robot Octagon; in particolare si esporranno le
caratteristiche meccaniche ed elettriche di ognuno, fornendo indicazioni utili riguardo
al loro funzionamento e alla loro compatibilità con gli altri componenti.
Introduzione
3
Introduzione
Nel secondo capitolo si presenteranno tutti i programmi realizzati per la gestione di
ogni modulo: soprattutto lo sviluppo di applicazioni che consentano ai vari dispositivi di comunicare tra di loro in modo da creare un sistema più complesso, robusto ai
disturbi, in grado di svolgere funzioni complesse come il rilevamento di ostacoli e la
comunicazione con macchine di supervisione anche a distanze elevate.
Nel terzo ed ultimo capitolo si spiegherà, inne, la realizzazione delle tre applicazioni
create per il robot: in particolare verrà descritta la creazione di un'interfaccia graca
che permette ad un qualsiasi utente di interagire e comandare il robot.
Una volta
che il robot sarà dotato di sensori più sosticati e precisi, quali ad esempio sensori
per la visione articiale, si potrà utilizzare l'applicazione per comandare il robot dalla
postazione remota.
Nelle altre due applicazioni sono stati realizzati dei controlli. Nella prima si è cercato,
con ottimi risultati, di migliorare le prestazioni del motore nel caso in cui venga richiesta una posizione angolare dell'albero motore ben precisa: a causa dell'inerzia del
motore, essa di per se da scarsi risultati, ma grazie ad un controllo a ciclo chiuso di
questo sottosistema le prestazioni del robot sono state signicativamente migliorate.
Nella seconda applicazione è stato realizzato un programma che permetta al robot
di seguire un oggetto e di rimanere sempre alla stessa distanza da esso.
Quest'ul-
tima applicazione, nello specico, è utile per realizzare una delle idee originarie, in
cui si era pensato di utilizzare dei robot Octagon per simulare il comportamento di
più autovetture ferme ad un incrocio in cui è presente un semaforo; in questo caso il
controllo decentralizzato su ogni robot dovrebbe essere in grado di coordinare i robot
massimizzando l'attraversamento dell'incrocio.
Introduzione
4
Capitolo 1
Descrizione Hardware
In questo capitolo verrà descritta la struttura hardware del robot;
si spiegherà nel particolare il funzionamento di: controller motori
M.M.B.e, sensori di distanza GP2D12, scheda Arduino, motori DC
a 12V, encoder magnetici, modulo per la comunicazione wireless e
circuito di alimentazione.
1.1 Sensori di sistanza
I robot Octagon sono dotati di sensori di distanza SHARP GP2D12, i quali vengono
montati su un supporto in alluminio solidale all'asse di un servomotore che ne pemette la rotazione; tali sensori permettono di rilevare la distanza degli oggetti che
ne riettono il raggio infrarosso. Per permettere a più robot di operare nello stesso
ambiente vengono scelti sensori ad infrarossi rispetto a sensori ad ultrasuono.
La disposizione iniziale, in cui due sensori erano posti due nella parte anteriore del
robot e due nella parte posteriore, è stata modicata posizionando tutti e quattro i sensori nella parte anteriore del robot, alne di poter realizzare una delle tre applicazioni
che verranno di seguito descritte.
I sensori GP2D12 hanno un range di scansione che varia dai 10 agli 80cm; quando
un oggetto si trova all'interno di tale range, riettendo la luce generata dall'emettitore
5
Cap. 1
1.1
Sensori di sistanza
Figura 1.1: Sensore SHARP GP2D12
a infrarosso, si ha una variazione del segnale analogico (prodotto dal sensore) che è
funzione non lineare della distanza del GP2D12 dall'ostacolo.
Figura 1.2: Package GP2D12
Le caratteristiche principali dei GP2D12 sono le seguenti:
•
Range di scansione 10cm - 80cm
•
Tensione di alimentazione compresa tra 4.5V e 5.5V
•
Consumo medio di corrente di 50mA
6
Cap. 1
•
1.2
Controller Motori
Tempo di risposta medio 39ms
Si riporta di seguito la funzione non lineare che intercorre tra distanza e tensione
generata dal sensore
Figura 1.3: Segnale generato dai sensori GP2D12
La funzione distanza di cui si ha bisogno è l'inversa di quella mostrata in gura
1.3, che è stata invertita e approssimata via software (gura 1.4).
L'approssimazione della curva è stata eettuata in funzione dei punti di massima
variazione del coeciente angolare della stessa.
1.2 Controller Motori
Il motor mind b enhanced (M.M.B.e) è il circuito che si occupa del controllo diretto
dei motori DC a 12V utilizzati per la realizzazione del robot Octagon. I motor mind
comunicano con la scheda Arduino attraverso un'interfaccia seriale TTL del tipo 8N1,
7
Cap. 1
1.2
Controller Motori
Figura 1.4: Relazione distanza tensione
protocollo che prevede 8 bit di dati, un bit di stop e non ha bit di parità . Il motor
mind supporta correnti di alimentazione elevate, è protetto contro i picchi di tensione,
quelli di corrente e contro il surriscaldamento. Le caratteristiche principali del motor
mind sono:
•
Fino a 1.75A di corrente continua (6A di picco);
•
Frequenza PWM di 242Hz o 15.5KHz;
•
Interfaccia seriale TTL a 2.4KBPS o 9.6KBPS di baud rate;
•
Memoria EEPROM accessibile all'utente sia in lettura che in scrittura;
•
Filtro PI per implementare il controllo a ciclo chiuso della velocità;
•
Led verde per la comunicazione visuale delle istruzioni ricevute;
8
Cap. 1
•
1.2
Controller Motori
Led rosso per la comunicazione visuale delle condizioni di errore.
Per controllare i motori il motor mind deve ricevere delle istruzioni di comando da
un'unità master, che in questo caso è la scheda Arduino. Le istruzioni che il motor
mind riceve sono così strutturate:
•
un primo byte di sincronizzazione, il quale rappresenta il numero 85 in base 10
(01010101 in base 2);
•
un secondo byte che indica il comando che il motor mind deve eseguire;
•
da zero a quattro byte separati per indicare i parametri del comando indicato
nel secondo byte.
1.2.1
Led di segnalazione
Il motor mind presenta due led per la comunicazione visuale: un led verde, indicato
nello schema elettrico come D2 ed un led rosso, indicato nello schema elettrico come
D3. Il led verde si attiva nel momento esatto in cui viene alimentato il motor mind e
nel momento in cui esso riceve un'istruzione corretta, assumendo, in tal modo, anche
funzione di test per il controllo delle istruzioni inviate , come ad esempio il baud rate.
Il led rosso viene, invece, attivato al vericarsi di condizioni di errore ben precise:
•
la temperatura sul PCB supera i 175 gradi centigradi;
•
la corrente assorbita supera i 6A;
•
la tensione di alimentazione dei motori scende sotto i 5.6 volt.
Viene disattivato non appena la condizione di errore scompare.
9
Cap. 1
1.2.2
1.2
Controller Motori
Comandi principali del M.M.B.e.
I controller M.M.B.e. sono una versione potenziata rispetto ai precedenti M.M.B. e
presentano un set di comandi più ricco e performante. La gura 1.5 presenta l'elenco
di tutti i comandi che possono essere utilizzati sul M.M.B.e
Figura 1.5: Elenco comandi M.M.B.e.
Come si può vedere ci sono due tipologie di comandi: quelli per operare direttamente
sul motore e quelli per stabilire i parametri interni del M.M.B.e., come ad esempio il
comando per impostare i termini kp e ki del ltro PI.
Nella prima tipologia si trovano ad esempio:
•
il comando SETDC (comando 3) il quale riceve un parametro il cui valore può
variare da 0 a 255; tale valore seleziona il duty cycle del segnale PWM che andrà
a comandare il motore: ad esempio per impostare un duty cycle del 50% basta
impostare il valore 127;
•
il comando SPDCON (comando 4) impone al motore una certa frequenza che può
variare da 0 a 65.535: in questo comando la frequenza viene espressa attraverso
10
Cap. 1
1.2
Controller Motori
due byte e si ottiene attraverso l'espressione:
f requency = F reqHi ∗ 256 + F reqLo
dove FreqHi e FreqLo sono i due byte trasmessi come parametri del comando.
Una volta trasmesso il comando il motor mind controllerà il motore in due modi
diversi a seconda di come è stato impostato il bit STATUS.PIMODE nel registro
STATUS:
•
STATUS.PIMODE = 0.
In questa congurazione denita incremento/decre-
mento, di default nel M.M.B.e, il comando SPDCON seleziona la frequenza
desiderata e la velocità di rotazione del motore è gestita nel seguente modo:
viene incrementata se il tachimetro interno al motor mind segnala una velocità inferiore a quella richiesta; viene decrementata se il tachimetro segnala una
velocità maggiore di quella richiesta.
•
STATUS.PIMODE = 1. In questa congurazione il M.M.B.e. esegue un controllo a ciclo chiuso sulla velocità attraverso un controllore PI, i cui parametri
Kp e Ki sono memorizzati nella memoria EEPROM e possono essere modicati
dall'utente.
Il segnale di errore
∆f = fdesiderata − fattuale
è moltiplicato per un termine proporzionale. La somma degli errori calcolati su
una data nestra temporale è poi moltiplicata per un termine integrale e quest'ultimo valore viene, inne, sommato al precedente ed il risultato riportato in
un registro a 32 bit, di cui verranno impiegati solo gli 8 bit più signicativi. Gli
8 bit così ottenuti verranno utilizzati per la generazione del nuovo duty cycle del
11
Cap. 1
1.3
Motori DC
segnale PWM di controllo dei motori. Se la modalità PI è attiva è raccomandata una frequenza non inferiore a 15 KHz. Il risultato delle operazioni eettuate
sul termine integrale sarà di gran lunga maggiore di 8 bit, pertanto si eettuerà il troncamento delle cifre meno signicative tramite divisione per
2n
con n
= PISCALER (valore indicato come terzo parametro del comando SPDCON).
Scegliendo valori inopportuni, specialmente sul parametro Ki, si ottiene un controllo del motore non adeguato; ne è un esempio pratico la scelta di un valore
di Ki troppo elevato, la quale causa la carica del termine integrale portando
facilmente il motore in saturazione e generando fastidiose sovraelongazioni.
1.3 Motori DC
Il movimento del robot Octagon avviene tramite due motori, i quali hanno i rispettivi
assi di rotazione coincidenti e discordi.
In particolare si è usato un motoriduttore
RHE158-99-100 dotato di soppressore di disturbi VDR sul collettore, cioè di un resistore non lineare che, superata la tensione caratteristica per cui è progettato, abbassa
bruscamente la propria resistenza in modo da attenuare ragguardevolmente il disturbo
(questi dispositivi sono infatti chiamati VDR, acronimo di Voltage Dependent Resistor: resistore variabile con la tensione); è dotato, inoltre, di ingranaggi in metallo e
di un asse posteriore del motore sporgente, per l'applicazione di encoders. La dimensione del riduttore e la cassa metallica consentono una coppia molto elevata.
Sono di seguito riportati il disegno con le dimensioni siche del motoriduttore e la
tabella delle caratteristiche tecniche.
12
Cap. 1
1.4
Encoder
Figura 1.6: Motore DC
Figura 1.7: Caratteristiche Motore DC
1.4 Encoder
I motoriduttori sopra descritti sono dotati di encoder ad eetto hall. Questi sensori di
posizione sono molto stabili termicamente e resistenti alle sollecitazioni meccaniche,
pertanto, nonostante la bassa risoluzione che li caratterizza, sono stati impiegati nel
presente lavoro per l'implementazione del controllo di posizione del robot. Per ogni
giro del motore sono disponibili 3 impulsi, ed essendo il rapporto di riduzione del
motoriduttore pari a 94.37:1 si avranno 283.11 impulsi generati dall'encoder per ogni
giro dell'asse esterno del motoriduttore; la precisione sulla posizione è quindi di 1.27
◦
o 0.75 mm. L'uscita di questi sensori è di tipo open drain con un massimo di 20mA,
ciò signica che per ricevere correttamente un onda quadra dobbiamo inserire una
resistenza di pull-up del valore 10KΩ.
13
Cap. 1
1.5
Motorizzazione della torretta
Figura 1.8: Schema di collegamento degli encoder
1.5 Motorizzazione della torretta
I sensori di posizione sono collocati su un supporto in alluminio solidale all'asse di
rotazione di un servomotore che ne permette l'orientamento. L'attuale posizionamen-
◦
to dei sensori non permette la scansione a 360 delle distanze degli oggetti intorno al
robot; essi, infatti, sono stati posizionati in modo da poter rilevare la distanza di un
oggetto che si muove davanti al robot, con una congurazione utile per due applicazioni che verranno discusse in seguito.
◦
Il servomotore utilizzato può compiere escursioni di 180 con una precisione di
1/4
di
grado. Di seguito è riportato lo schema di collegamento e la tabella delle caratteristiche tecniche del servomotore.
Figura 1.9: Caratteristiche servomotore
14
Cap. 1
1.6
Modulo wireless
Figura 1.10: Servomotore
I servomotori sono controllati mediante un segnale PWM (Pulse width modulation) in cui il valore del duty cycle impone un certo angolo all'asse del servomotore.
Per un controllo ottimale il segnale di controllo deve avere una frequenza di 50HZ.
Come è possibile notare dalla gura 1.11 il periodo dell'onda quadra del segnale di
controllo può variare da 10ms a 25ms. Il duty cycle di tale segnale impone una certa
posizione angolare all'asse della torretta.
Il servomotore montato sul robot sarà controllato dalla scheda Arduino nella quale sono presenti delle librerie che includono le funzioni per il controllo delle uscite
PWM presenti sulla scheda e grazie alle quali si può gestire il servomotore ad alto
livello richiamando una funzione e passando come parametro la posizione angolare
desiderata.
1.6 Modulo wireless
Una delle caratteristiche essenziali del robot Octagon è quella di avere un modulo
radio che permette la comunicazione wireless con un calcolatore no ad una distanza
di circa 250m in linea d'aria. Il modulo radio ER400TRS è un ricetrasmettitore che
trasferisce i dati in modalità half duplex.
L'ER400TRS è un sottosistema a basso consumo costituito da un microcontroller,
un regolatore di tensione ed un trasmettitore FM. Il microcontroller esegue la fun-
15
Cap. 1
1.6
Modulo wireless
Figura 1.11: Segnale di controllo del servomotore
zione di ricetrasmettitore RF, realizza l'interfaccia di trasferimento dati da e verso la
sorgente tramite I/O seriale e solleva, inoltre, il dispositivo host dal carico di lavoro
derivante dall'ottimizzazione del segnale radio, dal rilevamento degli errori, dalla correzione dei dati ricevuti in modo errato e dalla trasmissione dei dati in un formato
adatto al canale. Nel modulo è presente una memoria EEPROM programmabile che
permette di registrare i dati relativi alle varie congurazioni delle modalità operative
del ricetrasmettitore. L'ER400TRS è in grado di rilevare l'intensità del segnale ricevuto in modo da determinare il valore assoluto della distanza del robot dal calcolatore:
16
Cap. 1
1.6
Modulo wireless
Figura 1.12: ER400TRS
Figura 1.13: Diagrmma a blocchi del ER400TRS
per questo genera un segnale analogico la cui ampiezza è inversamente proporzionale
all'energia RF presente nella banda passante del ricevitore. L'intensità di questo segnale può variare da 0 (massimo segnale -50dbm) a 1.2V (minimo segnale -105dbm)
ed ha una pendenza di circa 50db/volt. In gura 1.14 è riportato il segnale analogico
descritto.
La velocità della comunicazione seriale è di 19200 bit/sec. Dei buer interni permettono di accodare 192 byte di dati sia in ingresso che in uscita, in attesa che vengano
trasmessi o utilizzati dall'host.
Purtroppo i moduli EasyRadio non consentono una comunicazione full duplex, in
quanto, operando a coppie sulla stessa frequenza sia in trasmissione che in ricezione,
sono costretti a condividere uno dei canali di livello sico: l'etere. Per quanto detto in
precedenza, non avendo la possibilità di trasmettere e ricevere su frequenze dierenti,
il canale di comunicazione wireless sarà half-duplex. Una ovvia soluzione al problema,
17
Cap. 1
1.7
Scheda ARDUINO
Figura 1.14: Segnale RSSI
sempre utilizzando i moduli ER400TRS, potrebbe essere impiegare due moduli su
ciascun host ed utilizzarli in modalità simplex, ma per contenere i costi e non ridurre
l'autonomia del robot si è scelto di equipaggiare gli Octagon con un solo modulo
EasyRadio e di limitare la dimensione dei frame che transitano nel canale radio.
1.7 Scheda ARDUINO
Tutti i dispositivi elettronici di cui è dotato il robot Octagon sono gestiti dalla scheda
Arduino duemilanove (il nome duemilanove deriva dal fatto che la scheda è in commercio dal 2009). Arduino è dotato di: un microcontrollore ATmega328 a 16MHz;
14 pin digitali che possono essere congurati come ingressi o come uscite digitali e
di queste 14, 6 possono essere utilizzate come uscite PWM; 6 ingressi analogici; un
oscillatore a 16MHz; un collegamento USB; un jack di alimentazione; un header ICSP;
un pulsante di reset.
18
Cap. 1
1.7
Scheda ARDUINO
Figura 1.15: Schema elettrico Arduino
1.7.1
Alimentazione e memorie Arduino
L'Arduino Duemilanove può essere alimentato o attraverso una connessione USB o
tramite un alimentatore esterno; nel secondo caso si può utilizzare per la connessione
una spina da 2.1 millimetri con centro positivo oppure si possono utilizzare i pin GND e
Vin del connettore di alimentazione. L'alimentazione è selezionata automaticamente.
La tensione che riceve la scheda Arduino può variare dai 6 ai 20V, ma si deve tenere
presente che con alimentazioni sotto i 7V il pin di uscita a 5V fornisce meno di cinque
volt e la scheda può essere instabile; se si alimenta con una tensione maggiore di
12V il regolatore potrebbe riscaldarsi e danneggiare la scheda. E' quindi consigliato
l'intervallo 7 - 12 volt. I robot Octagon sono equipaggiati di una batteria a 12V che
19
Cap. 1
1.7
Scheda ARDUINO
fornirà anche l'alimentazione, sempre a 12V, all'Arduino.
Il microcontrollore presente sulla scheda è un ATmega328 che dispone di: una memoria
ash di 32KB per memorizzare il codice (di cui 2KB utilizzati per il bootloader); di
una memoria SRAM di 2KB nella quale vengono memorizzate le variabili utilizzate
a run-time dal programma; una memoria EEPROM da 1KB che può essere letta e
scritta con la libreria EEPROM.
1.7.2
Input e Output
I 14 pin digitali presenti sulla scheda possono essere utilizzati sia come ingressi che
come uscite. Ogni pin può fornire o ricevere un massimo di 40mA ed è dotato di una
resistenza di pull-up di 20-50Ω. Alcuni di questi pin hanno funzioni specializzate:
•
Serial: 0(RX) e 1(tx), utilizzati per ricevere (RX) e trasmettere (TX) dati seriali
TTL;
•
Interrupt esterni:
2 e 3 i quali possono essere congurati per collegare un
interrupt;
•
PWM: 5, 6, 9, 10, 11 che forniscono un'uscita PWM ad 8-bit.
La scheda dispone anche di 6 ingressi analogici, ciascuno dei quali fornisce una risoluzione di 10 bit.
Di default questi ingressi misurano una tensione in ingresso che varia da 0 a 5V,
ma l'estremo superiore può essere modicato inserendo la tensione desiderata nel pin
AREF (Analog Reference). Il pin reset permette di resettare il microcontrollore.
L'arduino supporta anche altri protocolli di comunicazione come il protocollo I2C e
la comunicazione SPI.
20
Cap. 1
1.8
Circuito di alimentazione
1.8 Circuito di alimentazione
Il robot Octagon è dotato di una batteria al piombo che fornisce una tensione di
12V sia all'Arduino che ai due Motor mind.
Tutti gli altri dispositivi:
il modulo
di comunicazione wireless, i sensori di distanza, il servomotore, la parte logica dei
Motor mind e i due encoders sono alimentati con una tensione di 5V. Per ottenere
tale valore sono stati utilizzati due stabilizzatori di tensione LM1117T5.0; in gura
1.16 viene riportato il circuito di alimentazione del robot. Quando tutti i circuiti del
robot sono alimentati si ha un assorbimento massimo di corrente di 800mA che è il
valore massimo di corrente che può assorbire lo stabilizzatore LM1117T, per questo
si è scelto di usarne due invece che uno.
Inne in gura 1.17 si riporta lo schema
Figura 1.16: Circuito di alimentazione
elettrico completo del robot Octagon.
21
Cap. 1
1.8
Circuito di alimentazione
Figura 1.17: Schema elettrico robot Octagon
22
Capitolo 2
Gestione dell'hardware
In questo capitolo verranno descritti i software creati per la gestione
dell'hardware e le problematiche riscontrate nelle varie applicazioni
2.1 Gestione dei motori
Nei robot Octagon ogni motore è gestito da un controller M.M.B.e; i controller comunicano e vengono a loro volta gestiti dalla scheda Arduino. Possiamo immaginare il
motor mind come l'unità slave e l'Arduino come l'unità master.
In gura 2.1 viene mostrato lo schema a blocchi della gestione dei motori: il motor
Figura 2.1: Schema a blocchi per la gestione dei motori
mind riceve dal motore il segnale generato dall'encoder magnetico ed invia allo stesso
i segnali di alimentazione; la scheda Arduino comunica con il controller seguendo il
protocollo di comunicazione Seriale TTL 8-N-1, con il quale vengono utilizzati 10-bit
per spedire un singolo byte, di cui 8 sono bit di dati, uno è un bit di start ed un altro
23
Cap. 2
2.1
Gestione dei motori
è un bit di stop. I motor mind possono ricevere e trasferire dati ad una velocità di
2400 o 9600 bit/s.
Per ridurre i ritardi si è impostata la comunicazione con un baud/rate di 9600bit/s.
2.1.1
Errore di compilazione
In fase di programmazione della scheda Arduino non può essere però utilizzato lo schema mostrato in gura 2.1; non si riesce, infatti, ad eseguire l'upload del programma
scritto sull' Arduino e viene restituito, nella nestra di comunicazione un errore; la
scheda Arduino è collegata al pc attraverso la porta USB ed al motor mind attraverso
i pin di comunicazione seriale: dato che sulla scheda c'è un unico circuito integrato
che gestisce la comunicazione seriale e la porta USB, quando si è in questa fase si crea
qualche anomalia elettrica e l'upload non viene eseguito correttamente.
Le possibili soluzioni per risolvere questo problema sono due:
•
utilizzare un accoppiatore ottico per trasferire i dati dall'Arduino al M.M.B.e;
•
inserire un deviatore sul pin RX dell'Arduino.
Entrambe le soluzioni permettono di scollegare elettricamente l'Arduino dal controller
motori.
2.1.2
Librerie SoftSerial e NewSoftSerial
La scheda Arduino presenta, di default, un pin per la ricezione ed un pin la trasmissione; si è dovuta, quindi, trovare una soluzione anchè la scheda potesse comunicare
in modo seriale con tre diversi dispositivi del robot: i due motor mind ed il modulo
radio. Il problema è stato risolto collegando il modulo radio alla porta seriale di default dell'Arduino (pin 0 RX, pin 1 TX) ed i due motor mind ad altri 4 pin digitali
24
Cap. 2
2.1
Gestione dei motori
gestiti in modo che svolgano il ruolo di porte seriali.
In un primo momento si è pensato di creare delle funzioni che replicassero quelle della
gestione della porta seriale su gli altri pin, realizzando dei programmi che utilizzassero le funzioni generiche
digitalRead
e
digitalWrite
per leggere e scrivere byte
trasmessi e ricevuti ad una velocità pressata. Questa soluzione è stata poi abbandonata: infatti, dopo la creazione della prima funzione che replica la
Serial.print
per trasmettere byte dall'Arduino verso i motor mind, si è immediatamente riscontrato un rallentamento del programma, e ci si è resi conto che tali funzioni sarebbero
state poco essibili e non avrebbero potuto emulare tutte quelle che possono essere
utilizzate per la gestione della porta seriale. Inoltre per funzioni di ricezione, da parte
dell'Arduino, ci sarebbero stati notevoli problemi a livello di sincronizzazione.
La casa costruttrice della Scheda Arduino fornisce un insieme di librerie che possono essere implementate nel codice per l'esecuzione di funzioni non standard; una di
queste è la libreria SoftSerial che permette, appunto, di utilizzare semplici pin digitali
come porte seriali. In questa libreria ci sono, però, delle limitazioni:
•
velicità di lavoro no a 9600 bit/sec;
•
funzione Serial.Available() non disponibile;
•
perdita dei dati che arrivano quando la funzione Serial.Read() non è stata
invocata.
Esistono, però, anche altre librerie che non sono fornite dalla casa costruttrice di
Arduino, ma che possono essere comunque utilizzate: ne troviamo alcune sul sito di
ARDUINIANA, in particolare la libreria NewSoftSerial che è quella che verrà implementata nel codice di gestione dei motori per risolvere i problemi di cui sopra. Rispetto alla precedente SoftSerial, che esegue la tecnica del polling in fase di ricezione
25
Cap. 2
2.1
Gestione dei motori
con un considerevole rallentamento nell'esecuzione del programma, la NewSoftSerial
introduce notevoli miglioramenti:
1. implementa uno schema di buering circolare per rendere più eciente l'elaborazione RX;
2. può essere utilizzata su tutti i pin digitali dell'Arduino;
3. si possono denire simultaneamente più NewSoftwareSerial;
4. presenta una gamma di velocità più ampia;
5. fornisce un ag di overow() per rilevare il l'overow del buer;
6. supporta i processori ad 8 MHz e 20MHz;
7. supporta il software signal inversion.
Nell'uso di questa libreria si deve tener presente un importante accorgimento:
la
funzione mySerial.available non può essere utilizzata come condizione logica all'interno
di cicli while, perché questo causerebbe una ricezione errata dei dati.
Figura 2.2: Schema collegamento Arduino motor mind
26
Cap. 2
2.1.3
2.1
Gestione dei motori
Software gestione motori
Per completare la descrizione della gestione dei motori si riportano di seguito le
principale parti del codice scritto.
#include
#define
#define
#define
#define
1
<N e w S o f t S e r i a l . h>
2
3
rxPin_a 4
4
txPin_a 5
5
rxPin_b 7
6
txPin_b 8
7
8 N e w S o f t S e r i a l mySerial_a =
9 N e w S o f t S e r i a l mySerial_b =
N e w S o f t S e r i a l ( rxPin_a , txPin_a ) ;
N e w S o f t S e r i a l ( rxPin_b , txPin_b ) ;
In questa parte di codice viene inclusa la libreria NewSoftSerial, vengono deniti i pin
digitali da utilizzare per la comunicazione e le due comunicazioni una per ogni motor
mind.
void
1
setup ()
2 {
3 // d e f i n e p i n modes f o r tx , rx , l e d p i n s :
4
pinMode ( rxPin_a , INPUT) ;
5
pinMode ( txPin_a , OUTPUT) ;
6
pinMode ( rxPin_b , INPUT) ;
7
pinMode ( txPin_b , OUTPUT) ;
8
9
// s e t t h e d a t a r a t e f o r t h e S o f t w a r e S e r i a l p o r t
10
S e r i a l . begin (9600) ;
11
mySerial_a . b e g i n ( 9 6 0 0 ) ;
12
mySerial_b . b e g i n ( 9 6 0 0 ) ;
13 }
Nella funzione di setup della scheda Arduino vengono specializzati i pin per la trasmissione e per la ricezione e viene impostato il baud rate della comunicazione.
1
2
3
4
5
6
7
8
9
mySerial_b . p r i n t ( 8 5 ,BYTE) ;
mySerial_b . p r i n t ( 3 ,BYTE) ;
mySerial_b . p r i n t ( 1 5 0 ,BYTE) ;
delay (10) ;
( i =0; i <3; i ++)
{ b u f f e r 2 [ i ] = mySerial_b . r e a d ( ) ; }
for
27
Cap. 2
2.2
In questa porzione di codice si fa un esempio di scrittura e lettura di dati sul
Motor mind. Le prime tre righe di codice eseguono il comando SETDC sul M.M.B.e;
la prima è la trasmissione del byte 85, che, come detto in precedenza, è il byte di
sincronizzazione, vengono poi trasmessi il byte 3 che identica il comando da eseguire
ed inne il byte 150 che è il parametro dell'istruzione richiesta ovvero la velocità.
Nella seconda porzione di codice si ha una lettura di alcuni parametri del motor mind,
in particolare dei valori che assumono le variabili Pterm, Iterm, Piscalar. Come sopra
troviamo la trasmissione del byte 85 e del byte 15 che indica, invece, la richiesta di
trasmissione da parte del motor mind verso l'Arduino; viene inserito un ritardo di
10ms e con un ciclo for vengono memorizzati i valori nel vettore buer.
2.2 Gestione della comunicazione Arduino Matlab
In questo paragrafo viene descritta la realizzazione della comunicazione wireless tra
l'Arduino ed il calcolatore, per la quale si è scelto di utilizzare il software Matlab. La
gura 2.3 mostra lo schema a blocchi della comunicazione. Il modulo ER400TRS e
l'antenna che sono connessi al calcolatore sono racchiusi in un unico modulo mentre, il
modulo e l'antenna che sono connessi alla scheda Arduino sono sicamente separati.
I moduli per la comunicazione wireless ER400TRS rendono trasparente la comunicazione gestendo autonomamente i segnali da trasmettere e ricevere ed è per questo
motivo che nei codici Matlab e Arduino si utilizzaranno le comuni funzioni di lettura
e scrittura su porta seriale; la velocità di comunicaizone è di 19200bit/sec.
Per gestire la comunicazione con una porta seriale su Matlab viene denito un oggetto
di tipo porta seriale; l'oggetto contiene tutti i parametri della comunicazione: il Baudrate, la porta seriale con cui comunicare, il numero di bit da cui è composto un dato
ed il numero di bit di stop.
28
Cap. 2
2.2
Figura 2.3: Schema a blocchi comunicazione wireless
1
2
3
4
5
6
7
s = s e r i a l ( 'COM6 ' ) ;
s e t ( s , ' BaudRate ' , 1 9 2 0 0 ) ;
s e t ( s , ' DataBits ' , 8 ) ;
s e t ( s , ' StopBits ' , 1) ;
fopen ( s )
. . . . . comunicazione . . . . .
fclose (s)
Prima di iniziare la comunicazione è necessario connettersi alla porta con il comando
fopen(s);
2.2.1
terminata la comunicazione ci si disconnette con il comando
fclose(s).
Trasmissione
Per trasferire dati dall'Arduino a Matlab si esegue la seguente istruzione :
1 Serial . println ( alfa ) ;
dove alfa è un numero intero.
Nel caso in cui si vogliano trasmettere più valori, ad esempio gli elementi di un vettore,
si possono scrivere più
Serial.println() consecutive (oppure un'unica Serial.println()
all'interno di un ciclo for); è essenziale usare sempre l'istruzione
Serial.println(),
perché in fase di ricezione per distinguere la ne di un valore e l'inizio del successivo
si legge il carattere
\n
che con l'istruzione
Serial.print()
normale non viene tra-
29
Cap. 2
2.2
smesso.
Per trasmettere dati da Matlab verso Arduino si usa l'istruzione
1 f w r i t e ( s , vector , ' uint16 ' ) ;
dove la variabile vector è un vettore di interi senza segno a 16 bit di dimensione
arbitraria.
In questo caso, a dierenza del precedente, per trasmettere più valori
si usa un'unica istruzione che ha come argomento un vettore; come si può vedere
dall'istruzione, gli elementi del vettore vengono trasmessi come interi senza segno a
16
16-bit e si possono, quindi, spedire valori che vanno da 0 a 65536 (2 ).
2.2.2
Ricezione
Descriviamo di seguito il funzionamento dei codici che realizzano le due funzioni di
ricezione, i quali, a dierenza delle precedenti applicazioni, sono più complessi perchè
devono rilevare autonomamente il numero di dati che vengono ricevuti.
Entrambe le funzioni restituiscono i dati organizzati in un vettore, in modo da semplicarne poi l'utilizzo.
•
Ricezione dei dati provenienti dalla scheda Arduino.
La funzione Matlab che permette di ricevere dati da una porta seriale è la
segeunte:
1 f r e a d ( s , x , ' char ' )
in cui il parametro
x
è un valore intero che identica il numero di caratteri da
ricevere. Questa funzione restituisce un vettore i cui elementi sono le cifre dei
valori trasmessi intervallate dai caratteri di separazione (\n).
Si supponga ora di eseguire il seguente codice su Arduino:
30
Cap. 2
2.2
1 S e r i a l . println (2478) ;
2 S e r i a l . println (365) ;
in Matlab si riceve il vettore :
A = [50, 52, 55, 56, −35, −39, 51, 54, 53, −35, −39]
Apparentemente sembra che ci sia un errore, in realtà i singoli caratteri ricevuti
sono espressi nella codica ASCII; è, quindi, suciente convertire il vettore
ottenendone un secondo contenente i valori corretti:
B = [2, 4, 7, 8, −35, −39, 3, 6, 5, −35, 39]
come possiamo notare i caratteri corrispondenti a
\n
(-35, -39) rimangono co-
stanti.
La successiva operazione consiste nel costruire un vettore i cui elementi siano i dati trasmessi dalla scheda Arduino, nell'esempio il vettore [2478,365];
operazione realizzata attraverso il seguente codice:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
A = f r e a d ( s , 1 0 , ' char ' ) ;
B = A− ' 0 ' ;
NxM = s i z e ( v a l o r i ) ;
n = NxM( 1 ) ;
numero_valori = 0 ;
i =1:n
( v a l o r i ( i )==−35)
numero_valori=numero_valori +1;
end
end
numero_cifre ( numero_valori ) = 0 ;
p o s i z i o n e _ s l a s h ( numero_valori ) = 0 ;
j =1;
i =1;
( i <n )
(B( i ) == − 35)
posizione_slash ( j ) = i ;
j=j +1;
end
( j >numero_valori )
for
if
while
if
if
31
Cap. 2
2.2
21
i=n ;
22
end
23
( j<=numero_valori && (B( i ) ~= − 35) && (B( i ) ~= − 38) )
24
numero_cifre ( j ) = numero_cifre ( j ) +1;
25
end
26
i=i +1;
27 end
28 b u f f e r ( numero_valori ) = 0 ;
29
w=1: numero_valori ;
30
q=1: numero_cifre (w)
31
b u f f e r (w) = b u f f e r (w)+B( q +p o s i z i o n e _ s l a s h (w)
32
− numero_cifre (w) −1) ∗ 10^( numero_cifre (w)−q ) ;
33
end
34 end
if
for
for
Nel codice vengono deniti due vettori :
numero_cifre
e
posizione_slash;
gli elementi del primo indicano il numero di cifre dei singoli valori da ricevere:
nell'esempio precedente si ha
e
365
numero_cifre uguale a [4, 3] perchè i numeri 2478
sono composti rispettivemente da quattro e tre cifre.
Gli elementi del secondo, invece, indicano in quale posizione del vettore compare
il valore -35: nell'esempio si ha
posizione_slash
uguale a
[5, 10].
Combinando poi le informazioni contenute nei vettori descritti si ottiene il
vettore desiderato.
•
Ricezione dei dati proveninti da Matlab.
Come detto in precedenza i dati che vengono trasmessi da Matlab verso Arduino
sono a 16-bit; ne consegue che per trasmettere un valore verranno spediti due
byte e in ricezione si avranno, quindi, due valori ricevuti per ogni singolo valore
spedito da Matlab. Se il dato da trasmettere è minore di 256 uno dei due byte
ricevuti sarà nullo perchè è suciente un solo byte per esprimere numeri da
0 a 255; se, invece, il dato è maggiore di 256 verranno ricevuti due byte che
rappresentano il quoziente ed il resto della divisione tra interi
valore/256
La funzione seguente realizza la ricezione dei dati su Arduino.
32
Cap. 2
2.3
Gestione Sensori
if
1
( S e r i a l . a v a i l a b l e ( ) >0)
2 {
3
delay (10) ;
4
buffer [ Serial . available () ] ;
5
i =0;
6
( Serial . available () )
7
{
8
a = S e r i a l . read ( ) ;
9
S e r i a l . p r i n t l n ( a ,DEC) ;
10
buffer [ i ] = a ;
11
i ++;
12
}
13 }
int
while
La funzione
Serial.available()
restituisce il numero di dati presenti nel buer di
ricezione; se nel buer ci sono dati si verica la condizione logica del primo
if
e si
può eseguire il resto della funzione. All'interno del costrutto c'è un ritardo di 10ms
perché si deve attendere che tutti i dati vengano memorizzati nel buer e che quindi
la funzione
Serial.available()
restituisca il valore corretto. Viene denito un vet-
tore di grandezza opportuna e con un ciclo
while
vengono memorizzati nel vettore
i dati trasmessi dal software Matlab. A dierenza della comunicazione con i motor
mind, ora la funzione
di un costrutto
Serial.available()
si può utilizzare come condizione logica
while, questo perchè si stanno utilizzando le funzioni standard per la
gestione della porta seriale su Arduino e non funzioni della libreria NewSoftSerial.
2.3 Gestione Sensori
I sensori GP2D12 sono collegati agli input analogici della scheda Arduino. La funzione
che legge il valore dei sensori per ricavare la distanza di un oggetto che si trova nel
volume di scansione del robot, viene eseguita ad ogni ciclo della funzione
void loop()
di Arduino. Come detto nel paragrafo 1.1 il segnale generato dai sensori è funzione
non lineare della distanza a cui si trova un ostacolo; questa curva caratteristica è stata
33
Cap. 2
2.3
Gestione Sensori
approssimata a tratti via software.
Viene ora analizzato il codice che permette di determinare la distanza tra il robot ed
un eventuale ostacolo:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
int sensorPin_1 = 1 ;
int sensorValue_1 = 0 ;
double t e n s i o n e _ 1 = 0 ;
double d i s t a n z a _ o s t a c o l o _ 1 =
int d i s t a n z a 1 [ ] = { 0 , 0 , 0 , 0 ,
int media =0;
void s e t u p ( ) {}
void l o o p ( ) {
0;
0};
sensorValue_1 = analogRead ( sensorPin_1 ) ;
t e n s i o n e _ 1 = sensorValue_1 ∗ 5 / 1 0 2 3 . 0 ;
( tensione_1 < 0 . 4 )
{ d i s t a n z a _ o s t a c o l o _ 1 = 98 ; }
( 0 . 4 <= t e n s i o n e _ 1 && t e n s i o n e _ 1 < 0 . 5 9 )
{ d i s t a n z a _ o s t a c o l o _ 1 = − 150 ∗ t e n s i o n e _ 1 + 140 ; }
( 0 . 5 9 <= t e n s i o n e _ 1 && t e n s i o n e _ 1 < 0 . 8 1 )
{ d i s t a n z a _ o s t a c o l o _ 1 = −72∗ t e n s i o n e _ 1 + 96 ; }
{ d i s t a n z a _ o s t a c o l o _ 1 = − 36.7 ∗ t e n s i o n e _ 1 + 66 ; }
( 1 . 6 3 <= t e n s i o n e _ 1 && t e n s i o n e _ 1 < 2 . 6 )
{ d i s t a n z a _ o s t a c o l o _ 1 = −8∗ t e n s i o n e _ 1 + 2 8 . 5 ; }
( 2 . 6 <= t e n s i o n e _ 1 )
distanza1 [4]= distanza1 [ 3 ] ;
distanza1 [0]= distanza_ostacolo_1 ;
(
( ( ( d i s t a n z a 1 [0] − d i s t a n z a 1 [ 1 ] ) >10) | | ( ( d i s t a n z a 1 [0] − d i s t a n z a 1 [ 1 ] ) <−10) ) &&
( ( ( d i s t a n z a 1 [0] − d i s t a n z a 1 [ 4 ] ) >10) | | ( ( d i s t a n z a 1 [0] − d i s t a n z a 1 [ 4 ] ) <−10) ) )
{ distanza1 [0]= distanza1 [ 1 ] ; }
media=( d i s t a n z a 1 [ 0 ] + d i s t a n z a 1 [ 1 ] + d i s t a n z a 1 [ 3 ] + d i s t a n z a 1 [ 2 ] + d i s t a n z a 1 [ 4 ] )
/5;
42
(
( ( ( d i s t a n z a 2 [0] − d i s t a n z a 2 [ 1 ] ) <5)&&(( d i s t a n z a 2 [0] − d i s t a n z a 2 [ 1 ] )
>−5)) &&((( d i s t a n z a 2 [0] − d i s t a n z a 2 [ 2 ] ) <5)&&(( d i s t a n z a 2 [0] − d i s t a n z a 2 [ 2 ] )
if
if
if
if
if
if
if
if
if
34
Cap. 2
43
44
45
46
47
48
49
50
51
2.3
Gestione Sensori
>−5)) )
{
media=d i s t a n z a 2 [ 0 ] ;
distanza1 [4]= distanza2
}
[4];
[3];
[2];
[1];
[0];
Il codice segue i seguenti passi logici:
•
denizione dell'ingresso a cui si riceve il segnale proveniente dal sensore;
•
lettura del valore di tensione sul pin di ingresso
sensorValue_1 = analogRead(sensorPin_1);
•
conversione in segnali digitali da un convertitore A-D a 10 bit dei segnali in
ingresso agli input analogici dell'Arduino.
Per questo ad una tensione di 5V corrisponde il valore 1024
(210 )e
ad una ten-
sione di 0V corrisponde il valore 0.
Per ottenere, quindi, il valore di tensione corretto, ricevuto sul pin anologico, si
esegue la proporzione:
5 : 1024 = tensione1 : sensorV alue1
•
lettura del valore della tensione ricevuta per ottenere il valore numerico della
distanza (espressa in cm), individuazione della porzione di tratto lineare interessato e successivo calcolo della distanza.
Come si può notare il segnale di tensione ricevuto varia da 0.4V a 2.6V: se si
riceve un valore al di fuori di questo range vengono arbitrariamente assegnati
valori di distanza corrispondenti a 98cm e 99cm.
35
Cap. 2
Nella variabile
2.3
distanza_ostacolo_1
Gestione Sensori
è ora presente il valore della distanza.
Il segnale trasmesso dai sensori è soggetto a rumore, perciò, lasciando il robot fermo
davanti ad un ostacolo è possibile che si misurino valori di distanza che presentano
una certa oscillazione rispetto al valore corretto. Per eliminare tale rumore si calcola
la media tra cinque misure di distanza: la misura attuale e le ultime quattro misure
calcolate; in tal modo viene, quindi, determinato un nuovo valore di distanza ad ogni
ciclo di scansione e considerando i tempi tipici di esecuzione di una istanza si ha
un aggiornamento della variabile
distanza_ostacolo_1
ogni 10ms circa. Dato che i
tempi caratteristici del robot sono, invece, dell'ordine dei secondi la misura attuale e
le quattro precedenti corrispondono tutte alla stessa distanza che c'è tra il robot ed
un ostacolo.
Il valore di distanza misurato ad ogni ciclo può essere, a volte, molto diverso dai
valori misurati in precedenza. Questo delta può derivare da un disturbo sulla misura
o essere conseguenza della rimozione dell'ostacolo da parte di un agente esterno: il
valore deve essere, quindi, ltrato se aetto da disturbo o, al contrario, memorizzato
se rappresentante la misura reale.
Per ottenere la corretta misura vengono creati
due vettori contenenti entrambi i quattro valori precedenti ed il valore attuale: nel
primo la misura attuale viene memorizzata indipendentemente dal suo valore; nel
secondo, invece, la misura attuale viene memorizzata solo se il delta che la separa
dalle precedenti non è maggiore di una quantità predenita.
Unendo le informazioni contenute nei due vettori si riesce, in tal modo, ad ottenere
una misura di distanza corretta in cui tutti i disturbi siano ltrati.
36
Cap. 2
2.4
Gestione Servomotore
2.4 Gestione Servomotore
Nel software di sviluppo programmi per Arduino è presente la libreria servo.h, contenente le funzioni utili alla gestione dei servomotori.
Per utilizzare questa libreria si seguono i seguenti passi:
#include <Servo.h>
•
inclusione della libreria attraverso l'istruzione
•
creazione di un oggetto di tipo servo con l'istruzione
;
Servo myservo;
.
Le funzioni principali di questa libreria sono:
•
myservo.attach(pin): ssa la variabile myservo al pin passato come argomento
della funzione;
•
myservo.write(pos): scrive un valore per il servo controllando l'albero motore di
conseguenza;
•
myservo.read(): legge la posizione angolare corrente dell'asse del servomotore.
37
Capitolo 3
Applicazioni
In questo capitolo verranno descritte le tre applicazioni realizzate
con il robot Octagon: il comando tramite pc, la funzione inseguimento ed il controllo di posizione.
Nella prima si è creata un'interfaccia graca per comandare il moto
del robot in modo che si muova in tutte le direzioni e con velocità
stabilite dall'utente; con la funzione inseguimento il robot segue autonomamente un oggetto in movimento cercando di interporre tra
sè e l'oggetto sempre la stessa distanza; il controllo posizione è fatto sui motori e migliora di molto le prestazioni del robot quando si
richiede una posizione angolare ben precisa.
3.1 Interfaccia graca di comando
L'interfaccia graca di comando è stata la prima applicazione che si è ritenuto opportuno creare; grazie alla trasmissione wireless di istruzioni di vario tipo si rende agevole
comandare il robot da una postazione remota. Essa è stata realizzata utilizzando il
software Matlab ed in particolare l'applicazione GUI (Graphical User Interface).
3.1.1
Creazione dell'applicazione GUI
Per creare un'applicazione GUI si esegue inizialmente l'istruzione
guide
nella com-
mand window Matlab, da cui appare una nestra che permette di creare un nuovo
progetto o di aprirne uno già esistente. Scegliendo di creare un lavoro ex-novo si ini-
38
Cap. 3
Applicazioni
3.1
Interfaccia graca di comando
zierà a lavorare nella nestra mostrata in gura 3.1 Come si può notare, nella parte
Figura 3.1: Finestra di lavoro GUI
sinistra è presente un menù dal quale è possibile selezionare quali componenti inserire
nell'interfaccia graca.
Per il robot Octagon si è scelto di creare l'interfaccia mostrata in gura 3.2 In essa
sono presenti dei pulsanti per attivare il robot e delle caselle di testo per inserire i
parametri dei comandi quali: velocità di movimento; gradi che deve compiere il robot
e verso di rotazione; distanza da percorrere e numero di gradi di rotazione della torretta su cui sono montati i sensori.
Si spiega ora l'eetto di ogni comando sul robot:
•
AVANTI: comanda al robot di muoversi in avanti alla velocità specicata nella
casella velocità; con questo comando il robot si ferma solo nel caso in cui arrivi
ad una distanza di 15cm da un oggetto o venga premuto il comando ARRESTO;
39
Cap. 3
Applicazioni
3.1
Figura 3.2: Interfaccia robot octagon
•
INDIETRO: il robot si muove indietro secondo le stesse modalità del comando
AVANTI;
•
ARRESTO: ferma il robot qualunque movimento esso stia compiendo;
•
RUOTA ROBOT: comanda al robot di ruotare, sul proprio asse verticale, di
una quantità indicata nella casella gradi e nel verso indicato nella casella verso
rotazione. Se nell'editor c'è il numero 0 la rotazione avviene in senso antiorario,
se c'è 1 avviene la rotazione avviene in senso orario;
•
PERCORRI DISTANZA: fa muovere il robot no a compiere la distanza denita
nella casella [ cm ]. Se nella casella verso distanza c'è il numero 1 la distanza è
percorsa in avanti, se c'è 0 la distanza è percorsa all'indietro;
•
RUOTA SENSORI: ruota la torretta di una quantità espressa nella casella
[gradi] ;
•
ACQUISISCI DISTANZA: viene visualizzata la distanza dell'oggetto che si trova
davanti al robot.
40
Cap. 3
Applicazioni
3.1
Creata l'interfaccia in cui sono presenti tutti i componenti, Matlab genera automaticamente un M-le che contiene una funzione per ogni elemento presente nell'interfaccia:
nel momento in cui si interagisce con uno di essi (ad esempio viene premuto uno dei
pulsanti), viene richiamata la funzione associata al componente stesso. Per comandare il robot Octagon è suciente utilizzare solo due elementi: pulsanti ed editor di testo.
Le funzioni associate agli editor di testo eseguono un controllo sui valori inseriti,
in particolare vericano che essi appartengano al range ammesso. Si riporta di seguito
il codice associato all'editor Velocità.
1
2
3
4
5
6
7
8
9
10
f u n c t i o n v e l o c i t a _ C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
v a l o r e = s t r 2 d o u b l e ( g e t ( hObject , ' S t r i n g ' ) ) ;
v a l o r e _ v e l o c i t=v a l o r e +1;
( v a l o r e _ v e l o c i t >255)
valore_velocit = 255;
end
( isempty ( v a l o r e _ v e l o c i t ) )
s e t ( hObject , ' S t r i n g ' , ' 0 ' )
end
g u i d a t a ( hObject , h a n d l e s ) ;
if
if
Quando si preme un pulsante la funzione associata ad esso esegue le seguenti
operazioni:
•
lettura delle stringhe presenti negli editor di testo interessati;
•
conversione della stringa in un numero double;
•
creazione del vettore da trasmettere al robot;
•
trasmissione del vettore.
Viene di seguito riportata la funzione associata al pulsante PERCORRI DISTANZA.
1 f u n c t i o n p e r c o r r i _ d i s t a n z a _ C a l l b a c k ( hObject , ev ent da ta , h a n d l e s )
2 display percorri_ditanza ;
3 a = get ( handles . distanza , ' String ' ) ;
41
Cap. 3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
Applicazioni
3.1
a = str2double (a) ;
b = get ( handles . velocita , ' String ' ) ;
b = str2double (b) ;
c = get ( handles . verso_distanza , ' String ' ) ;
c = str2double ( c ) ;
display (a) ;
vettore = [1 4 0 0 0 ] ;
vettore (3) = a ;
vettore (4) = b ;
vettore (5) = c ;
s = s e r i a l ( 'COM6 ' ) ;
s e t ( s , ' BaudRate ' , 1 9 2 0 0 ) ;
s e t ( s , ' DataBits ' , 8 ) ;
s e t ( s , ' StopBits ' , 1) ;
fopen ( s ) ;
fw r i te ( s , vettore , ' uint16 ' ) ;
fclose (s) ;
L'ordine in cui vengono memorizzati i dati all'interno del vettore, che verrà poi trasmesso al robot, denisce il protocollo di trasmissione da seguire. Il primo elemento
identica il robot a cui trasferire i dati (il protocollo prevede la trasmissione dei dati
a più robot presenti nello stesso ambiente); il secondo elemento identica il comando che il robot dovrà eseguire. Gli elementi successivi sono, invece, i parametri del
comando dato: se, ad esempio, si è richiesto di percorrere una distanza pressata il
terzo elemento sarà la distanza da percorrere, il quarto la velocità da utilizzare ed il
quinto il verso.
3.1.2
Elaborazione dati
Quando il robot deve essere comandato tramite l'interfaccia creata, viene caricato su
Arduino il programma che si occupa della gestione dei dati ricevuti e dell'azionamento
opportuno dei motori. Questo programma esegue ciclicamente tre passi:
1. lettura del valore di distanza fornito dai sensori;
2. lettura del buer di ricezione del modulo radio;
42
Cap. 3
Applicazioni
3.1
3. interpretazione del vettore ricevuto e azionamento dei motori tramite opportune
funzioni.
Le funzioni che vengono richiamate permottono di far eseguire al robot il comando
richiesto da un utente che sta utilizzando l'interfaccia.
•
Avanti: è la funzione che viene richiamata quando un utente preme il pulsante
AVANTI. Essa controlla il primo bit del registro di stato di entrambi i motor
mind per controllare il verso di rotazione attuale di ogni singolo motore ed
esegue un cambiamento nel caso un cui la congurazione corrente non produca
un movimento in avanti del robot. Terminato il controllo aziona i motori alla
velocità desiderata;
void
int
int
1
avanti (
velocita )
2 {
3
temp=0;
4
5
6
delay (10) ;
7
temp=mySerial_b . r e a d ( ) ;
8
( temp==40)
9
{
10
11
12
}
13
mySerial_a . p r i n t ( 8 5 ,BYTE) ;
14
mySerial_a . p r i n t ( 5 ,BYTE) ;
15
delay (10) ;
16
temp=mySerial_a . r e a d ( ) ;
17
( temp==41)
18
{
19
20
21
}
22
23
24
mySerial_a . p r i n t ( v e l o c i t a ,BYTE) ;
25
26
27
mySerial_b . p r i n t ( v e l o c i t a +7,BYTE) ;
if
if
43
Cap. 3
•
Applicazioni
3.1
INDIETRO: il robot si muove seguendo la stessa logica adoperata nel comando
Avanti, ma in questo caso retrocedendo;
•
ARRESTO: arresta i motori
void
1
arresto ()
2 {
3
4
5
6
7 }
•
PERCORRI DISTANZA: calcola il numero di passi encoder necessari anchè
la rotazione del motore produca lo spostamento lineare richiesto. Come nella
funzione Avanti si ha il controllo del primo bit del registro di stato.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
void p e r c o r r i _ d i s t a n z a ( int
int a n g o l o _ r a d i a n t i ;
int a n g o l o _ g r a d i ;
{
distanza ,
int
velocita ,
int
verso )
angolo_radianti = ( distanza ∗0.01) /0.027;
angolo_gradi = angolo_radianti ∗180/3.141457;
gradiDes = angolo_gradi ;
i m p u l s i =0;
numero_giri =0;
alfa ;
beta ;
k;
k = (283.0/360.0) ;
i m p u l s i=k ∗ g r a d i D e s ;
( i m p u l s i >256)
{
numero_giri=i m p u l s i / 2 5 6 ;
a l f a=numero_giri ;
b e t a=i m p u l s i − (256 ∗ a ) ;
}
double
double
int
int
int
double
if
else
{
a l f a =0;
b e t a=i m p u l s i ;
}
temp=0;
mySerial_a . p r i n t ( a l f a ,BYTE) ;
mySerial_a . p r i n t ( beta ,BYTE) ;
int
44
Cap. 3
31
32
33
34
35
36
37
Applicazioni
3.2
Modellizzazione del sistema
mySerial_a . p r i n t l n ( v e l o c i t a ,BYTE) ;
mySerial_b . p r i n t ( a l f a ,BYTE) ;
mySerial_b . p r i n t ( beta ,BYTE) ;
mySerial_b . p r i n t l n ( v e l o c i t a +7,BYTE) ;
}
Nel codice non è riportata la porzione in cui si esegue il controllo del registro di
stato che è uguale a quello della funzione precedente.
•
RUOTA ROBOT: come Percorri Distanza, ma, in questo caso, il primo bit del
registro di stato dei due motor mind viene impostato in modo da produrre una
rotazione del robot.
•
RUOTA SENSORI: controlla la posizione angolare dell'asse del servomotore.
void
1
ruota_sensori (
2 {
3 myservo . w r i t e ( pos ) ;
4 }
int
pos )
3.2 Modellizzazione del sistema
Prima di spiegare le due applicazioni di controllo è opportuno ricavare un modello matematico del robot. Il robot Octagon può essere rappresentato dallo schema mostrato
in gura 3.3. Il motor mind riceve in ingresso lo scalare 'u' (che può variare tra 0 e
Figura 3.3: Modello matematico del sistema
45
Cap. 3
Applicazioni
3.3
Controllo distanza
255) e fornisce un valore di tensione direttamente proporzionale a tale variabile. Tutta
la parte elettrica del motore è approssimata con un blocco di guadagno di Ke che ricevendo la tensione v in entrata fornisce una corrente i. Nei motori a corrente continua
si può approssimare la coppia come una grandezza direttamente proporzionale alla
corrente e quindi attraverso un altro blocco Km si ottiene la coppia motrice fornita
dal motore. A tale valore è poi sottratto quello della coppia di carico opportunamente
diviso per il rapporto di riduzione n ottenendo la coppia utile. L'ultimo blocco, quello
che da la posizione angolare dell'albero motore, è ricavato dalla seguente equazione:
Cu = J · α + d · ω
dove
α
e
ω
(3.2.1)
sono rispettivamente l'accalerazione e la velocità angolare, J il momento
di inerzia e d il coeciente di attrito. La funzione di trasferimento di questo sistema
non è stata ricavata ma viene presa da una precedente tesi ed è la seguente:
F DT =
−0.058122(z − 2.958)
(z − 1)(z − 0.6671)(z 2 + 0.548z + 0.2061)
(3.2.2)
Su questa FDT sono fatte le due successive applicazioni.
3.3 Controllo distanza
La seconda applicazione realizzata per il robot Octagon è stata la funzione di insegui-
mento. In questo caso il robot interagisce autonomamente con l'ambiente esterno: nel
momento in cui i sensori non rilevano nessun ostacolo il robot rimane fermo, se invece
riscontrano la presenza di un ostacolo il robot si posiziona ad una distanza di 30cm
da esso, indierentemente dal fatto che la distanza iniziale sia minore o maggiore di
30cm.
Per realizzare tale applicazione è stato creato sia un sistema di controllo a ciclo chiuso
di tipo proporzionale.
46
Cap. 3
Applicazioni
3.3
Controllo distanza
Figura 3.4: Schema a blocchi funzione inseguimento
Figura 3.5: Posizionamento sensori
Ad ogni ciclo il programma controlla il valore di distanza fornito dal sensore numero
1 e denisce la variabile errore come la dierenza tra il valore letto dal sensore numero
uno e la costante 30:
errore = distanza1 − 30
la variabile errore è poi moltiplicata per una costante ed il risultato trasmesso alla
funzione del motor mind che controlla la velocità del motoriduttore. Queste appena
descritte sono le operazioni principali del programma, ma vengono eseguite anche altre
istruzioni che stabiliscono, ad esempio, l'avanzamento o la retrocessione del robot, o
rilevano se il valore di velocità esce dal range ammesso dal comando del motor mind.
SETDC.
1 e r r o r e = media − 30;
2 v e l o c i t a=e r r o r e ∗ 1 5 ;
3
( ( media==0)&&check1 )
4 {
if
47
Cap. 3
Applicazioni
3.3
Controllo distanza
5
6
7
8
9
check1=f a l s e ;
10 }
11
( media >0)
12 {
13
( ( ( e r r o r e >10) | | ( e r r o r e <−10) ) )
14
{
15
check=t r u e ;
16
}
17
( check )
18 {
19
check1=t r u e ;
20
( e r r o r e <0)
21
{
22
v e l o c i t a=v e l o c i t a ∗ ( − 1) ;
23
}
24
( ( e r r o r e >0)&&(temp==40)&&(temp_a==41) ) // s e l ' e r r o r e è p o s i t i v o
muovi a v a n t i
25
{
26
27
28
29
30
temp =41;
31
temp_a=40;
32
}
33
( ( e r r o r e <0)&&(temp==41)&&(temp_a=40) ) // s e l ' e r r o r e è n e g a t i v o muovi
indietro
34
{
35
36
37
38
39
temp =40;
40
temp_a=41;
41 }
42
( v e l o c i t a >240)
43
{ v e l o c i t a =240;}
44
( v e l o c i t a <80)
45
{ v e l o c i t a =80;}
46
( ( e r r o r e <8)&&(e r r o r e >−8))
47
{
48
v e l o c i t a =0;
49
check=f a l s e ;
50
51
52
53
if
if
if
if
if
if
if
if
if
48
Cap. 3
54
55
56
57
58
59
60
61
62 }
Applicazioni
3.4
Controllo di posizione
}
mySerial_b . p r i n t ( v e l o c i t a ,BYTE) ;
}
3.4 Controllo di posizione
Il motor mind ha la possibilità di eseguire una funzione denominata COUNT, che
riceve come parametri due interi x ed y, dove: il parametro x identica il numero di
passi encoder che il motore ruotando dovrà contare ed il parametro y è la velocità a
cui il robot deve muoversi. Questa funzione fornisce al motore una tensione, tale da
produrre una rotazione dell'albero alla velocità stabilita: la tensione viene fornita no
a quando il numero di passi encoder attuali è minore del numero di passi encoder richiesti; non appena questi due valori coincidono la funzione toglie tensione al motore.
Si deve tener conto del fatto che il motor mind a volte perde la misura di qualche
passo encoder e che l'inerzia del motore produce una rotazione ulteriore dell'albero
motore per il quale esso non si arresta nel momento esatto in cui viene tolta tensione;
ciò causa un errore di posizionamento, errore che cresce all'aumentare del numero di
passi encoder richiesti. Ne consegue che per grandi distanze l'errore accumulato avrà
un valore troppo elevato e non potrà essere trascurato.
L'ultima applicazione è propriamente un controllo di posizione che migliora signicativamente le prestazioni del robot.
In questo caso il segnale digitale prodotto
dall'encoder è trasmesso sia al M.M.B.e. sia alla scheda Arduino, che come detto in
precedenza è dotata di due pin per la ricezione di Interrupt esterni. Il segnale prodotto
dall'encoder ad eetto hall è un segnale digitale il cui valore logico 1 corrisponde ad
49
Cap. 3
Applicazioni
3.4
una tensione di circa 1.2V: per ottenere, quindi, un segnale digitale che possa variare
da 0V a 5V si usa un resistore di pull-up come mostrato in gura 3.6.
Per gestire
Figura 3.6: Schema collegamento encoder-Arduino
questo segnale di interrupt connesso al pin 3 la scheda Arduino utilizza la funzione
attachInterrupt(1,inc,CHANGE);
dove il numero 1 indica l'interrupt del pin 3 (se
ci fosse stato 0 si sarebbe indicato l'interrupt del pin 2), la stringa
inc
è il nome
della funzione che viene richiamata ogni qualvolta si verica la condizione impostata
come terzo parametro nella funzione
to
CHANGE
ovvero la funzione
inc
attachInterrupt.
è richiamata ogni volta che il segnale di interrupt
esegue una transizione da un livello logico ad un altro.
due modalità
RISING
e
FALLING,
In questo caso si è imposta-
in cui la funzione
inc
Si possono impostare altre
è richiamata solo quando si
verica una transizione del segnale da zero ad uno (RISING) o solo quando si verica
una transizione del segnale da uno a zero (FALLING).
Per realizzare l'applicazione è stato creato un piccolo sistema di controllo proporzionale integrale.
1
2
3
4
5
6
7
8
void
( setup )
{
a t t a c h I n t e r r u p t ( 1 , incremento , FALLING) ;
delay (10) ;
( temp==40)
if
50
Cap. 3
Applicazioni
3.4
Figura 3.7: Schema a blocchi controllo posizione
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
{
temp =41;
}
check=t r u e ;
}
loop ()
{
e r r o r e= i m p u l s i −e n c o d e r ;
v e l o c i t a=e r r o r e ;
( ( e r r o r e >10) | | ( e r r o r e <−10) )
{
check=t r u e ;
}
( check )
{
( e r r o r e <0){
}
void
if
if
if
if ( ( e r r o r e >0)&&(temp==40) )
{
}
temp =41;
if ( ( e r r o r e <0)&&(temp==41) )
{
}
if
{
}
temp =40;
( v e l o c i t a >255)
v e l o c i t a =255;
51
Cap. 3
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
if
{
}
Applicazioni
3.4
( v e l o c i t a <80)
v e l o c i t a =80;
if ( ( e r r o r e <2)&&(e r r o r e >−2))
{
v e l o c i t a =0;
check=f a l s e ;
}
}
delay (100) ;
}
void i n c r e m e n t o ( )
if ( temp==41)
{ e n c o d e r ++;}
if ( temp==40)
{
{ encoder −−;}
}
Nel codice sono presenti una serie di controlli per ottenere il movimento corretto del
robot:
•
il verso di rotazione del motore è determinato dal valore logico assunto dal primo
bit del registro di stato del motor mind, il quale viene letto e modicato nel caso
in cui il robot debba muoversi nel verso opposto;
•
la velocità imposta al robot deve rientrare nel range di ammissibilità della funzione SETDC, la quale ammette solo valori compresi tra 0 e 255; nel caso in cui
l'errore sia molto grande il valore velocità, calcolato come termine proporzionale
rispetto all'errore, esce dal suddetto range ed è saturato al valore massimo ammissibile, ovvero 255. Cosa analoga accade quando l'errore è piccolo: il robot,
infatti, presenta una massa non trascurabile ed è per questo che valori compresi
52
Cap. 3
Applicazioni
3.4
tra 0 e 80 non producono nessuno spostamento.
Nel codice è riportata, inne, anche la funzioneincremento(), che è la funzione richiamata da
attacInterrupt quando si ha il passaggio del segnale digitale, prodotto
dall'encoder, dal livello logico alto al livello logico basso. Purtroppo questa applicazione è stata realizzata per un solo motore, perchè il pin digitale numero 2 (ovvero
l'interrupt 0) ha un assorbimento di corrente troppo elevato e non riesce, quindi, a
rilevare le transizioni presenti nel segnale digitale prodotto dall'encoder.
53
Con il presente lavoro è stata realizzata un'unità robotica mobile di tipo dierenziale
che potrà essere utilizzata per creare un sistema robotico più complesso nel quale uno
o più robot potranno interagire tra di loro, collaborare con un'unità di supervisione e
controllo o compiere simultaneamente entrambe le cose.
Le scelte hardware hanno permesso di realizzare un'unità economica ed espandibile
al tempo stesso, utilizzabile per la simulazione di sistemi multirobot (ad esempio
un incrocio stradale), sorveglianza, mapping o per rilevazioni di vario genere.
In
particolare ogni robot, grazie alla sua modularità, potrà essere ampliato e dotato di
sensori specici per le diverse applicazioni in cui verrà utilizzato.
Le prime modiche che potranno essere applicate al robot sono:
•
inserimento di un fotoaccoppiatore tra l'Arduino e il modulo radio ER400TRS;
•
studio dell'interrupt 0 per collegare entrambi gli encoders dei motori alla scheda
Arduino;
•
trasmissione alla scheda Arduino dei segnali prodotti dai sensori utilizzando il
protocollo di comunicazione
•
I 2C ,
già prevista sul microcontrollore;
realizzazione di una PCB in modo da avere un sistema più robusto a sollecitazioni meccaniche e più stabile elettricamente.
54
Un'ultima modica potrà essere quella di sostituire l'attuale batteria al piombo con
una più leggera che mantenga la stessa carica elettrica, avendo, però, cura di non utilizzare una batteria eccessivamente leggera che, diminuendo troppo il carico, potrebbe
creare fenomeni di slittameto.
55
Appendice
Listato
Programma per il controllo di posizione sul motore
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
// i n c l u d e t h e S o f t w a r e S e r i a l l i b r a r y so you can use i t s f u n c t i o n s :
#include <N e w S o f t S e r i a l . h>
// d e f i n e p i n o f
rxPin_a
txPin_a
rxPin_b
txPin_b
#define
#define
#define
#define
s e r i a l communications
4
5
7
8
// b y t e d i r i c e z i o n e
byte incomingByte = 0 ;
incomingByte1 = 0 ;
int
// s e t up a new s e r i a l p o r t
N e w S o f t S e r i a l mySerial_a = N e w S o f t S e r i a l ( rxPin_a , txPin_a ) ;
N e w S o f t S e r i a l mySerial_b = N e w S o f t S e r i a l ( rxPin_b , txPin_b ) ;
b o o l e a n check ;
double g r a d i D e s =1800;
double i m p u l s i =0;
int a =0;
int a l f a ;
int b e t a ;
int i =0;
int j =0;
int b u f f e r 1 [ 2 ] ;
int v=0;
double k ;
volatile int e n c o d e r =
int e r r o r e =0;
int v e l o c i t a =0;
int temp=0;
void s e t u p ( )
0;
56
Appendice
36 {
37
a t t a c h I n t e r r u p t ( 1 , incremento , FALLING) ;
38 // d e f i n e p i n modes f o r tx , rx , l e d p i n s :
39
40
41
42
43
44
45
S e r i a l . begin (9600) ;
46
mySerial_a . b e g i n ( 9 6 0 0 ) ;
47
mySerial_b . b e g i n ( 9 6 0 0 ) ;
48 // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . c a l c o l o d e i
p a s s i encoder da e s e g u i r e . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
49 k = ( 2 8 3 . 1 1 / 3 6 0 . 0 ) ;
50 i m p u l s i=k ∗ g r a d i D e s ;
51
( i m p u l s i >256)
52
{
53
a=i m p u l s i / 2 5 6 ;
54
a l f a=a ;
55
b e t a=i m p u l s i − (256 ∗ a ) ;
56
}
57
58
{
59
a l f a =0;
60
61
}
62 S e r i a l . p r i n t ( " g r a d i D e s : " ) ;
63 S e r i a l . p r i n t l n ( g r a d i D e s ) ;
64 S e r i a l . p r i n t ( " i m p u l s i da e s e g u i r e : " ) ;
65 S e r i a l . p r i n t l n ( i m p u l s i ) ;
66 S e r i a l . p r i n t ( " p a r t e i n t e r a d e i g i r i e f f e t t u a t i d a l motore : " ) ;
67 S e r i a l . p r i n t l n ( a ) ;
68 S e r i a l . p r i n t ( " a l f a : " ) ;
69 S e r i a l . p r i n t l n ( a l f a ) ;
70 S e r i a l . p r i n t ( " b e t a : " ) ;
71 S e r i a l . p r i n t l n ( b e t a ) ;
72
73 // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . movimentazione
de l robot in avanti
74 mySerial_b . p r i n t ( 8 5 ,BYTE) ;
75 mySerial_b . p r i n t ( 5 ,BYTE) ;
76 d e l a y ( 1 0 ) ;
77 temp=mySerial_b . r e a d ( ) ;
78
( temp==40)
79 {
82 temp =41;
83 }
84 //
if
else
if
57
Appendice
..................................................................................
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
check=t r u e ;
}
111
112
113
114
115
116
117
{
temp =41;
}
118
119
120
121
122
123
124
125
126
127
128
129
130
131
{
temp =40;
}
void
loop ()
{
e r r o r e= i m p u l s i −e n c o d e r ;
v e l o c i t a=e r r o r e ;
( ( e r r o r e >10) | | ( e r r o r e <−10) )
{
check=t r u e ;
}
( check )
{
/∗
mySerial_b . p r i n t ( 8 5 ,BYTE) ; / / . . . . . . . . . . l e g g i i l r e g i s t r o d i s t a t o
delay (10) ;
temp=mySerial_b . read ( ) ;
∗/
if
if
if
( e r r o r e <0) // . . . . . . . . . . . . . . . . . . . . . . . . imponi sempre v e l o c i t à p o s i t i v a
{
}
if ( ( e r r o r e >0)&&(temp==40) ) // . . . . . . . . . . s e
avanti
l ' e r r o r e è p o s i t i v o muovi
if ( ( e r r o r e <0)&&(temp==41) ) // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . s e
n e g a t i v o muovi i n d i e t r o
l ' errore è
if
if
if
( v e l o c i t a >240)
{ v e l o c i t a =240;}
( v e l o c i t a <80)
{ v e l o c i t a =80;}
( ( e r r o r e <2)&&(e r r o r e >−2))
{ v e l o c i t a =0;
check=f a l s e ; }
58
Appendice
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
}
delay (100) ;
S e r i a l . p r i n t l n ( e r r o r e ,DEC) ;
/ ∗ i f ( c h e c k==f a l s e )
{
mySerial_b . p r i n t ( 1 7 5 ,BYTE) ;
delay (1000) ;
} ∗/
}
void i n c r e m e n t o ( )
if ( temp==41)
{ e n c o d e r ++;}
if ( temp==40)
{
{ encoder −−;}
}
Programma per la comunicazione con interfaccia graca
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
#include <Servo . h>
// d e f i n e p i n o f s e r i a l communications
#define rxPin_a 4
#define txPin_a 5
#define rxPin_b 7
#define txPin_b 8
// b y t e d i r i c e z i o n e
byte incomingByte = 0 ;
incomingByte1 = 0 ;
int
// c r e a t e s e r v o o b j e c t t o c o n t r o l a s e r v o
// a maximum o f e i g h t s e r v o o b j e c t s can be c r e a t e d
Servo myservo ;
59
Appendice
23 byte p i n S t a t e = 0 ;
24
25
grandezza_vettore ;
26
i;
27
a;
28
j;
29
30
sensorPin_1 = 1 ;
// s e l e c t t h e i n p u t p i n f o r t h e p o t e n t i o m e t e r
31
sensorPin_2 = 2 ;
32
sensorPin_3 = 3 ;
33
sensorPin_4 = 4 ;
34
35
sensorValue_1 = 0 ; // v a r i a b l e t o s t o r e t h e v a l u e coming from t h e
sensor
36
sensor
37
sensor
38
sensor
39
40
41
tensione_1 = 0 ;
42
tensione_2 = 0 ;
43
tensione_3 = 0 ;
44
tensione_4 = 0 ;
45
46
47
distanza_ostacolo_1 = 0;
48
49
50
51
52
vettore_distanze [ 4 ] ;
53
54
setup ()
55 {
56
// a t t a c h e s t h e s e r v o on p i n 9 t o t h e s e r v o o b j e c t
57
myservo . a t t a c h ( 9 ) ;
58
59
// d e f i n e p i n modes f o r tx , rx , l e d p i n s :
60
61
62
63
64
65
66
S e r i a l . begin (19200) ;
67
mySerial_a . b e g i n ( 9 6 0 0 ) ;
68
mySerial_b . b e g i n ( 9 6 0 0 ) ;
69
int
int
int
int
int
int
int
int
int
int
int
int
double
double
double
double
double
double
double
double
int
void
60
Appendice
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
}
void
{
loop ()
// . . . . . . . . . . . . . . . . . . . . . . . read t h e v a l u e from t h e s e n s o r :
// . . . . . . . . . . . . . . . . . . . . . . . . c o n v e r s i o n e v a l o r e l e t t o d a l s e n s o r e i n
tensione
// . . . . . . . . . . . . . . . . . . . . c o n v e r s i o n e v a l o r e l e t t o d i s t a n z a
( tensione_1 < 0 . 4 )
{ distanza_ostacolo_1 = 0 ;}
if
if
if
if
if
if
if
( 2 . 6 <= t e n s i o n e _ 1 )
//−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
( tensione_2 < 0 . 4 )
if
if
if
if
61
Appendice
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
if
if
if
( 2 . 6 <= t e n s i o n e _ 2 )
//−−−−−−−−−−−−−−−−−−−−−−−−−−−−
( tensione_3 < 0 . 4 )
if
if
if
if
if
if
if
( 2 . 6 <= t e n s i o n e _ 3 )
//−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
( tensione_4 < 0 . 4 )
if
if
if
if
if
if
if
( 2 . 6 <= t e n s i o n e _ 4 )
//−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
62
Appendice
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
/ ∗ v e t t o r e _ d i s t a n z e [0]= d i s t a n z a _ o s t a c o l o _ 1 ;
v e t t o r e _ d i s t a n z e [1]= d i s t a n z a _ o s t a c o l o _ 2 ;
S e r i a l . p r i n t (" d i s t a n z a 1 ") ;
S e r i a l . p r i n t ( distanza_ostacolo_1 , 1 ) ;
S e r i a l . p r i n t (" | | ") ;
S e r i a l . p r i n t ( distanza_ostacolo_1 , 1 ) ;
S e r i a l . p r i n t (" | | ") ;
S e r i a l . p r i n t ( distanza_ostacolo_ ,1 ) ;
S e r i a l . p r i n t (" | | ") ;
S e r i a l . p r i n t l n ( distanza_ostacolo_4 , 1 ) ;
d e l a y ( 1 0 0 0 ) ; ∗/
//−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− f i n e l e t t u r a s e n s o r i
//−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− l e t t u r a d a t i da matlab
( S e r i a l . a v a i l a b l e ( ) >0)
{
delay (10) ;
grandezza_vettore = S e r i a l . a v a i l a b l e () ;
buffer [ Serial . available () ] ;
i =0;
( Serial . available () )
{
// S e r i a l . p r i n t (" d a t i d i s p o n i b i l i : ") ;
// S e r i a l . p r i n t l n ( S e r i a l . a v a i l a b l e ( ) ,DEC) ;
a = S e r i a l . read ( ) ;
// S e r i a l . p r i n t l n ( a ) ;
buffer [ i ] = a ;
i ++;
}
( i =0; i <g r a n d e z z a _ v e t t o r e ; i ++)
{
Serial . print (" buffer ") ;
S e r i a l . p r i n t ( i ,DEC) ;
Serial . print (" : ") ;
S e r i a l . p r i n t l n ( b u f f e r [ i ] ,DEC) ;
}
( b u f f e r [2]==3) //−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− f u n z i o n e a v a n t i
{
avanti ( b u f f e r [4]+ b u f f e r [ 5 ] ∗ 2 5 6 ) ;
}
( b u f f e r [2]==2) //−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− f u n z i o n e
indietro
218
{
219
i n d i e t r o ( b u f f e r [4]+ b u f f e r [ 5 ] ∗ 2 5 6 ) ;
220
}
if
int
while
for
if
if
63
Appendice
221
if
222
223
224
225
226
if
227
228
229
230
if
231
232
233
234
235
236
237
238
239
if
if
( b u f f e r [2]==1) //−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− f u n z i o n e
arresto
{
arresto () ;
// S e r i a l . p r i n t l n (" a r r e s t o ") ;
}
( b u f f e r [2]==4) //−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− f u n z i o n e
percorri distanza
{
percorri_distanza ( b u f f e r [4]+ b u f f e r [ 5] ∗2 56 , b u f f e r [6]+ b u f f e r [ 7]∗2 56 ,
buffer [8]) ;
}
( b u f f e r [2]==5) //−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− f u n z i o n e r u o t a
robot
{
ruota_robot ( b u f f e r [ 4 ] + b u f f e r [ 5 ] ∗ 2 5 6 , b u f f e r [ 8 ] , b u f f e r [ 6 ] + b u f f e r
[7]∗256) ;
}
( b u f f e r [2]==6) //−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− f u n z i o n e r u o t a
sensori
{
ruota_sensori ( buffer [ 4 ] ) ;
}
}
(((10 < d i s t a n z a _ o s t a c o l o _ 1 )&&(d i s t a n z a _ o s t a c o l o _ 1 <15) ) | | ( ( 1 0 <
d i s t a n z a _ o s t a c o l o _ 2 )&&(d i s t a n z a _ o s t a c o l o _ 2 <15) ) | | ( ( 1 0 <
d i s t a n z a _ o s t a c o l o _ 3 )&&(d i s t a n z a _ o s t a c o l o _ 3 <15) ) | | ( ( 1 0 <
d i s t a n z a _ o s t a c o l o _ 4 )&&(d i s t a n z a _ o s t a c o l o _ 4 <15) ) )
{
arresto () ;
}
240
241
242
243 }
244 //−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− f i n e v o i d loop
−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
245
246
247 //−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− f u n z i o n e a v a n t i
−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
248
avanti (
velocita )
249 {
250
temp=0;
251
252
253
delay (10) ;
254
255
( temp==40)
256
{
257
258
259
}
260
void
int
int
if
64
Appendice
261
262
263
delay (10) ;
264
265
( temp==41)
266
{
267
268
269
}
270
271
272
273
274
275
276
277
278
279
280 }
281 //−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− f u n z i o n e i n d i e t r o
−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
282
indietro (
velocita )
283 {
284
285
temp=0;
286
287
288
delay (10) ;
289
290
( temp==41)
291
{
292
293
294
}
295
296
297
298
delay (10) ;
299
300
( temp==40)
301
{
302
303
304
}
305
306
307
308
309
310
if
void
int
int
if
if
65
Appendice
311
312 }
313
314 //−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− f u n z i o n e a r r e s t o
−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
315
arresto ()
316 {
317
// S e r i a l . p r i n t (" e s e g u i a r r e s t o ") ;
318
319
320
321
322 }
323
324 //−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− f u n z i o n e p e r c o r r i
d i s t a n z a −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
325
percorri_distanza (
distanza ,
velocita ,
verso )
326 {
327
distanza1 ;
328 d i s t a n z a 1= d i s t a n z a − d i s t a n z a ∗ ( 0 . 0 8 ) ;
329
angolo_radianti ;
330
angolo_gradi ;
331 a n g o l o _ r a d i a n t i = ( d i s t a n z a 1 ∗ 0 . 0 1 ) / 0 . 0 2 7 ;
332 a n g o l o _ g r a d i = a n g o l o _ r a d i a n t i ∗ 1 8 0 / 3 . 1 4 1 4 5 7 ;
333 // S e r i a l . p r i n t l n ( angolo_gradi ,DEC) ;
334
335
i m p u l s i =0;
336
numero_giri =0;
337
alfa ;
338
beta ;
339
i =0;
340
k;
341
342
343 k = ( 2 8 3 . 0 / 3 6 0 . 0 ) ;
345
( i m p u l s i >256)
346
{
347
348
349
b e t a=i m p u l s i − (256 ∗ a ) ;
350
}
351
352
{
353
a l f a =0;
354
355
}
356
357
temp=0;
358
( v e r s o ==0)
359 {
void
void
int
int
int
int
int
int
double
double
int
int
int
int
double
if
else
int
if
66
Appendice
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
delay (10) ;
( temp==40)
{
}
if
delay (10) ;
( temp==41)
{
}
if
}
if
{
( v e r s o ==1)
delay (10) ;
( temp==41)
{
}
if
delay (10) ;
( temp==40)
{
}
if
}
mySerial_a . p r i n t ( 8 5 ,BYTE) ; // . . . . . . . . . . . . . . . . . . . . . . . . . . e s e g u i comando
COUNT
mySerial_a . p r i n t ( a l f a ,BYTE) ;
mySerial_a . p r i n t ( beta ,BYTE) ;
mySerial_a . p r i n t l n ( v e l o c i t a ,BYTE) ;
67
Appendice
409 mySerial_b . p r i n t ( 8 5 ,BYTE) ; // . . . . . . . . . . . . . . . . . . . . . . . . . . e s e g u i comando
COUNT
411 mySerial_b . p r i n t ( a l f a ,BYTE) ;
412 mySerial_b . p r i n t ( beta ,BYTE) ;
413 mySerial_b . p r i n t l n ( v e l o c i t a +7,BYTE) ;
414 }
415
416 //−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− f u n z i o n e r u o t a r o b o t
−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
417
ruota_robot (
angolo ,
segno ,
velocita )
418 {
419
distanza ;
420
angolo1 ;
421
angolo1_rad ;
422 a n g o l o 1 = a n g o l o ;
423 angolo1_rad = ( 3 . 1 4 1 4 5 7 / 1 8 0 . 0 ) ∗ a n g o l o 1 ;
424
425 d i s t a n z a = angolo1_rad ∗ 0 . 1 1 5 ∗ 1 0 0 ;
426 S e r i a l . p r i n t l n ( angolo1_rad ,DEC) ;
427
distanza1 ;
428 d i s t a n z a 1= d i s t a n z a − d i s t a n z a ∗ ( 0 . 0 8 ) ;
429
angolo_radianti ;
430
angolo_gradi ;
431 a n g o l o _ r a d i a n t i = ( d i s t a n z a 1 ∗ 0 . 0 1 ) / 0 . 0 2 7 ;
432 a n g o l o _ g r a d i = a n g o l o _ r a d i a n t i ∗ 1 8 0 / 3 . 1 4 1 4 5 7 ;
433 S e r i a l . p r i n t l n ( angolo_gradi ,DEC) ;
434
435
i m p u l s i =0;
436
numero_giri =0;
437
alfa ;
438
beta ;
439
i =0;
440
k;
441
442
443 k = ( 2 8 3 . 0 / 3 6 0 . 0 ) ;
445
( i m p u l s i >256)
446
{
447
448
449
b e t a=i m p u l s i − (256 ∗ a ) ;
450
}
451
452
{
453
a l f a =0;
454
455
}
456
457
temp=0;
void
int
double
double
int
int
int
int
int
int
double
double
int
int
int
int
double
if
else
int
68
Appendice
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
if
{
( s e g n o==0)
delay (10) ;
( temp==41)
{
}
if
delay (10) ;
( temp==41)
{
}
if
}
if
{
( s e g n o==1)
delay (10) ;
( temp==40)
{
}
if
delay (10) ;
( temp==40)
{
}
if
}
mySerial_a . p r i n t ( 8 5 ,BYTE) ; // . . . . . . . . . . . . . . . . . . . . . . . . . . e s e g u i comando
COUNT
505 mySerial_a . p r i n t ( 6 ,BYTE) ;
506 mySerial_a . p r i n t ( a l f a ,BYTE) ;
507 mySerial_a . p r i n t ( beta ,BYTE) ;
69
Appendice
508 mySerial_a . p r i n t l n ( v e l o c i t a ,BYTE) ;
509
510 mySerial_b . p r i n t ( 8 5 ,BYTE) ; // . . . . . . . . . . . . . . . . . . . . . . . . . . e s e g u i comando
COUNT
512 mySerial_b . p r i n t ( a l f a ,BYTE) ;
513 mySerial_b . p r i n t ( beta ,BYTE) ;
514 mySerial_b . p r i n t l n ( v e l o c i t a ,BYTE) ;
515 }
516
517 //−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− f u n z i o n e r u o t a
s e n s o r i −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
518
ruota_sensori (
pos )
519 {
520 myservo . w r i t e ( pos ) ;
521 }
void
int
Programma per la funzione di inseguimento
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
// d e f i n e p i n o f
rxPin_a
txPin_a
rxPin_b
txPin_b
#define
#define
#define
#define
s e r i a l communications
4
5
7
8
b o o l e a n check ;
b o o l e a n check1=f a l s e ;
i =0;
j =0;
buffer1 [ 2 ] ;
e r r o r e =0;
v e l o c i t a =0;
temp=0;
temp_a=0;
distanza1 []={0 , 0 , 0 , 0 , 0};
distanza2 []={0 , 0 , 0 , 0 , 0};
int
int
int
int
int
int
int
int
int
int
int
int
int
int
distanza1_3 []={0 , 0 , 0 , 0 , 0 } ;
distanza2_3 []={0 , 0 , 0 , 0 , 0 } ;
distanza1_4 []={0 , 0 , 0 , 0 , 0 } ;
distanza2_4 []={0 , 0 , 0 , 0 , 0 } ;
sensorPin_1 = 1 ;
70
Appendice
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
sensor
sensor
sensor
sensor
double
double
double
double
tensione_1
tensione_2
tensione_3
tensione_4
=
=
=
=
// v a r i a b l e t o s t o r e t h e v a l u e coming from t h e
0;
0;
0;
0;
double d i s t a n z a _ o s t a c o l o _ 1
int media =0;
int media3 =0;
int media4 =0;
void
{
=
=
=
=
0;
0;
0;
0;
setup ()
// d e f i n e p i n modes f o r tx , rx , l e d p i n s :
S e r i a l . begin (9600) ;
mySerial_a . b e g i n ( 9 6 0 0 ) ;
mySerial_b . b e g i n ( 9 6 0 0 ) ;
// . . . . . . . . . . . . . . . . . . . . . . . . movimentazione d e l r o b o t i n a v a n t i
delay (10) ;
temp = mySerial_b . r e a d ( ) ;
( temp==40)
{
if
71
Appendice
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
temp =41;
}
// . . . . . . . . . . . . . . . .
delay (10) ;
temp_a = mySerial_a . r e a d ( ) ;
( temp_a==41)
{
temp_a=40;
}
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
check=t r u e ;
}
if
void
{
loop ()
// . . . . . . . . . . . . . . . . . . . . read t h e v a l u e from t h e s e n s o r :
// . . . . . . . . . . . . . . . . . . . . . . . . . . c o n v e r s i o n e v a l o r e l e t t o d a l s e n s o r e i n
tensione
// . . . . . . . . . . . . . . . c o n v e r s i o n e v a l o r e l e t t o d i s t a n z a
( tensione_1 < 0 . 4 )
112
113
114
115
116
117
118
119
120
121 { d i s t a n z a _ o s t a c o l o _ 1 = − 150 ∗ t e n s i o n e _ 1 + 140 ; }
122
123
124 { d i s t a n z a _ o s t a c o l o _ 1 = −72∗ t e n s i o n e _ 1 + 96 ; }
125
126
127 { d i s t a n z a _ o s t a c o l o _ 1 = − 36.7 ∗ t e n s i o n e _ 1 + 66 ; }
128
129
if
if
if
if
if
72
Appendice
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
if
if
( 2 . 6 <= t e n s i o n e _ 1 )
//−−−−−−−−−−−−−−
( tensione_2 < 0 . 4 )
if
if
if
if
if
if
if
( 2 . 6 <= t e n s i o n e _ 2 )
//−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
( tensione_3 < 0 . 4 )
if
if
if
if
if
if
if
( 2 . 6 <= t e n s i o n e _ 3 )
//−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
( tensione_4 < 0 . 4 )
if
73
Appendice
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
if
if
if
if
if
if
( 2 . 6 <= t e n s i o n e _ 4 )
//−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
( ( ( ( d i s t a n z a 1 [0] − d i s t a n z a 1 [ 1 ] ) >10) | | ( ( d i s t a n z a 1 [0] − d i s t a n z a 1 [ 1 ] )
<−10) ) && ( ( ( d i s t a n z a 1 [0] − d i s t a n z a 1 [ 2 ] ) >10) | | ( ( d i s t a n z a 1 [0] − d i s t a n z a 1
[ 2 ] ) <−10) ) && ( ( ( d i s t a n z a 1 [0] − d i s t a n z a 1 [ 3 ] ) >10) | | ( ( d i s t a n z a 1 [0] −
d i s t a n z a 1 [ 3 ] ) <−10) ) && ( ( ( d i s t a n z a 1 [0] − d i s t a n z a 1 [ 4 ] ) >10) | | ( (
d i s t a n z a 1 [0] − d i s t a n z a 1 [ 4 ] ) <−10) ) )
213 { d i s t a n z a 1 [ 0 ] = d i s t a n z a 1 [ 1 ] ; }
214 media=( d i s t a n z a 1 [ 0 ] + d i s t a n z a 1 [ 1 ] + d i s t a n z a 1 [ 3 ] + d i s t a n z a 1 [ 2 ] + d i s t a n z a 1 [ 4 ] )
/5;
215
(
( ( ( d i s t a n z a 2 [0] − d i s t a n z a 2 [ 1 ] ) <5)&&(( d i s t a n z a 2 [0] − d i s t a n z a 2 [ 1 ] )
>−5)) )
216
217 {
218 media=d i s t a n z a 2 [ 0 ] ;
219 d i s t a n z a 1 [ 4 ] = d i s t a n z a 2 [ 4 ] ;
if
if
74
Appendice
223
224
225
226
227
228
229
230
231
232
233
234
235
236
}
//−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
distanza1_3 [4]= distanza1_3 [ 3 ] ;
distanza1_3 [0]= distanza_ostacolo_3 ;
distanza2_3 [0]= distanza_ostacolo_3 ;
( ( ( ( d i s t a n z a 1 _ 3 [0] − d i s t a n z a 1 _ 3 [ 1 ] ) >10) | | ( ( d i s t a n z a 1 _ 3 [0] − d i s t a n z a 1 _ 3
[ 1 ] ) <−10) ) && ( ( ( d i s t a n z a 1 _ 3 [0] − d i s t a n z a 1 _ 3 [ 2 ] ) >10) | | ( ( d i s t a n z a 1 _ 3
[0] − d i s t a n z a 1 _ 3 [ 2 ] ) <−10) ) && ( ( ( d i s t a n z a 1 _ 3 [0] − d i s t a n z a 1 _ 3 [ 3 ] ) >10)
| | ( ( d i s t a n z a 1 _ 3 [0] − d i s t a n z a 1 _ 3 [ 3 ] ) <−10) ) && ( ( ( d i s t a n z a 1 _ 3 [0] −
d i s t a n z a 1 _ 3 [ 4 ] ) >10) | | ( ( d i s t a n z a 1 _ 3 [0] − d i s t a n z a 1 _ 3 [ 4 ] ) <−10) ) )
237 { d i s t a n z a 1 _ 3 [ 0 ] = d i s t a n z a 1 _ 3 [ 1 ] ; }
238 media3=( d i s t a n z a 1 _ 3 [ 0 ] + d i s t a n z a 1 _ 3 [ 1 ] + d i s t a n z a 1 _ 3 [ 3 ] + d i s t a n z a 1 _ 3 [ 2 ] +
distanza1_3 [ 4 ] ) / 5 ;
239
(
( ( ( d i s t a n z a 2 _ 3 [0] − d i s t a n z a 2 _ 3 [ 1 ] ) <5)&&(( d i s t a n z a 2 _ 3 [0] −
d i s t a n z a 2 _ 3 [ 1 ] ) >−5)) &&((( d i s t a n z a 2 _ 3 [0] − d i s t a n z a 2 _ 3 [ 2 ] ) <5)&&((
d i s t a n z a 2 _ 3 [0] − d i s t a n z a 2 _ 3 [ 2 ] ) >−5)) &&((( d i s t a n z a 2 _ 3 [0] − d i s t a n z a 2 _ 3
[ 3 ] ) <5)&&(( d i s t a n z a 2 _ 3 [0] − d i s t a n z a 2 _ 3 [ 3 ] ) >−5)) &&((( d i s t a n z a 2 _ 3 [0] −
d i s t a n z a 2 _ 3 [ 4 ] ) <5)&&(( d i s t a n z a 2 _ 3 [0] − d i s t a n z a 2 _ 3 [ 4 ] ) >−5)) )
240
241 {
242 media3=d i s t a n z a 2 _ 3 [ 0 ] ;
243 d i s t a n z a 1 _ 3 [ 4 ] = d i s t a n z a 2 _ 3 [ 4 ] ;
248 }
249 //−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
254 d i s t a n z a 1 _ 4 [ 0 ] = d i s t a n z a _ o s t a c o l o _ 4 ;
259 d i s t a n z a 2 _ 4 [ 0 ] = d i s t a n z a _ o s t a c o l o _ 4 ;
260
( ( ( ( d i s t a n z a 1 _ 4 [0] − d i s t a n z a 1 _ 4 [ 1 ] ) >10) | | ( ( d i s t a n z a 1 _ 4 [0] − d i s t a n z a 1 _ 4
[ 1 ] ) <−10) ) && ( ( ( d i s t a n z a 1 _ 4 [0] − d i s t a n z a 1 _ 4 [ 2 ] ) >10) | | ( ( d i s t a n z a 1 _ 4
[0] − d i s t a n z a 1 _ 4 [ 2 ] ) <−10) ) && ( ( ( d i s t a n z a 1 _ 4 [0] − d i s t a n z a 1 _ 4 [ 3 ] ) >10)
| | ( ( d i s t a n z a 1 _ 4 [0] − d i s t a n z a 1 _ 4 [ 3 ] ) <−10) ) && ( ( ( d i s t a n z a 1 _ 4 [0] −
d i s t a n z a 1 _ 4 [ 4 ] ) >10) | | ( ( d i s t a n z a 1 _ 4 [0] − d i s t a n z a 1 _ 4 [ 4 ] ) <−10) ) )
if
if
if
75
Appendice
261 { d i s t a n z a 1 _ 4 [ 0 ] = d i s t a n z a 1 _ 4 [ 1 ] ; }
262 media4=( d i s t a n z a 1 _ 4 [ 0 ] + d i s t a n z a 1 _ 4 [ 1 ] + d i s t a n z a 1 _ 4 [ 3 ] + d i s t a n z a 1 _ 4 [ 2 ] +
distanza1_4 [ 4 ] ) / 5 ;
263
(
( ( ( d i s t a n z a 2 _ 4 [0] − d i s t a n z a 2 _ 4 [ 1 ] ) <5)&&(( d i s t a n z a 2 _ 4 [0] −
d i s t a n z a 2 _ 4 [ 1 ] ) >−5)) &&((( d i s t a n z a 2 _ 4 [0] − d i s t a n z a 2 _ 4 [ 2 ] ) <5)&&((
d i s t a n z a 2 _ 4 [0] − d i s t a n z a 2 _ 4 [ 2 ] ) >−5)) &&((( d i s t a n z a 2 _ 4 [0] − d i s t a n z a 2 _ 4
[ 3 ] ) <5)&&(( d i s t a n z a 2 _ 4 [0] − d i s t a n z a 2 _ 4 [ 3 ] ) >−5)) &&((( d i s t a n z a 2 _ 4 [0] −
d i s t a n z a 2 _ 4 [ 4 ] ) <5)&&(( d i s t a n z a 2 _ 4 [0] − d i s t a n z a 2 _ 4 [ 4 ] ) >−5)) )
264
265 {
266 media4=d i s t a n z a 2 _ 4 [ 0 ] ;
272 }
273 //−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
274 // media=( d i s t a n z a _ o s t a c o l o _ 2+d i s t a n z a _ o s t a c o l o _ 1 ) / 2 ;
275 e r r o r e = media − 20;
276 v e l o c i t a=e r r o r e ∗ 1 5 ;
277
( ( media==0)&&check1 )
278 {
281 mySerial_a . p r i n t ( 8 5 ,BYTE) ;
282 mySerial_a . p r i n t ( 0 ,BYTE) ;
283 check1=f a l s e ;
284 }
285
286
287
( media >0)
288 {
289
( ( ( e r r o r e >10) | | ( e r r o r e <−10) ) )
290 {
291 check=t r u e ;
292 }
293
294
( check )
295 {
296
297 check1=t r u e ;
298
( e r r o r e <0) // . . . . . . . . . . . . . . . . . . . . . . . . imponi sempre v e l o c i t à p o s i t i v a
299 {
300 v e l o c i t a=v e l o c i t a ∗ ( − 1) ;
301 }
302
303
( ( e r r o r e >0)&&(temp==40)&&(temp_a==41) ) // . . . . . . . . . . . . . . . s e l ' e r r o r e è
p o s i t i v o muovi a v a n t i
304 {
if
if
if
if
if
if
if
76
Appendice
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
temp =41;
temp_a=40;
}
if ( ( e r r o r e <0)&&(temp==41)&&(temp_a=40) ) // . . . . . . . . . . . . . . s e
n e g a t i v o muovi i n d i e t r o
l ' errore è
{
temp =40;
temp_a=41;
}
if
if
if
( v e l o c i t a >240)
{ v e l o c i t a =240;}
( v e l o c i t a <80)
{ v e l o c i t a =80;}
( ( e r r o r e <2)&&(e r r o r e >−2))
{
v e l o c i t a =0;
check=f a l s e ;
}
( ( media3 >10)&&(media3 <35) )
{
( v e l o c i t a >150)
{
mySerial_a . p r i n t ( v e l o c i t a − 50 ,BYTE) ;
}
if
if
else
{
mySerial_b . p r i n t ( v e l o c i t a +50 ,BYTE) ;
77
Appendice
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
}
}
else
if ( ( media4 >10)&&(media4 <35) )
{
if ( v e l o c i t a >150)
{
{
mySerial_b . p r i n t ( v e l o c i t a − 50 ,BYTE) ;
}
else
}
{
mySerial_a . p r i n t ( v e l o c i t a +50 ,BYTE) ;
}
else
{
}
}
}
}
delay (100) ;
// S e r i a l . p r i n t l n ( media3 ,DEC) ;
}
78
Elenco delle gure
1.1
Sensore SHARP GP2D12 . . . . . . . . . . . . . . . . . . . . . . . . .
6
1.2
Package GP2D12
. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6
1.3
Segnale generato dai sensori GP2D12 . . . . . . . . . . . . . . . . . .
7
1.4
Relazione distanza tensione
. . . . . . . . . . . . . . . . . . . . . . .
8
1.5
Elenco comandi M.M.B.e.
. . . . . . . . . . . . . . . . . . . . . . . .
10
1.6
Motore DC
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
13
1.7
Caratteristiche Motore DC . . . . . . . . . . . . . . . . . . . . . . . .
13
1.8
Schema di collegamento degli encoder . . . . . . . . . . . . . . . . . .
14
1.9
Caratteristiche servomotore
. . . . . . . . . . . . . . . . . . . . . . .
14
1.10 Servomotore . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
15
1.11 Segnale di controllo del servomotore . . . . . . . . . . . . . . . . . . .
16
1.12 ER400TRS
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
17
1.13 Diagrmma a blocchi del ER400TRS . . . . . . . . . . . . . . . . . . .
17
1.14 Segnale RSSI
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
18
1.15 Schema elettrico Arduino . . . . . . . . . . . . . . . . . . . . . . . . .
19
1.16 Circuito di alimentazione . . . . . . . . . . . . . . . . . . . . . . . . .
21
1.17 Schema elettrico robot Octagon
. . . . . . . . . . . . . . . . . . . . .
22
Schema a blocchi per la gestione dei motori . . . . . . . . . . . . . . .
23
2.1
79
ELENCO DELLE FIGURE
ELENCO DELLE FIGURE
2.2
Schema collegamento Arduino motor mind . . . . . . . . . . . . . . .
26
2.3
Schema a blocchi comunicazione wireless
. . . . . . . . . . . . . . . .
29
3.1
Finestra di lavoro GUI
. . . . . . . . . . . . . . . . . . . . . . . . . .
39
3.2
Interfaccia robot octagon . . . . . . . . . . . . . . . . . . . . . . . . .
40
3.3
Modello matematico del sistema . . . . . . . . . . . . . . . . . . . . .
45
3.4
Schema a blocchi funzione inseguimento
. . . . . . . . . . . . . . . .
47
3.5
Posizionamento sensori . . . . . . . . . . . . . . . . . . . . . . . . . .
47
3.6
Schema collegamento encoder-Arduino
. . . . . . . . . . . . . . . . .
50
3.7
Schema a blocchi controllo posizione
. . . . . . . . . . . . . . . . . .
51
80
Bibliograa
[1] Danilo Luzi, Progettazione e realizzazione di un robot per lo studio del moto di
formazioni',
81

Realizzazione e controllo di un robot mobile differenziale

Transcript

Documenti analoghi

UNITA` di MISURA dell`INFORMATICA

Formati dei file di immagini

Soluzioni documentali integrate per la Direzione del

Robot Chicken 3x03

not afraid to be different

SLE 4442 - Future Tech

Magimix comunicato Patissier_settembre2014

tabella dei caratteri ascii

Bit, byte, word, Kilo, Mega, Giga, Tera, Peta