Realizzazione di un Line Follower 4WD

Transcript

UNIVERSITÀ DEGLI STUDI DI ROMA
TOR VERGATA
FACOLTÀ DI INGEGNERIA
CORSO DI LAUREA IN INGEGNERIA
DELL’AUTOMAZIONE
A.A. 2011/2012
Tesi di Laurea
Realizzazione di un LineFollower 4WD
RELATORE
CANDIDATO
Ing. Daniele Carnevale
Damiano Mattei
“Ogni tecnologia sufficientemente avanzata
è indistinguibile dalla magia.”
Indice
Ringraziamenti
1
Introduzione
2
1 Panoramica generale
4
1.1
Il progetto . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4
1.2
STm32 VL Discovery . . . . . . . . . . . . . . . . . . . . . . . . . . .
5
2 Hardware
9
2.1
Struttura . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
9
2.2
Motori in corrente continua CC . . . . . . . . . . . . . . . . . . . . .
11
2.3
PWM e duty cicle . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
13
2.4
Sensori Sharp e QRD . . . . . . . . . . . . . . . . . . . . . . . . . . .
14
2.5
Modulo wireless Digi International XBee . . . . . . . . . . . . . . . .
16
2.6
Alimentazione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
18
3 Realizzazione e risultati finali
20
3.1
Gestione del motore e del servo . . . . . . . . . . . . . . . . . . . . .
20
3.2
Gestione dei sensori . . . . . . . . . . . . . . . . . . . . . . . . . . . .
23
3.3
Sistema di controllo e software implementato . . . . . . . . . . . . . .
25
3.4
Comunicazione Seriale . . . . . . . . . . . . . . . . . . . . . . . . . .
32
INDICE
I
INDICE
3.5
Risultati ottenuti e conclusioni . . . . . . . . . . . . . . . . . . . . . .
35
Appendice A
Codice creato
37
Appendice B
Immagini del LineFollower
51
Elenco delle figure
54
Bibliografia
55
INDICE
II
Ringraziamenti
Un primo ringraziamento va all’Ing. Daniele Carnevale per la sua disponibilità e per
il suo supporto didattivo. Vorrei inoltre ringraziare i miei 00 maestri
00
più importanti,
i miei genitori, per tutto quello che hanno fatto per me... se oggi sono arrivato fin
qui questo lo devo a voi, e mia sorella Aurora, per aver portato allegria e 00 scompiglio00
nelle mie giornate. Un ringraziamento speciale va a Roberto per il suo prezioso aiuto
didattico e morale.
Vorrei, inoltre, dedicare qualche riga ai miei amici più stretti, i miei compagni di
viaggio. Vorrei ringraziarvi per i momenti di cazzeggio, per gli esami preparati insieme
e per le numerose incazzature prese nei laboratori perchè qualcosa non funzionava mai.
Ho capito che l’università è si tanto studio ed impegno, ma oltre ai libri ci sono persone
speciali con cui condividi ogni momento ed ogni emozione, bella o brutta, e su cui puoi
sempre contare. Un grazie a tutti voi.
Un ringraziamento speciale va a due persone che hanno trascorso con me ogni minuto
di questi anni, il Panda (Matteo) ed il Katana (Antonio), che hanno reso tutto più
facie di quello che realmente era; rifarei tutti questi anni solo per conoscervi di nuovo.
Grazie per tutto amici miei.
Infine vorrei ringraziare una persona per me molto importante, Martina, per esserci
sempre stata nei momenti più difficili dandomi la forza per affrontarli, e per rendermi
felice ogni giorno.
Introduzione
1
Introduzione
Grazie all’evoluzione tecnologica, negli ultimi anni la robotica ha compiuto passi da
gigante, sia nell’ambito delle nuove tecnologie utilizzate per il suo sviluppo, sia nelle
discipline dove essa trova applicazione. Ciò è stato possibile anche grazie alla sua
diffusione nel contesto dell’hobbistica, in cui l’utente medio ha realizzato le proprie
idee contribuendo cosı̀ ad ampliare i settori di applicazione, svincolandola dal suo impiego principale, ovvero l’industria di alto livello. Il termine robotica nasce nel 1920,
quando lo scrittore ceco Karel Capek lo introdusse nel suo racconto R.U.R (Rossum’s
Universal Robot’s) con il significato di lavoro pesante. Successivamente il termine
inglese 00 robotics00 comparve in un racconto di fantascienza dello scrittore Isaac Asimov, da tutti conosciuto, tra le altre cose, come l’inventore delle famose tre leggi della
robotica. Essa è una scienza giovane, legata alla natura che si tenta di ricreare in
maniera automatizzata; mira, inoltre, a sostituire l’uomo in tutte quelle funzioni per
lui inadatte o gravose. Nasce come una branca dell’ingegneria ma successivamente
molte discipline si sono avvicinate ad essa, sia a livello umanistico, come la psicologia,
sia a livello scientifico, come l’elettronica, la fisica e l’informatica. Tra i vari ambiti di
applicazione della robotica, è possibile collocare questo lavoro di tesi, finalizzato alla
realizzazione di un LineFollower 4wd, in tre scenari differenti: la robotica industriale,
la domotica ed il semplice intrattenimento ludico. In relazione al primo contesto,
questi robot vengono utilizzati all’interno delle aziende per compiere spostamenti e/o
Introduzione
2
Introduzione
trasporti di prodotti; in particolare, è preferibile utilizzarli in situazioni in cui la presenza di spazi angusti richiede accuratezza, precisione e velocità. La stessa logica
dei LineFollower, applicata su dispositivi differenti, può comportare un’agevolazione
nello svolgimento delle mansioni domestiche quotidiane; un esempio pratico lo si ha
con la notevole diffusione dei 00 robot spazzini00 , che negli ultimi anni sono sempre più
presenti nelle case dei consumatori. Infine, il progressivo calo dei costi di produzione
delle apparecchiature elettroniche, ha fatto si che queste entrassero nelle abitazioni
non più solamente come prodotti Hi-Tech, riservati ad utenti specializzati nel campo,
ma come una sorta di 00 gioco00 con cui sperimentare e realizzare i propri progetti.
L’elaborato proposto è costituito da un primo capitolo in cui viene esposta una panoramica generale dell’intero progetto ed in cui, in particolare, vengono fornite informazioni sulle principali caratteristiche e funzioni della scheda Stm32 VL Discovery. Nel
capitolo successivo, oltre ad alcuni riferimenti teorici sui segnali PWM, viene presentata la struttura fisica del robot e vengono analizzate le componenti meccaniche ed
elettroniche che lo costituiscono. Il terzo ed ultimo capitolo contiene una descrizione
puntuale delle fasi di realizzazione pratica del LineFollower e dei relativi codici di programmazione, oltre ad un modello di comunicazione seriale utilizzato per l’interazione
PC-Robot.
Introduzione
3
Capitolo 1
Panoramica generale
In questo capitolo viene presentata l’idea generale del progetto e la
scheda STm32 VL Discovery.
1.1
Il progetto
Il LineFollower (letteralmente inseguitore di linea) è un dispositivo automatizzato
che ha l’obiettivo di compiere un percorso seguendo una linea tracciata sul pavimento. Di solito, tale linea può assumere due colorazioni, bianca o nera: la scelta del
colore comporta un diverso settaggio della sensoristica; data la maggiore diffusione
di pavimentazioni chiare, in questo lavoro di tesi si è deciso di utilizzare il colore
nero. Generalmente questi robot sono composti da due gruppi di sensori: il primo è
costituito da quelli dediti al rilevamento della linea; la loro disposizione può assumere
diverse geometrie che influenzeranno la scelta dell’algoritmo di controllo da implementare. Del secondo gruppo fanno parte i sensori che si occupano del rilevamento
degli ostacoli, inseriti per evitare urti e conseguenti danneggiamenti del dispositivo.
Uno dei più importanti e conosciuti LineFollower è presente nel Large Hadron Collider
(LHC), l’acceleratore di particelle situato presso il CERN di Ginevra. In questo caso
il compito del dispositivo è di compiere grandi spostamenti di apparecchiature all’in-
4
Cap. 1 Panoramica generale
§1.2 STm32 VL Discovery
terno del tunnel, ottimizzando i tempi e mantenendo un elevato livello di precisione.
Figura 1.1: Tunnel del LHC al CERN di Ginevra con la linea bianca del LineFollower
1.2
STm32 VL Discovery
La linea STm32VL Discovery include un microcontrollore STM32F100 in un package LQFP, 64 pin multifunzione ed un debugger/programmer ST-LINK on-board, che
consente la programmazione della scheda tramite USB. Essa è provvista di una memoria flash da 128 KB, 8 KB di ram, due LED (verde e blu), un pulsante programmabile
(user ) ed un pulsante di reset (reset). E’ progettata per essere alimentata tramite
porta USB oppure da un’alimentazione esterna di 5V o 3.3V.
L’ambiente di sviluppo è basato su tre piattaforme: IAR Embedded Workbench,
MDK-ARM ed Atollic True Studio for ARM Lite 3.0.0 (basato su Eclipse), che è
stato utilizzato per la realizzazione di questo progetto. Alcuni dei principali vantaggi
della STm32 sono la potenza di calcolo del processore, che può essere portata fino a
24Mhz, il suo costo moderato, ma soprattutto la sua versatilità. Infatti questo dispositivo può essere utilizzato non solo con il suo processore interno ma, attraverso un
5
opportuno settaggio, può compilare processori esterni connessi ad essa, e ciò può essere effettuato tramite appositi jumper. Inoltre presenta specifici header di estensione
per il collegamento I/O con altre schede o dispositivi.
Figura 1.2: STm32 VL Discovery
Le principali periferiche della STm32 sono:
• GPIO : Tale periferica è suddivisa in 4 sub-periferiche (GPIOA, GPIOB, GPIOC
e GPIOD) ognuna delle quali gestisce 16 pin, tranne la sub-periferica GPIOD
che ne amministra solamente 3. Si ricava, quindi, che dei 64 pin che compongono la board, solamente 51 sono utilizzati dalla GPIO, addetta al settaggio dei
singoli pin ed alla gestione degli input/output;
• TIMER : La loro struttura generale è quella di un registro a contatore che
viene aggiornato alla scadenza di un determinato intervallo di tempo, ovvero
il periodo del timer, in relazione alla frequenza interna della STm32, pari a
24Mhz. Una volta raggiunto il valore massimo del contatore, che viene scelto
dall’utente tramite impostazioni, il timer si azzera generando un evento. Sono
6
presenti 16 timer nella scheda, ognuno dei quali possiede 4 canali differenti, che
possono generare un segnale in uscita basato sul confronto tra un valore stabilito
dell’utente ed il valore del timer stesso. I timer inoltre possono essere utilizzati
per svariate funzioni; nel nostro caso sono stati utilizzati come generatori di
segnali PWM, secondo la logica descritta prima;
• DMA : La sua sigla sta per Direct Memory Access, il che significa che le periferiche tramite esso possono accedere direttamente alla memoria del sistema,
senza passare per il processore. Questo metodo comporta un elevato risparmio
di tempo poiché il processore non è più occupato ad interfacciare le periferiche
con la memoria, e quindi può compiere altre operazioni. L’uso del DMA, inoltre,
è risultato fondamentale per la lettura da parte dell’ADC, dato che in questo
è presente un solo registro dati. Quindi, nella lettura di più segnali esterni, il
DMA si occupa di trasferire il dato letto dal registro dati dell’ADC ad una variabile in memoria, specificata nei settaggi. Questo permette di poter accedere
alle singole letture effettuate, cosa che risulterebbe impossibile se non ci fosse il
DMA , a causa della sovrascrittura del registro dati ADC che avviene in circa
15µsec;
• ADC : L’Analog to Digital Converter è un dispositivo che utilizza il campionamento e la quantizzazione per convertire un segnale a tempo continuo e continuo
di ampiezza, cioè un segnale analogico, in un segnale a tempo discreto e discreto in ampiezza, ovvero digitale. Tale periferica, nella catena di acquisizione e
distribuzione dati, è utilizzata per convertire i segnali in ingresso derivanti dai
sensori, in segnali digitali utilizzabili dal microprocessore. L’STm32, come detto
precedentemente, presenta un solo ADC avente 16 canali che, una volta elabo-
7
rato il segnale, deposita il valore nel registro dati, che verrà sovrascritto alla
successiva lettura, dopo circa 15µsec;
• USART : L’acronimo sta per Universal Synchronous-Asynchronous
Receiver-Transmitter e consiste nella conversione di flussi di bit di dati da un
formato parallelo a un formato seriale asincrono/sincrono, e viceversa. Viene
utilizzata per la comunicazione tra i dispositivi ed il PC; nel nostro caso è stata
utilizzata per gestire la STm32 tramite comandi inviati da PC.
8
Capitolo 2
Hardware
In questo capitolo vengono descritte le parti che compongono il robot
e si fa riferimento agli aspetti teorici relativi ai segnali PWM.
2.1
Struttura
Per la realizzazione del dispositivo proposto si è scelto di utilizzare una Tamiya Audi
R8 LMS 24H Nurburgring TT-01E (riproduzione in scala 1/10 della vettura Audi R8)
utilizzata nelle competizioni di modellismo FIA nella categoria GT3. In relazione alle
sue caratteristiche tecniche, essa è dotata di:
• un telaio TT-01E con trasmissione cardanica 4WD;
• un motore CC 540 collocato longitudinalmente;
• un controllo elettronico di velocità Tamiya TEU-104BK
con inversione di marcia;
• 4 sospensioni indipendenti a doppio braccio con molle elicoidali;
• pneumatici 2.000 (52mm) di diametro esterno e 0.9400 (24mm) di larghezza.
9
§2.1 Struttura
Cap. 2 Hardware
Figura 2.1: Controllo elettronico Tamiya TEU-104BK
È stato successivamente aggiunto un servo Hitec HS-422 Deluxe, utilizzato per
direzionare le ruote anteriori, con le seguenti caratteristiche:
• velocità operativa 0.21sec/60◦ a 4.8V;
• coppia d’uscita 3.3kg.cm a 4.8V;
• doppio cuscinetto Dual Oilite.
Figura 2.2: Servo Hitec HS-422 Deluxe
Al di sopra del telaio è stata posizionata una millefori in vetronite, utilizzata per
un duplice scopo: da un lato, per la realizzazione del circuito elettrico; dall’altro,
10
Cap. 2 Hardware
§2.2 Motori in corrente continua CC
per la collocazione di: STM32, modulo wireless di ricezione/trasmissione XBee ed un
potenziometro per la regolazione della velocità del motore.
La sensoristica del LineFollower è suddivisa in due gruppi: il primo contiene tre sensori
QRD1114 utilizzati per la rilevazione del percorso, collocati a loro volta all’interno
di un circuito che si occupa della polarizzazione dei transistor interni ai sensori stessi
e del filtraggio del segnale in uscita; il secondo è composto da tre sensori SHARP
GP2Y0A21YK0F, il cui compito consiste nella rilevazione degli ostacoli durante il
tragitto: dei tre, due sono collocati lateralmente in direzione radiale con le ruote ed
uno in posizione centrale.
Infine, nella parte anteriore del robot, è stato installato un Bumper in poliestere
con l’obiettivo di proteggere i sensori QRD1114 da eventuali urti, mentre sono stati
eliminati gli ammortizzatori per evitare che, su una superficie non omogenea, i sensori
potessero urtare il pavimento o modificare la loro distanza dal suolo, andando ad
influenzare i valori rilevati.
2.2
Motori in corrente continua CC
Come detto precedentemente, per il progetto viene utilizzato un motore in corrente
continua Johnson 540 posizionato longitudinalmente al telaio della macchina, in cui
è incluso un motoriduttore che hai il duplice compito di diminuire le velocità elevate
generate dai motori e di incrementare la coppia in uscita.
I motori in corrente continua presentano una struttura costituita da due corpi principali: la parte mobile, detta rotore, e la parte fissa, chiamata statore. Una corrente
elettrica passa in un avvolgimento di spire che si trova nel rotore, creando un campo
elettromagnetico. Quest’ultimo, a sua volta, è immerso in un altro campo magnetico fisso generato dallo statore, che è formato da due o più coppie polari (calamite,
11
§2.2 Motori in corrente continua CC
Cap. 2 Hardware
elettrocalamite, ecc.). Per induzione elettromagnetica, il rotore inizia a girare poichè
il suo campo magnetico e quello dello statore tendono ad allinearsi. Un interruttore
rotante, detto collettore a spazzole, per ottenere una rotazione continua del motore,
inverte due volte ad ogni ciclo la direzione della corrente elettrica che percorre gli
avvolgimenti del rotore, in modo tale che il suo campo magnetico quello dello statore
non raggiungano mai l’allineamento perfetto. Questo sistema di spazzole fa sı̀ che la
polarità all’interno degli avvolgimenti del rotore sia alternata durante la rotazione:
tecnicamente, quindi, si tratta di un motore a corrente alternata, anche se alimentato
con una corrente continua.
Figura 2.3: Motore CC elementare
La velocità di rotazione del motore dipende da tre fattori principali:
• tensione applicata;
• corrente assorbita dal rotore;
• carico applicato.
mentre la coppia generata è proporzionale alla corrente.
La presenza di avvolgimenti elettrici sul rotore comporta, però, due conseguenze negative: in primo luogo, se il motore è di grossa potenza, si hanno problemi per la
12
§2.3 PWM e duty cicle
Cap. 2 Hardware
dispersione del calore generatosi per effetto Joule; in secondo luogo, gli avvolgimenti
appesantiscono il rotore, aumentando il momento d’inerzia: ciò influenza negativamente la rapidità e la precisione del motore, rendendo il controllo di quest’ultimo più
complesso.
2.3
PWM e duty cicle
Un PWM , dall’acronimo inglese Pulse Width Modulation, è un segnale che sfrutta la
modulazione digitale di larghezza di impulso. Questo tipo di modulazione permette di
ottenere una tensione media variabile dipendente dal rapporto tra la durata dell’impulso positivo, TON , e di quello totale, T. Tale rapporto in percentuale viene definito
duty cicle (D).
1
= TON + TOF F
f
(2.3.1)
TON
TON
100 =
100
T
TON + TOF F
(2.3.2)
T =
D=
In particolare un segnale PWM è caratterizzato da tre fattori: una frequenza costante
f, un duty cicle variabile e due valori di ampiezza, che di solito assumono il valore di
0V e +5V.
La Figura 2.4 mostra diversi valori di duty cicle. Come è possibile osservare, un duty
cicle dello 0% equivale ad un segnale con valore basso (0V) per tutta la durata del
periodo T, mentre se il duty cicle è del 100% il segnale assume sempre il valore alto
(+5V). Nel caso del 50% si ottiene un segnale ad onda quadra che assume il valore
alto per metà del periodo T ed il valore basso per la restante metà, ottenendo, quindi,
l’equivalenza tra tempo TON e tempo TOF F . Con l’utilizzo dei PWM è possibile
controllare l’assorbimento di potenza di un carico elettrico, nel nostro caso un motore
CC.
13
§2.4 Sensori Sharp e QRD
Cap. 2 Hardware
Figura 2.4: Esempi di duty cicle
2.4
Sensori Sharp e QRD
Come precedentemente affermato, il dispositivo proposto presenta due classi di sensori, con differenti mansioni. Il primo tipo è costituito da sensori QRD1114, utilizzati
per rilevare il percorso tracciato che il LineFollower dovrà seguire. Tali sensori sono
suddivisi in due parti: la prima è costituita da un diodo LED, emettitore, che ha il
compito di emanare un fascio di raggi infrarossi. Questi, riflessi a terra, saranno poi
ricevuti dalla seconda parte che costituisce il sensore, ovvero un fototransistor che
svolge la funzione di ricevitore.
Figura 2.5: Sensori QRD
14
§2.4 Sensori Sharp e QRD
Cap. 2 Hardware
La distinzione tra percorso e pavimento si basa sul diverso valore di riflessione delle
superfici: infatti, superfici chiare hanno un alto grado riflettente, contrariamente a
quelle scure, aventi un grado riflettente più basso. In base al valore rilevato dal ricevitore, in uscita si avrà un determinato voltaggio, che può variare da poche centinaia
di mV per le superfici più chiare, fino ad un massimo di +5V (valore di alimentazione
dei sensori) per le superfici più scure.
Il secondo tipo, invece, è costituito da sensori di tipo SHARP GP2Y0A21YK0F.
Caratteristica principale di questi sensori è la capacità di rilevare oggetti ad una distanza avente un range da 10 cm a 80 cm. La misurazione della distanza è il risultato
della combinazione tra tre diverse unità che compongono il sensore: l’IRED (infrared
emitting diode) che si occupa dell’emissione di raggi infrarossi, il PSD (position sensitive detector ) che rileva i raggi rifratti, ed il circuito di elaborazione del segnale, che
invia in uscita un valore di tensione corrispondente alla distanza rilevata. Il range dell’output varia da pochi mV per oggetti rilevati alla distanza massima, fino a circa +5V
per oggetti rilevati alla distanza minima. La riflettività degli oggetti, la temperatura
dell’ambiente ed il tempo di funzionamento del sensore non incidono sulla percezione
delle distanze, poiché il sensore adotta un metodo di rilevazione triangolare.
Figura 2.6: Sensore SHARP GP2Y0A21YK0F
15
Cap. 2 Hardware
2.5
§2.5 Modulo wireless Digi International XBee
Modulo wireless Digi International XBee
Il modulo XBee è una soluzione a basso costo ed a basso consumo per la creazione
di una rete wireless, di trasmissione e ricezione dati, tra più unità ed il calcolatore
ricevente. Alcuni moduli sono dotati di antenna a filo, altri invece dispongono di
un’antenna integrata in un chip; la dimensione dell’antenna a filo è di circa 27mm,
mentre le dimensioni generali del dispositivo sono 33x24x7, che si presenta con una
doppia fila di 10 pin con passo 2mm.
Sono in commercio attualmente tre diverse versioni dei moduli XBee, tutte operanti
a 2.4Ghz: la Serie 1, la Serie 2 e la Serie Pro con caratteristiche tecniche crescenti. Si
parte dalla Serie 1, che permette di raggiungere una copertura di 30 metri (all’interno
di locali), fino alla Serie Pro che raggiunge circa 90 metri in ambienti chiusi e ben 1600
metri in ambienti aperti. Tutti i moduli hanno a disposizione 12 diversi canali per
garantire la coesistenza di più reti differenti senza interferenze tra le trasmissioni, che
possono essere di tipo: Point-to-Point, Point-to-Multipoint, Peer-to-Peer sia in modo
Unicast che Broadcast, e possono raggiungere una velocità massima di 250Kbps.
La connessione tra XBee e gli utilizzatori avviene tramite adattatori ed è di tipo
seriale, con un BaudRate che varia da 1200baud/s a 115200baud/s. Diversi sono i
vantaggi derivanti dall’utilizzo dei moduli XBee: per prima cosa, questi dispositivi
sono bidirezionali, caratteristica importante poiché la maggior dei sistemi economici
in commercio sono unidirezionali, ovvero il trasmettitore non ha idea se il ricevitore
abbia ricevuto i dati o meno. Un ulteriore vantaggio consiste nell’indirizzamento univoco: ogni XBee, infatti, possiede un numero seriale che permette di poter settare una
comunicazione tra due o più unità ignorando i segnali inviati da altri moduli. Infine,
ogni modulo ha, implementata al suo interno, una logica precostruita contenente tutti
16
Cap. 2 Hardware
§2.5 Modulo wireless Digi International XBee
i controlli tipici di una trasmissione wireless, quali ad esempio l’error checking, ovvero
un buffer integrato che controlla che non siano avvenuti problemi nella ricezione o
nell’invio dei dati, e permette il recupero dei pacchetti corrotti o persi.
Figura 2.7: Moduli XBee Serie 2, con antenna a filo ed a chip.In rosso è evidenziato
il codice identificativo del dispositivo.
Tutti i moduli XBee utilizzano un protocollo ZigBee, ideato appositamente per loro,
che si basa a sua volta sullo standard IEEE 802.15.4. La differenza principale dallo
standard 802.15.4 è che il protocollo ZigBee prevede un collegamento di tipo mesh,
utile quando due dispositivi XBee devono comunicare tra di loro ma la distanza che
li separa è eccessiva: in questo caso altri moduli presenti nella rete, operanti come
router, possono effettuare il 00 rilancio00 del messaggio. Tale 00 rilancio00 avviene in modo
automatico: i nodi intermedi, infatti, 00 capiscono00 l’esigenza e si organizzano affinché
il messaggio possa giungere a destinazione. Inoltre, se viene rimosso un nodo dalla
rete, gli altri moduli si accorgono dell’evento e si riorganizzano, in maniera autonoma,
per ristabilire una rete funzionante.
La rete che utilizza questo protocollo ha bisogno di un modulo settato come coordinatore che ha il compito di attivarla, dopo di che agisce come un normale router.
Tutti gli altri dispositivi possono essere settati come router oppure come end device. I
router, come detto precedentemente, possono essere utilizzati come ripetitori quando
i dispositivi comunicanti sono troppo distanti, mentre gli end device si differenziano
17
Cap. 2 Hardware
§2.6 Alimentazione
per la loro incapacità di rilanciare il segnale.
Il protocollo ZigBee è stato ideato per connessioni che richiedono una banda piuttosto limitata, ma che sono integrate in un grande numero di nodi in grado sia di
ricevere che di inviare pacchetti dati. È un protocollo molto più 00 leggero00 e semplice
da utilizzare rispetto ai protocolli Bluetooth e Wi-Fi: ciò favorisce un basso consumo
energetico dei dispositivi che ne fanno uso.
Per configurare i moduli XBee, la DIGI (ex Maxstream) rilascia un’apposita utility
per PC, la XCTU. Le funzioni di base permettono il settaggio del BaudRate, l’aggiornamento del firmware, le diverse configurazioni sulla comunicazione e semplici test
di comunicazione tra i moduli. La Serie 1 si differenzia dalla Serie 2 e dalla Serie
Pro poiché i dispositivi sono già configurati per comunicare tra loro, e ciò consente
di saltare la fase di configurazione. Inoltre, è importante utilizzare nella rete moduli
appartenenti alla stessa serie, poiché si potrebbero avere problemi di comunicazione
a causa delle diverse caratteristiche tecniche.
In questo lavoro di tesi si è scelto di utilizzare la Serie 2 con antenna a chip per due
motivi principali: da un lato presenta caratteristiche intermedie tra le diverse versioni;
dall’altro, il modulo copre una distanza idonea allo scopo proposto nel progetto.
2.6
Alimentazione
L’alimentazione all’intero sistema è fornita da un pacco batterie XCELL composto da
6 pile NiMh ricaricabili in configurazione L2x3 da 1.2V ciascuna, ognuna delle quali
è in grado di erogare 4.500mAh: disposte in serie forniscono, dunque, una tensione
totale di 7.2V.Il peso complessivo del pacco batterie è di 436gr ed è disposto sul fondo
del telaio del LineFollower.
A questa fonte è collegato il controllo elettronico Tamiya TEU-104BK, che fornisce la
18
§2.6 Alimentazione
Cap. 2 Hardware
potenza necessaria per il funzionamento del motore e l’alimentazione alla ricetrasmittente. Nel progetto proposto, non essendo presente la ricetrasmittente, la tensione
ad essa originariamente indirizzata è stata deviata ad un regolatore lineare di tensione LM1117, in modo da stabilizzarla ai +5V per alimentare la STm32 ed il modulo
XBee. Successivamente, le alimentazioni di tutti i sei sensori del LineFollower sono
state prelevate dal pin +5V presente sulla STm32: questo è stato possibile grazie al
fatto che i sensori assorbono una bassissima quantità di corrente, che non compromette
il funzionamento del microcontrollore.
Figura 2.8: Schema elettrico generale
Figura 2.9: Schema elettrico del LM1117
19
Capitolo 3
Realizzazione e risultati finali
In questo capitolo si fa riferimento alla programmazione del
linefollower ed ai risultati ottenuti.
3.1
Gestione del motore e del servo
Per la gestione dei motori e del servo, si è fatto uso di due periferiche della STm32:
la GPIO, configurando il pin pA1 per il motore ed il pin pA2 per il servo come pin
push-pull aventi il seguente settaggio:
GPIO_Struct.GPIO_Pin=GPIO_Pin_9|GPIO_Pin_1|GPIO_Pin_2; //Pin utilizzati
GPIO_Struct.GPIO_Mode=GPIO_Mode_AF_PP; //Pin funzione alternativa Push Pull
GPIO_Struct.GPIO_Speed= GPIO_Speed_50MHz; //Max velocità di output
GPIO_Init(GPIOA,&GPIO_Struct); //Inizializzo la GPIOA
Successivamente si è passati al settaggio della periferica TIMER per generare i segnali
PWM utilizzati per il pilotaggio del motore e del servo.
La configurazione per il motore si è rilevata abbastanza complessa. Questo perché il
controllo elettronico che si occupa della gestione dell’alimentazione del motore e della
gestione del suo PWM, richiede una taratura iniziale attraverso il radiocomando. È
20
Cap. 3 Realizzazione e risultati finali
§3.1 Gestione del motore e del servo
presente, infatti, un pulsante che va premuto seguendo una determinata procedura di
settaggio presente sui datasheet, che si articola nel seguente modo:
• prima pressione, il led lampeggia una volta: viene richiesto il segnale di posizione
neutrale;
• seconda pressione, il led lampeggia ad intermittenza: viene richiesto il segnale
di massima accelerazione;
• terza pressione, il led lampeggia ad intermittenza maggiore: viene richiesto il
segnale di minima accelerazione.
Come detto precedentemente, tale configurazione richiede la presenza del radiocomando, non in dotazione con modellino utilizzato per la costruzione del LineFollower.
Per rimediare a tale mancanza, si è ricorsi all’utilizzo di un oscilloscopio e di un Arduino Uno per generare una grande quantità di segnali PWM, individuando quelli
accettati dal controllo elettronico che consentivano di progredire nelle fasi elencate
precedentemente: in caso contrario veniva emesso un beep per segnalare un PWM
errato.
Si è giunti, quindi, ad attribuire a ciascuna fase i seguenti valori di duty cicle:
• posizione neutrale: duty cicle di 1.4 ms;
• massima accelerazione: duty cicle di 1.6ms;
• minima accelerazione: duty cicle di 0.9ms.
Tali valori sono stati poi convertiti per generare segnali PWM tramite la periferica
TIMER, rispettivamente in :
• TIM2->CCR2=1680;
21
§3.1 Gestione del motore e del servo
• TIM2->CCR2=1920;
• TIM2->CCR2=1080.
Per la simulazione del radiocomando è stato utilizzato un potenziometro, mediante la
seguente riga di codice:
TIM2->CCR2=0.205*LettureADC[6]+1080;
per poterlo poi utilizzare per la variazione di velocità del LineFollower; il valore del
coefficiente angolare e dell’intercetta sono stati ricavati attraverso un semplice calcolo
matematico.
Per quanto riguarda il servo, sono stati utilizzati i datasheet per identificare i tempi
di duty cicle relativi alla posizione centrale e alle curvature verso destra e sinistra. I
valori individuati sono:
• posizione centrale: 0.9 ms;
• curvatura a destra: 0.5ms;
• curvatura a sinistra: 1.3ms.
I parametri convertiti per il settaggio della periferica TIMER sono:
• TIM2->CCR3=1300;
• TIM2->CCR3=850;
• TIM2->CCR3=1950.
22
3.2
§3.2 Gestione dei sensori
Gestione dei sensori
Per l’elaborazione dei dati provenienti dai sensori, questi ultimi sono stati collegati
alla STm32 attraverso due canali differenti della periferica GPIO. Per i sensori QRD
sono stati riservati i pin pC0, pC1 e pC2 della sub-periferica GPIOC configurati come
input floating:
GPIO_Struct.GPIO_Mode=GPIO_Mode_AIN; //Pin analogici
GPIO_Init(GPIOC,&GPIO_Struct); //Inizializzo la GPIOC
mentre per i sensori SHARP la sub-periferica impiegata é stata la GPIOA, con i pin
pA3, pA4 e pA5, con la medesima configurazione:
GPIO_Struct.GPIO_Mode=GPIO_Mode_AIN; //Pin analogici
GPIO_Init(GPIOA,&GPIO_Struct). //Inizializzo la GPIOA
I dati acquisiti dalle due tipologie di sensori sono stati poi inviati alla periferica
ADC che, discretizzandoli, li ha resi utilizzabili nell’algoritmo di controllo. Si è fatto
poi ricorso alla comunicazione seriale tra STm32 e PC per visionare a schermo i
valori restituiti dai sensori per ogni misurazione effettuata; tale analisi ha permesso
di ricavare:
• per i sensori QRD i valori soglia utili alla distinzione della colorazione nera e
bianca: nel primo caso, il valore restituito risulta superiore a 3800, fino ad arrivare a saturazione con il valore 4095 (questo perché la periferica ADC converte
i dati in ingresso in un valore a 12 bit); nel secondo caso, il valore restituito
risulta invece inferiore a 300;
23
§3.2 Gestione dei sensori
• per i sensori SHARP i valori relativi alla presenza di ostacoli a diverse distanze.
Questi sono stati ricavati eseguendo in laboratorio tre test per ogni sensore,
rispettivamente a 30cm, 50cm ed 80cm. In ciascun test è stata calcolata la
media di venti misurazioni effettuate, che è stata successivamente utilizzata nel
software come valore soglia per la rilevazione di eventuali ostacoli presenti nel
percorso.
Sebbene i sensori QRD siano già forniti di un circuito di filtraggio per la luce solare,
é stato aggiunto un ulteriore circuito elettrico finalizzato sia alla polarizzazione del
fototransistor che alla stabilizzazione ed al filtraggio del segnale restituito in uscita.
Figura 3.1: Circuito di stabilizzazione e polarizzazione del QRD1114
Il filtraggio del segnale è stato realizzato tramite un filtro Passa Basso, RC, con
frequenza di taglio:
Fc =
1
2πRC
(3.2.1)
Analizzando i dati ricevuti non filtrati, é stato possibile valutare la componente armonica del disturbo, pari a circa 1700hz. Si é scelto,quindi, di utilizzare una resistenza
da 10KΩ ed un condensatore da 100µF, in modo tale impostare la frequenza di taglio
160hz, ovvero una decade prima delle frequenze da escludere, in grado di generare errori durante la discretizzazione dell’ADC. La scelta del condensatore non ha introdotto
24
§3.3 Sistema di controllo e software implementato
un’eccessiva componente di ritardo nel circuito, ottenendo un buon compromesso tra
attenuazione dei disturbi e tempi di ritardo.
3.3
Sistema di controllo e software implementato
Sfruttando i valori restituiti dai tre sensori QRD montati nella parte anteriore, il dispositivo deve essere in grado di individuare l’andamento del tracciato e modificare
conseguentemente la propria direzione, per mantenersi centrato sulla linea nera. É
necessario specificare che i sensori distano circa 1cm l’uno dall’altro, e che la linea
usata per comporre il tracciato ha una larghezza fissata a 1.5cm. In questo modo,
quando il LineFollower é centrato sul tracciato disegnato, il solo sensore che rileva la
linea nera é il sensore centrale. Quindi, nel nostro caso, quello che principalmente ci
interessa é individuare la presenza della linea tracciata con un nastro isolante nero
(avente un fattore di riflessione inferiore al 10%) su una pavimentazione chiara (con
fattore di riflessione superiore al 90%).
Come detto precedentemente, i valori restituiti dai sensori durante la distinzione delle
due colorazioni sono da 3800 a 4095 per la colorazione nera, mentre per la distinzione
del bianco é inferiore a 300. Questi valori sono stati successivamente discretizzati
nell’algoritmo, assegnando il valore 1 quando il sensore 00 vede00 una superficie scura,
ed il valore 0 per una superficie chiara. La distinzione cosı̀ effettuata, però, porta un
brusco cambiamento dallo stato 00 nero00 allo stato 00 bianco00 con conseguenti e consistenti oscillazioni durante il moto del LineFollower. Per eliminare tali oscillazioni è
stata introdotta una isteresi, il cui compito è di robustificare la rilevazione dei due
stati.
Il controllo implementato é riassumibile da un automa a stati finiti, dove ogni stato é
00
etichettato00 dai bit ottenuti dai tre sensori, rispettivamente SX (sensore di sinistra),
25
CX (sensore centrale) e DX (sensore di destra).
Figura 3.2: Automa a stati finiti
Il comportamento dell’algoritmo implementato è il seguente:
• se solamente il sensore centrale rileva il colore nero, il dispositivo procede in modo rettilineo (SX.CX.DX=010), con un valore di PWM pari a TIM2->CCR3=1300 ;
• se due sensori adiacenti percepiscono il colore nero (SX.CX.DX=110 o 011) il
dispositivo curva lentamente verso la direzione rilevata (rispettivamente sinistra
o destra), con un PWM pari a TIM2->CCR3=1500 per il primo caso, e pari a
TIM2->CCR3=1150 per il secondo caso;
• se un solo sensore laterale rileva il colore nero (SX.CX.DX=100 o 001) la curvatura risulta pronunciata; viene quindi inviato un PWM pari a TIM2->CCR3=1650
per una curvatura verso sinistra, oppure pari a TIM2->CCR3=1000 per una
curvatura verso destra.
26
Nel caso di una curvatura verso destra, partendo dalla condizione iniziale in cui il
dispositivo è centrato sulla linea nera, la figura 3.3 mostra le tre fasi seguite dal LineFollower; è opportuno sottolineare che, grazie all’elevata velocità di rilevazione del
segnale proveniente dai sensori QRD (circa 1ms), non è possibilile passare direttamente dalla fase 1 (SX.CX.DX=010) alla fase 3 (SX.CX.DX=001), senza rilevare la
fase intermedia (SX.CX.DX=011).
Figura 3.3: Fasi di una curvatura a destra
• 1: nella fase che precede l’ingresso in curva, il dispositivo procede dritto poiché
solamente il sensore centrale rileva la linea (SX.CX.DX=010);
• 2: entrando in curva, sia il sensore centrale che quello laterale destro rilevano il
nastro, pertanto il dispositivo 00 capisce00 di dover affrontare una curva ed inizia
a sterzare lentamente verso destra (SX.CX.DX=011);
• 3: qualora la curva sia particolarmente pronunciata, la modifica precedente della
traiettoria non sarà sufficiente a mantenere il LineFollower centrato sulla linea;
si arriverà ad una situazione in cui solamente il sensore destro restituirà il valore
1 (SX.CX.DX=001): in questo caso l’algoritmo prevede una sterzata più accentuata, in modo da aumentare il raggio di curvatura e permettere al dispositivo
27
di rientrare in pista. Se la sterzata risulta sufficiente, dopo quest’ultima fase il
dispositivo seguirà nuovamente, in ordine, la fase 2 per poi concludere con la
fase 1, tornando alla condizione iniziale.
Esiste una particolare combinazione, (SX.CX.DX= 000), la cui gestione necessita della
conoscenza della fase precedente eseguita dal dispositivo. Per fare ciò è stata introdotta una nuova variabile, s, che assume valore pari ad 1 se il LineFollower si trova in
una fase di curvatura a destra, mentre assume il valore 2 se la curvatura avviene verso
sinistra. In questo modo, analizzando il valore della variabile, l’algoritmo deduce la
fase precedente del dispositivo.
Se la combinazione rilevata dai sensori é SX:CX:DX=000 ed il valore della variabile s é pari ad 1, il dispositivo si trovava nella fase di curvatura massima a destra
(SX:CX:DX=001) ma la sterzata non è risultata sufficiente per rientrare in pista e
ciò ha determinato la fuoriuscita del dispositivo dal percorso. In questa situazione la
combinazione SX.CX.DX=000 diminuisce notevolmente il raggio di curvatura, riportando il Linefollower sul percorso.
Se la combinazione rilevata é, invece, SX.CX.DX=111, i tre sensori si trovano tutti sulla linea nera. Questo potrebbe accadere per diverse motivazioni: presenza di
incroci, una lettura errata dovuta alla presenza di sporcizia, oppure per cause non
considerate, come un fuori controllo del dispositivo. É stato stabilito che in presenza
di questa combinazione il dispositivo non effettui nessuna curvatura e prosegua dritto.
É bene specificare, inoltre, che la fase SX.CX.DX=101 potrebbe verificarsi solo in
presenza di bivi, tuttavia non previsti, insieme agli incroci, nel tracciato del lavoro
proposto. Per questa ragione, tale situazione non viene presa in considerazione all’interno dell’algoritmo.
28
Infine, con l’applicazione e l’implementazione dell’isteresi, è emerso un comportamento
più lineare e fluido del dispositivo rispetto alla sua assenza, attenuando notevolmente
le oscillazioni che si generavano nel passaggio da una fase all’altra.
Figura 3.4: Casi SX.CX.DX=111 e SX.CX.DX=101 non considerati
Per quanto riguarda il software implementato, questo é stato scritto in linguaggio
C utilizzando l’ambiente di sviluppo Atollic True Studio for ARM Lite. Le funzioni
principali che il programma esegue sono le seguenti:
• lettura valori dell’ADC e memorizzazione tramite la periferica DMA;
• controllo rilevazione ostacoli;
• analisi dei dati e pilotaggio del servo in base alla legge di controllo calcolata;
• invio dai dati a Processing, tramite seriale, per la visualizzazione a schermo.
Il primo passo effettuato é stato il settaggio di tutte le periferiche utilizzate nel programma. In primo luogo é stata settata la periferica GPIO per la configurazione di
tutti i pin, utilizzati dai sensori (6 pin), dai timer (3 pin), dalla seriale (2 pin), e dal
potenziometro per la regolazione manuale della velocità del motore (1 pin). Successivamente si é passati al settaggio della periferica ADC. Come detto precedentemente,
a causa della presenza di un solo registro di memorizzazione dati nell’ADC, è stato
29
necessario ricorrere all’uso della periferica DMA, per poter memorizzare, senza perdita dei dati, i valori rilevati dai sei sensori montati sul dispositivo. L’utilizzo del DMA
è stato necessario a causa dell’elevata velocità operativa dell’ADC, che impedisce di
effettuare una copia via software dei valori convertiti. Infine é stata configurata l’intera periferica TIMER. Sono stati utilizzati sostanzialmente due timer diversi: il primo,
il TIM2, é stato configurato per essere utilizzato con motore e servo, per la generazione dei rispettivi segnali PWM; si è fatto ricorso allo stesso timer poiché entrambe
i dispositivi lavorano con una frequenza di 50hz quindi, attraverso i parametri TIMER Struct.TIM Period e TIME Struct.TIM Prescaler, è stato impostato il periodo
secondo la formula:
T empo =
periodo · prescaler
24M
(3.3.1)
Per ottenere, quindi, un periodo di 20ms come quello utilizzato per il TIM2, si
può impostare il parametro TIMER Struct.TIM Period =24000 ed il parametro TIMER Struct.TIM Prescaler =20. Inoltre, per distinguere i due diversi segnali PWM,
al motore é stato assegnato il canale 2 del TIM2, mentre al servo è stato assegnato il
canale 3 del TIM2. Il duty cicle dei due segnali PWM è stato configurato tramite il
parametro TIMEROC Struct.TIM Pulse, che assume un valore diverso nei due canali
utilizzati.
Il secondo timer utilizzato è il TIM3; la funzione di quest’ultimo é quella di essere
usato per l’impostazione del tempo di campionamento, che è stato posto a 10ms, ovvero ad una frequenza di 100hz.
Per assicurarsi che il loop del programma venga eseguito esattamente ogni 10ms, si
è fatto uso degli interrupt. Infatti, impostando specifici parametri all’interno delle
periferiche TIMER, é possibile generare un evento di interrupt ogni volta che il timer arriva alla fine di un periodo. Alla fine di ogni ciclo, uno specifico flag viene
30
aggiornato, indicando il completamento con successo delle operazioni da svolgere. In
relazione all’evento di interrupt, é stata implementata una funzione che indica se tutte
le operazioni richieste dal programma sono state eseguite nel tempo limite di 10ms.
Tale funzione ha il compito di verificare lo stato del flag di completamento: se il flag
risulta aggiornato, allora il loop riprende senza problemi, mentre se il flag non risulta
aggiornato, ciò significa che le operazioni richieste dal programma non sono state eseguite nel tempo prestabilito. Viene quindi segnalato il problema tramite il lampeggio
dei due LED presenti sulla STm32.
È stato, infine, implementata un’ulteriore legge di controllo basata su un controllore
PD. Il comportamento proporzionale del controllore è legato alla relazione:
P = Kp · (3.3.2)
= y − QRB
(3.3.3)
con
dove il valore Kp varia in relazione ai parametri caratteristici del progetto (nel caso
proposto è relativo al valore del PWM del motore), mentre il valore è pari all’errore
commesso tra il valore del segnale derivante dal sensore QRD (y, si è scelto, arbitrariamente, di utilizzare la lettura del sensore QRD destro) ed il valore di identificazione
del 00 bianco00 (QRB=300). In particolare, il valore di viene utilizzato in tutti gli stati dell’automa, ad esclusione dello stato SX.CX.DX=010, ovvero quello relativo alla
posizione centrale del LineFollower.
Per ottenere inoltre l’attenuazione di possibili oscillazioni viene introdotta un’azione
di tipo derivativa che cerca di compensare, in ragione della sua velocità di cambiamento, la variazione del segnale di errore, comportandosi come un fattore di smorzamento.
31
§3.4 Comunicazione Seriale
In formule, si ha:
D = Kd ·
d
dt
(3.3.4)
in cui D è il valore della correzione e Kd è un valore che dipende dal peso che si vuole
dare alla componente derivativa dell’intero controllore. Viene inoltre fissato con il
tempo t il momento attuale e t-1 quello del passo precedente, a cui sono associati i
due errori, t ed t−1 .
Nel codice implementato la componente derivativa è stata realizzata approssimando
notevolmente la funzione di derivazione; ci si è infatti basati solamente sulla differenza
dei due errori commessi nei tempi diversi.
Il regolatore PD a tempo discreto presenta la seguente formula:
C = P + D = Kp · t + Kd ·
(t − t−1 )
Ts
(3.3.5)
ed il codice implementato è il seguente:
int PD(int sensore,float kp,float kd){
if (sensore>3800){
sensore=3800;
}
err=sensore-300;
P=kp*err;
if(kd>0){
D=kd*(err-old_err)/Ts;
old_err=err;
}
else {
D=0;
}
c=P+D;
return(c);
}
3.4
Comunicazione Seriale
Parte essenziale del software realizzato é la comunicazione seriale, utilizzata soprattutto per la fase di taratura dei sensori, grazie alla stampa a schermo on-line dei valori
32
rilevati dal microprocessore.
Uno dei primi problemi affrontati nell’implementazione della comunicazione seriale è
stato quello relativo alla stesura di un protocollo di invio e ricezione di pacchetti da 8
bit: ciò è dovuto al fatto che i valori utilizzati nel software sono stati scelti a 32 bit,
ma la periferica USART può gestire solamente pacchetti da 8 bit ciascuno. É stato
necessario, quindi, mettere a punto una funzione apposita (invia) che divide i dati
in 4 byte gestibili dalla USART, ognuno dei quali viene inviato tramite opportune
funzioni, applicabili solo a valori di 8 bit, proprie della USART.
Il protocollo implementato è il seguente:
void invia(float dato)
{
int valore1= dato*100;
int valore2=valore1&0xFF;
int valore3=(valore1 >>8)&0xFF;
while(USART_GetFlagStatus(USART1,USART_FLAG_TC)==RESET);
USART_SendData(USART1,valore2);
}
La funzione mostra che inizialmente il dato viene moltiplicato per 100 al fine di
ottenere una precisione fino alla seconda cifra decimale, che verrebbe persa nell’eseguire il cast ad intero. Subito dopo vengono identificati i 4 byte da inviare: valore2,
valore3, valore4 e valore5, ricavati dal dato principale tramite le operazioni di shift di
bit e l’operazione AND.
Una volta inviati, l’operazione da eseguire é speculare a quella effettuata nell’invio
del dato; è stato quindi implementato un ulteriore protocollo in Processing per la
33
ricezione dei 4 byte inviati dalla STm32 e la ricostruzione del dato.
La funzione implementata é la seguente, presente all’interno della routine draw() di
Processing:
void draw()
{
if ( myPort.available() > 0) {
valore=myPort.read(); // Controllo se la porta è attiva
if(i<=3){
dati[i]=valore; // memorizzo i dati ricevuti
i=i+1;
}
if(i>3) {
i=0;
dato=((dati[3] << 24)+(dati[2] << 16)+(dati[1] << 8)+(dati[0]))/100.0;
output.println(dato);
println(dato);
}
}}
Viene in primo luogo verificato se la seriale è pronta e successivamente vengono
letti i 4 byte ricevuti dal modulo XBee. In seguito, vengono memorizzati i byte in
un vettore, dati [], e, conclusa l’operazione di memorizzazione, si ricostruisce il dato
inviato con l’operazione di shift dei bit, facendo attenzione a collocare i byte nella
posizione corretta, per non commettere errori di ricostruzione.
Il registro di memorizzazione dei dati USART della STm32 è anch’esso singolo come
quello della periferica ADC, quindi anche per l’implementazione della comunicazione
seriale si è dovuto ricorrere all’utilizzo del DMA. In generale, non é possibile sapere
quando un dato è stato ricevuto: per ovviare a questo problema, il DMA è stato
programmato per generare un interrupt ogni volta che il trasferimento dei 4 byte
viene completato. Quando quest’evento si verifica, si avvia una funzione che sposta i
dati ricevuti dal DMA in una variabile d’appoggio, definita dall’utente nelle configurazioni, ed aggiorna un flag, indicando che l’intero dato é stato ricevuto; il controllo
di quest’ultimo flag viene effettuato in polling. Conclusa l’esecuzione della funzione
34
§3.5 Risultati ottenuti e conclusioni
associata all’interrupt, il DMA può dedicarsi alla successiva ricezione.
3.5
Risultati ottenuti e conclusioni
L’idea iniziale su cui si basava il progetto era la realizzazione di un robot LineFollower
capace di sostenere velocità elevate: ciò spiega la scelta di utilizzare una macchina per
le competizioni di modellismo. Le aspettative iniziali, però, sono state ridimensionate
durante la realizzazione del progetto, a causa delle specifiche proprietà meccaniche
della struttura; nel corso dei numerosi test effettuati é emersa una incapacità del
LineFollower, ad esempio, di compiere curve particolarmente pronunciate, dovuta all’inadeguatezza del raggio di curvatura delle ruote. Inoltre, la realizzazione di un
dispositivo veloce richiedeva la presenza di un ampio spazio di prova, non disponibile
al momento dei test. Il risultato del lavoro é stato, dunque, un dispositivo sicuramente
meno veloce dell’idea iniziale, elemento che ha comportato il settaggio di un tempo di
campionamento non particolarmente elevato; allo stesso tempo, però, il LineFollower
si é dimostrato altamente preciso e reattivo nel seguire il tracciato, caratteristica di
fondamentale importanza in questo tipo di dispositivi.
Il progetto, inoltre, illustra l’utilizzo e la gestione delle schede della linea STm32 per
la realizzazione di un dispositivo automatizzato, ottenuto grazie alla realizzazione dei
circuiti elettrici utilizzati per la corretta gestione dei sensori. É emerso che il posizionamento dei sensori SHARP in direzione radiale con l’asse delle ruote ha favorito una
rilevazione degli ostacoli migliore rispetto ad un eventuale posizionamento nella parte
anteriore centrale del dispositivo. Inoltre, la scelta effettuata per la disposizione dei
sensori QRD ha consentito lo sviluppo e l’implementazione di una legge di controllo
semplice ed efficiente.
Per quanto riguarda la gestione dei moduli Wi-fi XBee, la loro configurazione si è
35
dimostrata semplice ed intuitiva. La comunicazione tra dispositivo e PC é risultata
veloce e priva di problemi, anche sulle lunghe distanze; tale efficienza viene meno nel
momento in cui il pacco batterie, che fornisce l’alimentazione al modulo di ricezione, inizia a scaricarsi: in questo caso la comunicazione avviene a tratti, con scarsa
reattività nella ricezione dei comandi inviati.
36
Appendice A
Codice creato
Codice C per STm32
Listing 3.1: Codice C for STm32 VL Discovery
# include <s t d d e f . h>
# include ”stm32f10x .h”
# include ”STM32vldiscovery .h”
// Mie f u n z i o n i
void invia ( float dato ) ;
void Ack ( void ) ;
void Delay ( int ) ;
int PDD ( int sensore , float kp , float kd ) ;
int PDS ( int sensore , float kp , float kd ) ;
// V a r i a b i l i g l o b a l i
char s ;
char send =0;
char end =1;
char finito =1;
int start =0;
int stop ;
// V a r i a b i l i f u n z i o n e PID
float Pd ;
float Dd ;
int errd =0;
int old_errd =0;
static int outd ;
float Ps ;
float Ds ;
int errs =0;
int old_errs =0;
static int outs ;
// MAIN
int main ( void ) {
37
// V a r i a b i l i
locali
uint16_t LettureADC [ 7 ] , QSX , QCX , QDX ;
int QRD , SX3 , SX5 , SX8 , DX3 , DX5 , DX8 , CX3 , CX5 , CX8 , valore , qsx , qdx , qcx ;
double q , m ;
// V a l o r i L i m i t e d e i S e n s o r i
QRD =3800;
SX3 =1140;
SX5 =650;
SX8 =220;
DX3 =1120;
DX5 =520;
DX8 =250;
CX3 =1110;
CX5 =630;
CX8 =300;
// C o e f f i c i e n t i r e t t a p o t e n z i o m e t r o motore
q =1080;
m =0.205;
// STRUTTURE
G P I O _ I n i t T y p e D e f GPIO_Struct ;
D MA _I ni t Ty pe De f DMA_Struct ;
A DC _I ni t Ty pe De f ADC_Struct ;
U S A R T _ I n i t T y p e D e f Usart_Struct ;
USART_ClockInitTypeDef UsartClock_Struct ;
T I M _ T i m e B a s e I n i t T y p e D e f TIMER_Struct ; // TIMER
T I M _ O C I n i t T y p e D e f TIME ROC_Stru ct ; // PWM
N V I C _ I n i t T y p e D e f NVIC_Struct ; // s t r u t t u r a che g e s t i s c e l ’ i n t e r r u p t
// A b i l i t o c l o c k p e r i f e r i c h e
R C C _ A P B 2 P e r i p h C l o c k C m d ( R C C _ A P B 2 P e r i p h _ G P I O A | R C C _ A P B 2 P e r i p h _ G P I O C | ←R C C _ A P B 2 P e r i p h _ A D C 1 | RCC_APB2Periph_USART1 , ENABLE ) ;
R C C _ A H B P e r i p h C l o c k C m d ( RCC_AHBPeriph_DMA1 , ENABLE ) ;
// a b i l i t o t i m e r 3
R C C _ A P B 1 P e r i p h C l o c k C m d ( R C C _ A P B 1 P e r i p h _ T I M 2 | RCC_APB1Periph_TIM3 , ENABLE ) ;
// I n i z i a l i z z o
i l e d d e l l a Stm32vl
S T M 3 2 v l d i s c o v e r y _ L E D I n i t ( LED3 ) ; //PC9
S T M 3 2 v l d i s c o v e r y _ L E D I n i t ( LED4 ) ; //PC8
// R e s e t t o s t r u t t u r a
G PI O_ St r uc tI ni t (& GPIO_Struct ) ;
// S e t t o PA3 PA4 PA5 p e r i s e n s o r i Sharp ( sx , cx , dx )
GPIO_Struct . GPIO_Pin=GPIO_Pin_3 | GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_10 ;
GPIO_Struct . GPIO_Mode= GPIO_Mode_AIN ;
GPIO_Init ( GPIOA , &GPIO_Struct ) ;
// S e t t o PA9 p e r USART1 TX, PA1 p e r TIM2 Motori e PA2 TIM2 S e r v o
GPIO_Struct . GPIO_Pin=GPIO_Pin_9 | GPIO_Pin_1 | GPIO_Pin_2 ;
38
GPIO_Struct . GPIO_Mode=G PI O_ Mo d e_ AF _P P ;
GPIO_Struct . GPIO_Speed= G P I O _ S p e e d _ 5 0 MH z ;
GPIO_Init ( GPIOA ,& GPIO_Struct ) ;
// S e t t o PA10 p e r l a USART1 RX e PA0 p e r l o USER BOTTON
GPIO_Struct . GPIO_Pin=GPIO_Pin_10 | GPIO_Pin_0 ;
GPIO_Struct . GPIO_Mode= G P I O _ M o d e _ I N _ F L O A T I N G ;
GPIO_Init ( GPIOA , &GPIO_Struct ) ;
// S e t t o PC0 PC1 PC2 p e r i s e n s o r i Qrd ( sx , cx , dx ) e PC3 P o t e n z i o m e t r o
GPIO_Struct . GPIO_Pin=GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3 ;
GPIO_Struct . GPIO_Mode=GPIO_Mode_AIN ;
GPIO_Init ( GPIOC ,& GPIO_Struct ) ;
// SETTAGGIO DMA
// I n i z i a l i z z o
i l DMA p e r l ’ADC
// i n d i r i z z o d e l l a p e r i f e r i c a dove l e g g e l ’ADC
DMA_Struct . D M A _ P e r i p h e r a l B a s e A d d r =( uint32_t ) ADC1+0x4C ;
// i n i d i r i z z o d i m e m o r i z z a z i o n e
DMA_Struct . D M A _ M e m o r y B a s e A d d r= ( uint32_t ) &LettureADC ;
// d i r e z i o n e d e l t r a s f e r i m e n t o
DMA_Struct . DMA_DIR=D M A _ D I R _ P e r i p h e r a l S R C ;
// d i m e n s i o n e d e l b u f f e r . u g u a l e a l numero d i c a n a l i d e l l ’ADC
DMA_Struct . DMA_ BufferSi ze =7;
// non i n c r e m e n t o l ’ i n i d i r i z z o d e l l a p e r i f e r i c a
DMA_Struct . D M A _ P e r i p h e r a l I n c=D M A _ P e r i p h e r a l I n c _ D i s a b l e ;
// v i e n e i n c r e m e n t a t o l ’ i n d i r i z z o d i memoria dopo o g n i l e t t u r a
DMA_Struct . DMA_MemoryInc=D M A _ M e m o r y I n c _ E n a b l e ;
// d i m e n s i o n e d e l dato da t r a s f e r i r e i n memoria 16 b i t
DMA_Struct . D M A _ P e r i p h e r a l D a t a S i z e=D M A _ P e r i p h e r a l D a t a S i z e _ H a l f W o r d ;
// d i m e n s i o n e d e l dato da t r a s f e r i r e d a l l a p e r i f e r i c a 16 b i t
DMA_Struct . D M A _ M e m o r y D a t a S i z e=D M A _ M e m o r y D a t a S i z e _ H a l f W o r d ;
// m o d a l i t à c i r c o l a r e
DMA_Struct . DMA_Mode=D M A _ M o d e _ C i r c u l a r ;
// p r i o r i t à d e l c a n a l e
DMA_Struct . DMA_Priority=D M A _ P r i o r i t y _ H i g h ;
// d i s a b i l i t o l a f u n z i o n e d i t r a s f e r i m e n t o memory t o memory
DMA_Struct . DMA_M2M=D MA _M 2M _ Di sa b le ;
// Assegno l ’ i n i z i a l i z z a i o n e a l c a n a l e 1 d e l DMA1 c i o è q u e l l o d e l l ’ADC
DMA_Init ( DMA1_Channel1 ,& DMA_Struct ) ;
// A b i l i t o
i l c a n a l e 1 d e l DMA
DMA_Cmd ( DMA1_Channel1 , ENABLE ) ;
//SETTAGGI0 PWM E TIMER
/∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗
39
∗
tempo=( p e r i o d o ∗ p r e s c a l e r ) /24Mhz
ESEMPI :
∗
∗
1ms−−−> p e r i o d o =12000
∗
p r e s c a l e r =2
∗
∗
2ms−−−> p e r i o d o =24000
∗
p r e s c a l e r =2
∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗/
// U t i l i z z o l a s t e s s a s t r u t t u r a dato che S e r v o e Motore u t i l i z z a n o s e g n a l e d i 50Hz
// imp os to c o n t e g g i o i n a v a n t i
TIMER_Struct . T IM _C ou n te rM od e=T I M _ C o u n t e r M o d e _ U p ;
// c l o c k i n t e r n o= c l o c k c o n t a t o r e
TIMER_Struct . T I M _ C l o c k D i v i s i o n=TIM_CKD_DIV1 ;
// I l t i m e r c o n t a da 0 a 23999
TIMER_Struct . TIM_Period =24000;
// f r o n t i d i c l o c k
TIMER_Struct . TIM_Prescaler =20−1;
// a s s e g n o a l TIM2 l e p r o p r i e t à s e t t a t e s o p r a
T I M _ T i m e B a s e I n i t ( TIM2 , &TIMER_Struct ) ;
// a b i l i t o l a IRQ p e r i l TIM2
TIM_ITConfig ( TIM2 , TIM_IT_Update , ENABLE ) ;
// C o n f i g u r o i PWM d e l motore e d e l s e r v o
// Motore
TIMER OC_Stru ct . TIM_OCMode=T IM _O CM o de _P WM 1 ;
// a b i l i t o l ’ Output Compare p e r g e n e r a l e i pwm
TIMER OC_Stru ct . T IM _O ut p ut St at e=T I M _ O u t p u t S t a t e _ E n a b l e ;
// s p e c i f i c o l a p o l a r i t à d e l l ’ o ut pu t
TIMER OC_Stru ct . TIM_O CPolari ty=T I M _ O C P o l a r i t y _ H i g h ;
// duty C i c l e
TIMER OC_Stru ct . TIM_Pulse =1750;
// a b i l i t o i l c a n a l e 2
TIM_OC2Init ( TIM2 , &TIME ROC_Stru ct ) ;
// S e r v o
TIMER OC_Stru ct . TIM_OCMode=T IM _O CM o de _P WM 1 ;
// a b i l i t o l ’ Output Compare p e r g e n e r a l e i pwm
TIMER OC_Stru ct . T IM _O ut p ut St at e=T I M _ O u t p u t S t a t e _ E n a b l e ;
// s p e c i f i c o l a p o l a r i t à d e l l ’ o ut pu t
TIMER OC_Stru ct . TIM_O CPolari ty=T I M _ O C P o l a r i t y _ H i g h ;
// duty C i c l e
TIMER OC_Stru ct . TIM_Pulse =1300;
// a b i l i t o i l c a n a l e 2
TIM_OC3Init ( TIM2 , &TIME ROC_Stru ct ) ;
// f a c c i o p a r t i r e i l TIM2
TIM_Cmd ( TIM2 , ENABLE ) ;
//
TIMER 3 p e r tempo campionamento a 10 ms
TIMER_Struct . T IM _C ou n te rM od e=T I M _ C o u n t e r M o d e _ U p ;
TIMER_Struct . T I M _ C l o c k D i v i s i o n=TIM_CKD_DIV1 ;
TIMER_Struct . TIM_Period =12000; // 10ms
TIMER_Struct . TIM_Prescaler =20−1;
T I M _ T i m e B a s e I n i t ( TIM3 , &TIMER_Struct ) ;
// a b i l i t o l ’ i n t e r r u p t p e r i l TIM3 p e r update , o v v e r o appena s i a z z e r a
40
TIM_ITConfig ( TIM3 , TIM_IT_Update , ENABLE ) ;
// I n t e r r u p t p e r TIM3 , s i g e n e r a appena i l TIM3 f i n i s c e d i c o n t a r e
NVIC_Struct . N VI C_ IR Q Ch an ne l=TIM3_IRQn ;
NVIC_Struct . N V I C _ I R Q C h a n n e l C m d=ENABLE ;
// p r i o r i t à d e g l i i n t e r r u p t ( s e ne ho p i u d i uno )
NVIC_Struct . N V I C _ I R Q C h a n n e l P r e e m p t i o n P r i o r i t y =0;
NVIC_Struct . N V I C _ I R Q C h a n n e l S u b P r i o r i t y =2;
NVIC_Init (& NVIC_Struct ) ;
// ADC
// im p os to l ’ADC1
ADC_DeInit ( ADC1 ) ;
// Imposto l e c a r a t t e r i s t i c h e Base d e l ADC
// c o n f i g u r o l a m o d a l i t à d i f u n z i o n a m e n t o d e l l ’ADC
ADC_Struct . ADC_Mode=A D C _ M o d e _ I n d e p e n d e n t ;
// s p e c i f i c o l a t i p o l o g i a d e l l a c o n v e r s i o n e ( modo c o n t i n u o )
ADC_Struct . A D C _ C o n t i n u o u s C o n v M o d e=DISABLE ;
// s p e c i f i c o c o n v e r s i o n e m u l t i c a n a l e
ADC_Struct . A D C _ S c a n C o n v M o de=ENABLE ;
// d e f i n i s c o i l t r i g g e r e s t e r n o ( a t t i v o l ’ adc da s o f t w a r e e no da s e g n a l e )
ADC_Struct . A D C _ E x t e r n a l T r i g C o n v=A D C _ E x t e r n a l T r i g C o n v _ N o n e ;
// s p e c i f i c o i l modo d i a l l i n e a m e n t o d e i d a t i dopo l a c o n v e r s i o n e ( a DX)
ADC_Struct . ADC_DataAlign=A D C _ D a t a A l i g n _ R i g h t ;
// s p e c i f i c o i l numero d i c a n a l i da u s a r e i n m o d a l i t à ” r e g u l a r group ”
ADC_Struct . A D C _ N b r O f C h a n n el =7;
// I n i z i a l i z z o l a p e r i f e r i c a con l e i m p o s t a z i o n e c o n t e n u t e n e l l a s t r u t t u r a ADC
ADC_Init ( ADC1 ,& ADC_Struct ) ;
// A b i l i t o l a r i c h i e s t a d e l DMA p e r L ’ADC1
ADC_DMACmd ( ADC1 , ENABLE ) ;
// A b i l i t o l a p e r i f e r i c a ADC1
ADC_Cmd ( ADC1 , ENABLE ) ;
// C a l i b r a z i o n e d e l l a p e r i f e r i c a ( l a r e s e t t o , a t t e n d o , l a s e t t o
, attendo )
A D C _ R e s e t C a l i b r a t i o n ( ADC1 ) ;
while ( A D C _ G e t R e s e t C a l i b r a t i o n S t a t u s ( ADC1 ) ) ;
A D C _ S t a r t C a l i b r a t i o n ( ADC1 ) ;
while ( A D C _ G e t C a l i b r a t i o n S t a t u s ( ADC1 ) ) ;
// Imposto c a n a l i d e i s e n s o r i p e r l ’ADC e l o r o p r i o r i t à
// ( nome p e r i f , c a n a l e da a t t i v a r e , p r i o r i t à s e n s o r e , tempo campionamento )
// Sharp
A D C _ R e g u l a r C h a n n e l C o n f i g ( ADC1
// Sharp
// Sharp
// QDR
// QDR
// QDR
// P o t e n z i o m e t r o
, ADC_Channel_3 , 1 , A D C _ S a m p l e T i m e _ 5 5 C y c l e s 5 ) ;
41
// USART
// D e f i n i s c o l e p r o p r i e t à d e l l a USART
// v e l o c i t à u s a r t
Usart_Struct . USAR T_BaudRa te =115200;
Usart_Struct . USART_Parity=U SA RT _P a ri ty _N o ;
Usart_Struct . USAR T_StopBi ts=U S A R T _ S t o p B i t s _1 ;
Usart_Struct . U S A R T _ W o r d L e n g th=U S A R T _ W o r d L e n g t h _ 8 b ;
Usart_Struct . USART_Mode=USART_Mode_Tx | USART_Mode_Rx ;
Usart_Struct . U S A R T _ H a r d w a r e F l o w C o n t r o l=U S A R T _ H a r d w a r e F l o w C o n t r o l _ N o n e ;
// I n i z i a l i z z o l a USART
USART_Init ( USART1 , &Usart_Struct ) ;
// im p os to i l c l o c k
U s a r t C l o c k _ S t r u c t . USART_Clock=U S A R T _ C l o c k _ D i s a b l e ;
U s a r t C l o c k _ S t r u c t . USART_CPHA=U S A R T _ C P H A _ 1 E d ge ;
U s a r t C l o c k _ S t r u c t . USART_CPOL=USAR T_CPOL_L ow ;
U s a r t C l o c k _ S t r u c t . USART_LastBit=U S A R T _ L a s t B i t _ D i s a b l e ;
U SA RT _C l oc kI ni t ( USART1 ,& U s a r t C l o c k _ S t r u c t ) ;
// A b i l i t o l a USART
USART_Cmd ( USART1 , ENABLE ) ;
// ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗
// LOOP
// ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗
TIM_Cmd ( TIM3 , ENABLE ) ; // a v v i o i l TIM3 p e r i l
finito =1;
while ( 1 ) {
’ campionamento ’
// a v v i o l e t t u r a d e l l ’ADC
A D C _ S o f t w a r e S t a r t C o n v C m d ( ADC1 , ENABLE ) ;
// v e r i f i c o che i l dma f i n i s c a d i l a v o r a r e
while ( D M A _ G e t F l a g S t a t u s ( DMA1_FLAG_TC1 )==RESET ) ;
// Assegno p e r comodità i nomi a i s e n s o r i QRD
QSX=LettureADC [ 3 ] ;
QCX=LettureADC [ 4 ] ;
QDX=LettureADC [ 5 ] ;
// V e r i f i c o che
ci siano o s t a c o l i
if ( ( LettureADC [ 1 ] > CX5 ) ) | | ( LettureADC [ 2 ] > DX3 ) | | ( LettureADC [ 0 ] > SX3 ) ) {
stop =1;
S T M 3 2 v l d i s c o v e r y _ L E D O n ( LED4 ) ;
// im p os to l e r u o t e d r i t t e
TIM2−>CCR3 =1300;
// fermo motore
TIM2−>CCR2 =1750;
// Se non c i sono o s t a c o l i
else {
stop =0;
S T M 3 2 v l d i s c o v e r y _ L E D O f f ( LED4 ) ;
42
}
// ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗
// CONTROLLO
// ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗
Ack ( ) ;
// Se i s e n s o r i non r i v e l a n o o s t a c o l i ed a v v i o i l motore
if ( stop==0 && start==1){
//PD
if ( ( QSX >300) ) {
TIM2−>CCR3= PDS ( QSX , 0 . 1 5 7 , 0 . 1 ) ;
}
else {
if ( ( QDX >300) ) {
TIM2−>CCR3= PDD ( QDX , − 0 . 1 2 8 , 0 . 8 ) ;
}
}
//ALGORITMO
// ISTERESI
if ( QDX>QRD ) {
qdx ==1;}
if ( QCX>QRD ) {
qcx ==1;}
if ( QSX>QRD ) {
qsx ==1;}
// SENSORE DX
if ( qdx==1){ // è n e r o
if ( QDX <3500) {
qdx =2; // è non n e r o
}}
else { // è non n e r o ( qdx==2)
if ( QDX >=3800){
qdx =1; // è n e r o
}}
if ( qdx==0){ // è b i a n c o
if ( QDX >500) {
qdx =3; // è non b i a n c o
}}
else { // è non b i a n c o ( qdx==2)
if ( QDX <=300){
qdx =0; // è b i a n c o
}}
43
// SENSORE CX
if ( qcx==1){ // è n e r o
if ( QCX <3500) {
qcx =2; // è non n e r o
}}
else { // è non n e r o
if ( QCX >=3800){
qcx =1; // è n e r o
}}
if ( qcx==0){ // è b i a n c o
if ( QCX >500) {
qcx =3; // è non b i a n c o
}}
else { // è non b i a n c o
if ( QCX <=300){
qcx =0; // è b i a n c o
}}
// SENSORE SX
if ( qsx==1){ // è n e r o
if ( QSX <3500) {
qsx =2; // è non n e r o
}}
else { // è non n e r o
if ( QSX >=3800){
qsx =1; // è n e r o
}}
if ( qsx==0){ // è b i a n c o
if ( QSX >500) {
qsx =3; // è non b i a n c o
}}
else { // è non b i a n c o
if ( QSX <=300){
qsx =0; // è b i a n c o
}}
// CASI ALGORITMO
if ( ( qdx==0)&&(qcx==1)&&(qsx==0) ) { // c a s o 010
valore =2;
TIM2−>CCR3 =1300;
TIM2−>CCR2 =1697;
s =0;
}
if ( ( qsx==1)&&(qcx==1)&&(qdx==1) ) { // c a s o 111
valore =6;
TIM2−>CCR3 =1300;
TIM2−>CCR2 =1697;
s =0;
}
valore =3;
TIM2−>CCR3 =1150;
TIM2−>CCR2 =1697;
44
s =1;
}
valore =1;
TIM2−>CCR3 =1050;
TIM2−>CCR2 =1697;
s =1;
}
valore =5;
TIM2−>CCR3 =1500;
TIM2−>CCR2 =1697;
s =2;
}
valore =4;
TIM2−>CCR3 =1600;
TIM2−>CCR2 =1697;
s =2;
}
valore =7;
if ( s==1){
// l ’ u l t i m a v o l t a ho g i r a t o a dx
TIM2−>CCR3 =700;
TIM2−>CCR2 =1680;
s =0;
}
if ( s==2){
// l ’ u l t i m a v o l t a ho g i r a t o a sx
TIM2−>CCR3 =1900;
TIM2−>CCR2 =1680;
s =0;
}
}
}
// ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗
// INVIO DATI USART
// ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗
// I d e n t i f i c o
Ack ( ) ;
i l comando r i c e v u t o da P r o c e s s i n g
if ( send==1){
// I n v i o d a t i p e r stampa a schermo
invia ( LettureADC [ 0 ] ) ; // s h a r p sx
invia ( LettureADC [ 1 ] ) ; // s h a r p cx
invia ( LettureADC [ 2 ] ) ; // s h a r p dx
invia ( LettureADC [ 3 ] ) ; // sx qrd
invia ( LettureADC [ 4 ] ) ; // cx qrd
invia ( LettureADC [ 5 ] ) ; // dx qrd
invia ( LettureADC [ 6 ] ) ; // p o t e n z i o m e t r o
invia ( TIM2−>CCR2 ) ; // p u l s e Motore
invia ( TIM2−>CCR3 ) ; // p u l s e S e r v o
invia ( valore ) ; // s t a t o a l g o r i t m o i n c u i mi t r o v o
45
}
// Se non a b i l i t o i l motore , l o g e s t i s c o con i l p o t e n z i o m e t r o
if ( stop==0){
// Equazione p o t e n z i o m e t r o motore
TIM2−>CCR2=m ∗ LettureADC [ 6 ] + q ;
}
finito =1;
while ( end ) ;
end =1;
finito =0;
}
return 0 ;
}
// FUNZIONI PRIVATE
// P r o t o c o l l o i n v i o ( p a s s o da 32 a 8 b i t )
void invia ( float dato ) {
int valore1= dato ∗ 1 0 0 ;
int valore2=valore1&0xFF ;
int valore3 =( valore1 >>8)&0xFF ;
// TC t r a n s f e r t c o m p l e t e
while ( U S A R T _ G e t F l a g S t a t u s ( USART1
USART _SendDa ta ( USART1 , valore2 ) ;
}
, USART_FLAG_TC )==RESET ) ;
// Funzione r i c e z i o n e comando da p r o c e s s i n g
void Ack ( void ) {
uint16_t ack=U S A R T _ R e c e i v e D a t a ( USART1 ) ;
if ( ack==1){
send =1;
// t r a s m i s s i o n e a v v i a t a , Verde on
}
if ( ack==2){
send =0;
// t r a s m i s s i o n e i n t e r r o t t a , Verde o f f
}
if ( ack==3){
TIM2−>CCR2 =1700;
// a v v i o motore , Blu on
}
if ( ack==4 ) {
46
TIM2−>CCR2 =1750;
// d i s a b i l i t o motore , Blu o f f
}
}
// Funzione r i t a r d o
void Delay ( int n )
{
for ( ; n != 0 ; n−−) ;
}
// Funzione d i I n t e r r u p t g e n e r a t o d a l c o n t e g g i o f i n i t o d e l TIM3
void T IM 3_ IR Q Ha nd le r ( void )
{
if ( finito==0){
// TIM3 s c a d e prima che i l c i c l o w h i l e f i n i s c a , non r i s p e t t o i 10ms
while ( 1 ) {
// l a m p e g g i a n o i l e d p e r s e g n a l a r e
Delay ( 0 x5FFFF ) ;
Delay ( 0 x5FFFF ) ;
}
}
end =0;
// P u l i s c o
il
f l a g d e l l ’ i n t e r r u p t , da e s e g u i r e manualmente
T I M _ C l e a r I T P e n d i n g B i t ( TIM3 , TIM_IT_Update ) ;
// P u l i s c o
il
f l a g d i f i n e c o n t e g g i o , da e s e g u i r e manualmente
TIM_ClearFlag ( TIM3 , T IM _F LA G _U pd a te ) ;
}
// F u n z i o n i PD
int PDD ( int sensore , float kp , float kd ) {
if ( sensore >3800) {
// s e s u p e r i o r e a 3800 s i c u r a m e n t e è n e r o
// l o b l o c c o a 3800 a l t r i m e n t i i l s e r v o s f o r z a t r o p p o
sensore =3800;
}
errd=sensore −300;
Pd=kp ∗ errd ;
if ( kd >0){
Dd=kd ∗ ( errd−old_errd ) ;
old_errd=errd ;
}
else {
Dd =0;
}
outd=Pd+Dd +1300;
return ( outd ) ;
}
47
int PDS ( int sensore , float kp , float kd ) {
if ( sensore >3800) {
// s e s u p e r i o r e a 3800 s i c u r a m e n t e è n e r o
// l o b l o c c o a 3800 a l t r i m e n t i i l s e r v o s f o r z a t r o p p o
sensore =3800;
}
errs=sensore −300;
Ps=kp ∗ errs ;
if ( kd >0){
Ds=kd ∗ ( errs−old_errs ) ;
old_errs=errs ;
}
else {
Ds =0;
}
outs=Ps+Ds +1300;
return ( outs ) ;
}
48
Codice Processing
// C o n f i g u r a z i o n e s e r i a l e
import processing . serial . ∗ ;
Serial myPort ;
PrintWriter output ;
int [ ] dati= new int [ 4 ] ;
int valore ;
int i =0;
float dato ;
void setup ( ) {
myPort=new Serial ( this , Serial . list ( ) [ 0 ] , 1 1 5 2 0 0 ) ;
// C r e a z i o n e f i l e D a t i . t x t
output = createWriter ( ”Dati . txt” ) ;
size ( 8 0 0 , 1 0 0 ) ;
background ( 0 ) ;
// F i n e s t r a comandi
text ( ”Premere : ” , 1 5 , 3 0 ) ;
text ( ” ’ T ’ per visualizzare i dati (Verde on)” , 1 5 , 5 5 ) ;
text ( ” ’ S ’ per terminare la visione dei dati (Verde o f f )” , 2 8 0 , 5 5 ) ;
text ( ” ’ C ’ per accendere motore (Blu on)” , 1 5 , 8 5 ) ;
text ( ” ’ F ’ per spegnere i l motore (Blu o f f )” , 2 8 0 , 8 5 ) ;
fill ( 2 , 1 0 2 , 1 5 3 ) ;
}
// P r o t o c o l l o r i c e z i o n e
void draw ( ) {
if ( myPort . available ( ) > 0 ) {
valore=myPort . read ( ) ; // c o n t r o l l o s e l a p o r t a è a t t i v a
if ( i<=3){
dati [ i ]= valore ; // memorizzo i d a t i r i c e v u t i
i=i +1;}
if ( i >3) {
i =0;
dato =(( dati [ 3 ] << 2 4 ) +(dati [ 2 ] << 1 6 ) +(dati [ 1 ] << 8 ) +(dati [ 0 ] ) ) / 1 0 0 . 0 ;
output . println ( dato ) ;
println ( dato ) ; // stampo a schermo
}
}
}
// G e s t i o n e comandi
void keyPressed ( ) {
if ( key== ’ t ’ ) {
// a b i l i t o T r a s m i s s i o n e d a t i
myPort . write ( 1 ) ;
}
if ( key== ’ s ’ ) {
// d i s a b i l i t o T r a s m i s s i o n e d a t i
}
if ( key== ’ c ’ ) {
// a z i o n o motore
}
if ( key== ’ f ’ ) {
// b l o c c o motore
49
}
output . flush ( ) ;
}
50
Appendice B
Immagini del LineFollower
Figura 3.5: Vista dall’alto
51
Figura 3.6: Vista laterale
Figura 3.7: Vista frontale
52
Figura 3.8: Sensori QRD
53
Elenco delle figure
1.1
Tunnel del LHC al CERN di Ginevra con la linea bianca del LineFollower
5
1.2
STm32 VL Discovery . . . . . . . . . . . . . . . . . . . . . . . . . . .
6
2.1
Controllo elettronico Tamiya TEU-104BK . . . . . . . . . . . . . . .
10
2.2
Servo Hitec HS-422 Deluxe . . . . . . . . . . . . . . . . . . . . . . . .
10
2.3
Motore CC elementare . . . . . . . . . . . . . . . . . . . . . . . . . .
12
2.4
Esempi di duty cicle . . . . . . . . . . . . . . . . . . . . . . . . . . .
14
2.5
Sensori QRD . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
14
2.6
Sensore SHARP GP2Y0A21YK0F . . . . . . . . . . . . . . . . . . . .
15
2.7
Moduli XBee Serie 2, con antenna a filo ed a chip.In rosso è evidenziato
il codice identificativo del dispositivo. . . . . . . . . . . . . . . . . . .
17
2.8
Schema elettrico generale . . . . . . . . . . . . . . . . . . . . . . . . .
19
2.9
Schema elettrico del LM1117 . . . . . . . . . . . . . . . . . . . . . . .
19
3.1
Circuito di stabilizzazione e polarizzazione del QRD1114 . . . . . . .
24
3.2
Automa a stati finiti . . . . . . . . . . . . . . . . . . . . . . . . . . .
26
3.3
Fasi di una curvatura a destra . . . . . . . . . . . . . . . . . . . . . .
27
3.4
Casi SX.CX.DX=111 e SX.CX.DX=101 non considerati . . . . . . . .
29
3.5
Vista dall’alto . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
51
3.6
Vista laterale . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
52
54
ELENCO DELLE FIGURE
ELENCO DELLE FIGURE
3.7
Vista frontale . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
52
3.8
Sensori QRD . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
53
55
Bibliografia
[1] C.Bonivento C.Melchiorri R.Zanasi, “Sistemi di Controllo Digitale”
[2] STm32Vl Discovery, “http://www.st.com/internet/evalboard/product/250863.jsp”
[3] STM32F100RB, “http://www.st.com/internet/mcu/product/216844.jsp”
[4] Digi XBee “http://examples.digi.com/it/”
56

Realizzazione di un Line Follower 4WD

Transcript

Documenti analoghi

Marmitek IP RoboCam brochure

Bepi Colombo Prize 2008

RICERCATORI FINALISTI DEL BEPI COLOMBO PRIZE

medusa - Vitrociset

[email protected]

Presentazione standard di PowerPoint

AIRSIDE DRIVING CERTIFICATE

Laboratorio di Linguaggi di Algoritmi e Strutture Dati Prova di

RC 3.000

Esercizi proposti - Corsi a Distanza