Documento 2556776

ISTITUTO TECNICO INDUSTRIALE STATALE
Leonardo da Vinci
Articolo I.
a)
Via Tambroni - 47900 RIMINI
Tel. 0541/384211-384159 Fax 0541/383292 RNTF010004
82007870403
Sito: www.itisrn.it e-mail: [email protected]
C.F.
EQ2R
ROBOT IN EQUILIBRIO SU DUE RUOTE
A cura di: Ubaldi Leonardo & Bracalenti Andrea
INDICE
 Descrizione generale
 Caratteristiche tecniche
 Principi teorici
1. Teoria della PWM
2. Descrizione del sistema
3. Sistemi di controllo
4. Controllo proporzionale sul pic
5. Problematiche riscontrate
 Descrizione dei circuiti
1. Circuito microcontrollore
2. Circuito Driver di potenza L298
3. Circuito filtri
 Descrizione hardware usato
1.
2.
3.
4.
5.
6.
Microcontrollore Pic 16F876
Dual Full-Bridge Driver L298
Sensori di prossimità GP2D120
Motori DC
Ruote e mozzi
Batterie
 Listato software di controllo
 Datasheet
1. L298
2. PIC 16F876
3. GP2D120
2
 Descrizione generale
È un robot a due ruote che ha il suo centro di gravità sopra le ruote. Senza
un sistema di controllo attivo, il robot cadrebbe. Quando il robot s’inclina
in avanti o indietro avverte la caduta tramite i suoi sensori (di distanza o
prossimità) e si muove in modo da compensare la caduta e tornando in
posizione eretta.
 Caratteristiche tecniche
Il prototipo si compone di quattro livelli:
- la base a cui sono fissati i motori che fanno muovere il robot tenendolo in equilibrio
- la parte immediatamente soprastante dove sono poste le batterie ( 2 x 12 V 1.3AH al piombo )
- il ‘piano’ dei circuiti di potenza comprendente driver L298 e filtri antirumore dei motori
- la parte superiore dove è installato il microcontrollore PIC che si occupa di gestire il
funzionamento dei motori.
Inoltre ad una distanza di circa 13 cm dal suolo sono montati i sensori di distanza .
Il robot legge i due valori analogici dati da sensori (che sono proporzionali alla distanza tra sensore
e suolo) e, dopo averli confrontati ed essersi calcolato angolo e verso di caduta decide con quanta
forza (variando il valor medio della tensione in uscita con l’uso della PWM) e in che direzione far
muovere i motori.
 Principi teorici
1. Teoria della PWM
La PWM è una tecnica usata in campo digitale per permettere a dispositivi quali, ad esempio, i
microcontrollori di generare un segnale adatto a pilotare motori in corrente continua variandone la
velocità.
I motori DC sono, infatti, sensibili al valor medio del segnale applicato all’armatura e con la PWM
si danno impulsi rettangolari costantemente ripetuti nel tempo. Quando si va a variare la lunghezza
dell’impulso positivo ossia il duty-cicle (senza alterare il periodo) si cambia anche la velocità del
motore a cui è applicato. Chiaramente maggiore è il ducty-cicle, maggiore è la velocità a cui
ruotano i motori e viceversa. Il PIC 16F876 può svolgere questa funzione automaticamente e
parallelamente al programma eseguito grazie a un dispositivo hardware che manda costantemente il
segnale PWM ad un uscita ricevendo dal software solo i valori di periodo e durata del livello alto.
Il periodo del segnale deve essere caricato nel registro PR2 (registro di periodo del TRM2) mentre il
duty-cicle viene caricato nel registro CCRR1L (gli 8 bit di ordine più alto) e sui bit 4 e 5 del registro
CCP1CON (i due bit di ordine più basso) arrivando quindi ad un valore di TMR2 lungo 10 bit e in
grado di generare 1024 possibili velocità diverse. Il valore caricato in PR2 viene continuamente
confrontato con il valore di TMR2 (che è incrementato con la frequenza fosc/4 ); quando i 2 valori
sono uguali viene posto alto il pin RC2/CCP1 (uscita della PWM) e il TMR2 viene azzerato. Poi
TMR2 (incrementato alla stessa frequenza) viene confrontato con CCP1L e i bit 4 e 5 del registro
CCP1CON. Quando c’è uguaglianza viene posto a 0 il pin RC2/CCP1. Poi il processo inizia da
capo generando un nuovo periodo del segnale.
3
2. Descrizione del sistema
Il sistema può essere schematizzato come in figura:
Inizialmente viene tarato il riferimento che sarà un valore costante: φ=0. Ogni instante di
campionamento verrà letto il valore di distanza ricavato dalla lettura dei sensori. Il controllore:
 Lancia la conversione del valore analogico dato dai due sensori e salva in due registri a 8
bit le due letture
 Determina quale delle due distanze lette è maggiore e di conseguenza decide il verso in cui
far ruotare i motori
 Calcola il modulo dell’errore sottraendo al valore più grande quello più piccolo
 In base al modulo dell’errore decide quale valore di duty-cicle dare ai motori
 Carica il valore in CCP1L facendo ripartire la PWM e ritorna al primo punto
Il driver L298 amplifica il segnale PWM del microcontrollore da 5 a 24 V e fornisce la corrente
richiesta dai motori.
Il filtro serve a tagliare i disturbi generati dai motori
La funzione dei due motori è quella di riportare il sistema nel punto di equilibrio instabile del
pendolo trasformando il segnale elettrico in una rotazione, facendolo avanzare o indietreggiare
(compensando, cioè, eventuali spostamenti del sistema dal suo punto di equilibrio).
Consideriamo ad esempio il caso riportato in figura:
il sistema si sbilancia cadendo all'indietro. Grazie ai sensore di intensità luminosa viene rilevata la
nuova posizione angolare −θ. Mediante la legge di controllo vengono azionati i due motori: questi
4
hanno il compito di fornire al sistema una componente −v per riportarlo nella posizione di
riferimento. Dualmente accade se il sistema cade in avanti come vediamo in figura:
3. Sistemi di controllo
Il sistema di controllo è il sistema che, letti i dati in ingresso, decide come operare sulle uscite per
ottenere il comportamento desiderato del sistema da controllare. Nel caso di progetto di sistema che
tenga in equilibrio un robot su 2 ruote, lo schema è quello di figura:
Un primo semplice sistema di controllo è il controllo ON / OFF cioè i motori vengono azionati a
piena velocità nel verso opposto all’angolo di caduta in modo da correggerlo e far raddrizzare il
robot .
Questo metodo ha però il notevole svantaggio di bloccare i motori solo nel momento esatto in cui i
due sensori danno la lettura tale che il modulo dell’errore è uguale a 0; perciò i motori (che
impiegano un certo tempo affinché l’inerzia diventi ininfluente) continuano a ruotare e il robot si
inclina nel verso opposto; si viene quindi a creare una condizione di oscillazione che difficilmente
porterà il robot a stare in equilibrio.
Questo sistema si può migliorare facendo in modo che man mano che la differenza tra i due valori
letti si riduce, diminuisca anche la velocità dei motori: così quando il prototipo è in prossimità del
punto di equilibrio la bassa velocità dei motori lo fa oltrepassare di un piccolo angolo e
l’oscillazione della struttura viene notevolmente ridotta. Questo sistema è detto Controllo
Proporzionale.
4. Controllo proporzionale sul PIC
Il PIC è un microcontrollore perciò non riesce ad elaborare dati analogici senza prima averli
convertiti in forma digitale; questo vale sia per gli ingressi sia per le uscite che sono sempre digitali
(infatti per ottenere una variazione del valor medio in uscita, e quindi della velocità dei motori, si
opera con la tecnica della PWM). Si può quindi fare un sistema di controllo proporzionale ma
approssimato con la filosofia di associare una certa velocità dei motori ad ogni angolo di caduta del
robot. In pratica il range di errori (differenza tra valore dato dal sensore anteriore e da quello
posteriore) è stato diviso in un certo numero di intervalli (nel nostro caso 6) e ad ogni intervallo in
cui cade l’errore è assegnato un certo valore di Ton (tempo in cui l’uscita PWM sta al livello alto in
uscita) in modo da dare la velocità adatta a recuperare la caduta.
5
5. Problematiche riscontrate
 Le spazzole dei motori generano un forte rumore di disturbo che si manifesta come una
forza controelettromotrice. Questa contro fem, trasmessa attraverso i cavi di connessione tra driver
e motori, va a disturbare il microcontrollore (arrivando anche a resettarlo) e impedisce il corretto
funzionamento del circuito. L’inconveniente si poteva risolvere
principalmente in due modi: sostituendo il driver con un circuito costituito da relé e con
alimentazione separata oppure inserendo un filtro tra driver e motori in modo da bloccare i disturbi
e lasciare passare solo la continua. La soluzione scelta prevede l’uso di un filtro LC per ogni
motore:
Vo varia applicando la legge del partitore di tensione al ramo 2 costituito da 2L e dal secondo C.
Sostituendo i valori si ottiene:
Si deduce quindi che i filtri hanno ciascuno 2 poli complessi coniugati con parte reale uguale a 0
perciò il sistema filtri è oscillante.
 Inoltre i sensori utilizzati sono di semplice uso e medio costo ma danno due importanti
svantaggi: innanzitutto non permettono la stabilità del robot su superfici inclinate, come discese o
salite, in quanto le letture non darebbero un valore di errore proporzionale all’ inclinazione del
prototipo impedendone cosi il normale funzionamento. L’altro svantaggio è che le letture effettuate
sono sensibili alla superficie sottostante al robot in quanto delle superfici chiare riflettono molto
meglio i raggi infrarossi dei sensori di prossimità rispetto a superfici scure quindi ogni qual volta si
cambia superficie d'appoggio o viene modificata l'illuminazione della stanza i parametri del sistema
di controllo vanno modificati.
6
 Descrizione dei circuiti
1. Circuito microcontrollore
Il circuito è composto da uno stadio di alimentazione consistente in un regolatore di tensione 7805
che abbassa i 9V della batteria ai 5V utilizzabili dal PIC e dai sensori GP2D120.
Il secondo stadio è formato dal microcontrollore PIC 16F876 avente una rete di oscillazione a
quarzo da 20MHz e due condensatori da 22pF; agli ingressi del PIC AN0 e AN1 (pin 2 e 3) sono
connessi i sensori di prossimità (all’alimentazione sono collegati due diodi di protezione
dall’erronea inversione di polarità); le uscite RB1 e RB2 (pin 22 e 23) vanno ai connettori del driver
dei motori DC. Inoltre sono presenti 3 led: il verde su RC0 (pin 11) indica l’avvio del programma, il
giallo su RC1 (pin 12) visualizza il segnale PWM mandato ai motori e il rosso su RC2 (pin 13) che
indica il verso di caduta.
7
2. Circuito driver di potenza L298
Il circuito si basa sul driver L298 e comprende 4 diodi di scarica della tensione generata dagli
avvolgimenti dei motori; gli Enable sono cortocircuitati tramite cavi a 5 V in modo da lasciare
sempre abilitati tutti i canali; i Sense sono collegati a massa tramite resistori da 1 Ω ma in questo
progetto non sono utilizzate le uscite di sense.
8
3. Circuito filtri
Il circuito è composto da due filtri passivi a pi-greco (uno per ogni motore).
Ognuno di essi comprende 2 condensatori da 100nF posti in parallelo al segnale dato ai motori e 2
induttanze da 160mH poste in serie al segnale.
9
 Descrizione hardware usato
1.
MICROCONTROLLORE PIC167876
Costruttore: Microchip ©
Il microcontrollore PIC 16F876 della Microchip © è un integrato a 28 pin in grado di gestire fino a
22 ingressi di cui 5 analogici. Gli ingressi analogici sono internamente collegati ad un multiplexer e
sono poi convertiti dall’ADC interno del PIC. Questa famiglia di microcontrollori è programmabile
con l’uso di soli 35 diverse istruzioni. In questo progetto è usato con un oscillatore al quarzo da 20
MHz perciò per eseguire ogni istruzione vengono impiegati 200 nsec.
Per ulteriori informazioni consultare il datasheet in appendice.
2.
DUAL FULL-BRIDGE DRIVER L298
Costruttore: SGSThomsonMicroelettronics
L’ integrato L298 è un dual full-bridge driver, vale a dire un integrato in grado di pilotare 2 motori
DC contemporaneamente. Può erogare correnti di 2A per canale e 3A di picco per canale (non
ripetitivi). Ogni canale è controllato da una logica d’ingresso TTL compatibile e ciascuna coppia di
piloti (un intero ponte) è dotata di un ingresso (enable) che inibisce e fa spegnere tutti e quattro i
transistor; se l’ingresso enable è al livello logico alto il ponte è attivo.
Per la logica di comando è prevista un’alimentazione separata in modo tale che sia possibile sia
alimentarla con la stessa alimentazione usata per i motori che alimentarla con un basso voltaggio
(per ridurre la dissipazione).
Inoltre, l’ L298 è dotato di pin di ‘sense’ ossia un’uscita analogica (da -1 a 2.3V) che da una
tensione proporzionale alla corrente assorbita dai motori; tra questi pin e la massa va collegata una
resistenza da 1Ω e che possa dissipare una potenza di 1-2W; se non si vogliono usare i ‘sense’ la
resistenza va comunque messa in quanto fondamentale al funzionamento dell’integrato (la
resistenza inserita tra questi pin e massa permette di controllare la corrente sul carico).
Nel circuito devono inoltre essere implementati 4 diodi di scarica delle induttanze degli
avvolgimenti per ognuno dei 2 motori (2 diodi per canale).
Per ulteriori informazioni consultare il datasheet in appendice.
10
3.
SENSORE DI PROSSIMITA’ GP2D120
Costruttore: Sharp
È un sensore per la misura di distanza che dà un’uscita analogica in tensione proporzionale alla
distanza letta. Funziona utilizzando tecnologia a raggi infrarossi e riesce perciò a misurare un
piccolo range di distanze (da 4 a 30cm) e la misura è influenzata dal tipo di superficie su cui si
riflettono i raggi. Ha già il condizionatore di segnale e il filtro integrati e richiede un’alimentazione
di 5V.
4.
MOTORI DC
Costruttore: Maxon
Sono motori in corrente continua già dotati di riduzioni e decoder HP (non usato per questo
progetto). L’albero ha un diametro da 6 mm e il motore è complessivamente lungo 160mm. La
tensione massima d’armatura applicabile è di 24V.
11
5.
RUOTE E MOZZI
Costruttore: Proline e Lynxmotion
Ruote Off-Road 44 x 87 mm di diametro usate per equipaggiare Robot che devono affrontare
qualsiasi tipo di terreno.
Ogni mozzo è utilizzabile per un asse da 6mm e collega l'asse del motore alla ruota, specifico per
le ruote da modellismo. Sono realizzati in alluminio.
6.
BATTERIE
Costruttore: Beghelli
Sono batterie ricaricabili al piombo acido da 12 V 1.3 AH e sono poste in serie a formare una
tensione di 24V. Le batterie al piombo sopportano bene le richieste dei picchi di corrente dovute
all’avviamento della rotazione dei motori
12
 Listato software
status
porta
portb
portc
intcon
tmr2
t2con
ccpr1l
ccp1con
adresh
adcon0
selreg
na
ni
pwmopt
mod_e
ton
tempo
liv1a
liv2a
liv3a
liv4a
liv5a
liv6a
liv1b
liv2b
liv3b
liv4b
liv5b
liv6b
na
nc
nc
n2
n1
n0
option_reg
trisa
trisb
trisc
pr2
adcon1
rc0
rc1
ra0
ra1
rp0
rp1
cf
zf
rb0
rb1
rb2
rb3
verso
bitlev
cambfr
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
equ
h'03'
h'05'
h'06'
h'07'
h'0b'
h'11'
h'12'
h'15'
h'17'
h'1e'
h'1f'
h'20'
h'21'
h'22'
h'23'
h'24'
h'25'
h'26'
h'29'
h'2a'
h'2b'
h'2c'
h'2d'
h'2e'
h'31'
h'32'
h'33'
h'34'
h'35'
h'36'
h’37’
h’38
h’39’
h'3a'
h'3b'
h'3c'
h'81' ^ h'81'
h'85' ^ h'80'
h'86' ^ h'80'
h'87' ^h'80'
h'92'^h'80'
h'9f' ^ h'80'
d'0'
d'1'
d'0'
d'1'
d'5'
d'6'
d'0'
d'2'
d'0'
d'1'
d'2'
d'3'
d'0'
d'1'
d'2'
13
ORG
H'0000'
;INIZIO PROGRAMMA
main
call
call
call
call
call
bsf
setup_io
set_pwm
setup_adc
setup_liva
setup_livb
portc,rc0
;START ALL’ADC
ripe
movlw
movwf
call
movf
movwf
clrf
call
movf
movwf
subwf
btfss
goto
b'00000001'
selreg
;sceglie conversione an1
adconv
;converte an0
adresh,0
na
;risultato conversione in na
selreg
;sceglie conversione su an0
adconv
;converte an2
adresh,0
ni
;risultato conversione in ni
na,0
status,cf ;na < ni ?
fmagb
;sottoprogrammi di sutup
movwf
bsf
call
goto
mod_e
pwmopt,verso
livellib
conti
;modulo dell'errore per pwm (na > ni)
;setto il verso avanti
bcf
movf
subwf
movwf
call
pwmopt,verso
na,0
ni,0
mod_e
livellia
;setto il verso indietro (n1 < na)
conti
btfsc
goto
pwmopt,verso
avan
;controlla verso motori
indi
bcf
bcf
bcf
movf
movwf
goto
portb,rb1
portb,rb2
portc,rc1
ton,0
ccpr1l
ripe
bsf
bsf
bsf
movf
sublw
movwf
goto
portb,rb1
portb,rb2
portc,rc1
ton,0
d'255'
ccpr1l
ripe
fmagb
avan
;SOTTOPROG DI SETUP
set_pwm
bsf
status,rp0
bcf
status,rp1
movlw d'255'
movwf pr2
bcf
status,rp0
movlw h'0f'
movwf ccp1con
movlw b'00000101'
;modulo dell'errore x pwm
;verso indietro
;ton→ ccpr1l
;verso avanti
;ton→ ccpr1l
;periodo pwm
;abilita modalita’ pwm
;abilita tmr2 e prescaler a 4
14
movwf t2con
return
setup_io
setup_adc
bsf
bcf
movlw
movwf
movlw
movwf
movlw
movwf
bcf
return
status,rp0
status,rp1
b'11111111'
trisa
b'00000000'
trisb
b'00000000'
trisc
status,rp0
movlw
movwf
bsf
bcf
clrf
bcf
bcf
return
b'10000001'
adcon0
status,rp0
status,rp1
adcon1
status,rp0
status,rp1
;portA come ingressi
;portB come uscite
;portC come uscite
;imposta ancon0
;imposta adconv1
;IMPOSTA I LIVELLI DEGLI ERRORI
setup_liva
movlw b'00000101'
movwf liv1a
movlw b'00001001'
movwf liv2a
movlw b'00001000'
movwf liv3a
movlw b'00001100'
movwf liv4a
movlw b'00010000'
movwf liv5a
movlw b'00010111'
movwf liv6a
return
setup_livb
movlw
movwf
movlw
movwf
movlw
movwf
movlw
movwf
movlw
movwf
movlw
movwf
return
b'00000000'
liv1b
b'00000010'
liv2b
b'00000011'
liv3b
b'00000110'
liv4b
b'00001001'
liv5b
b'00010101'
liv6b
;CONTROLLA I LIVELLI DELL'ERRORE E IMPOSTA TON
livellia
movf liv1a,0
subwf mod_e,0
btfsc
status,cf
;errore < livello1
goto
step2a
step2a
clrf
goto
ton
finliva
movf
liv2a,0
;se si
motori spenti
15
subwf
btfsc
goto
mod_e,0
status,cf
step3a
movlw d'88'
movwf ton
goto
finliva
step3a
movf
subwf
btfsc
goto
liv3a,0
mod_e,0
status,cf
step4a
movlw d'120'
movwf ton
goto
finliva
step4a
movf
subwf
btfsc
goto
liv4a,0
mod_e,0
status,cf
step5a
movlw d'170'
movwf ton
goto
finliva
step5a
movf
subwf
btfsc
goto
liv5a,0
mod_e,0
status,cf
step6a
;errore < livello2
;se si
ton2→ ton
;errore < livello3
;se si
ton3→ ton
;errore < livello4
;se si
ton4→ ton
;errore < livello5
ton5→ ton
movlw d'210'
movwf ton
goto
finliva
;se si
step6a
movlw d'255'
movwf ton
;se no motori al massimo
finliva
return
livellib
movf
subwf
btfsc
goto
liv1b,0
mod_e,0
status,cf
step2b
clrf
goto
ton
finlivb
movf
subwf
btfsc
goto
liv2b,0
mod_e,0
status,cf
step3b
step2b
movlw d'88'
movwf ton
goto
finlivb
step3b
movf
subwf
btfsc
goto
liv3b,0
mod_e,0
status,cf
step4b
;errore < livello1
;se si
motori spenti
;errore < livello2
;se si
ton2→ ton
;errore < livello3
16
movlw d'120'
movwf ton
goto
finlivb
step4b
movf
subwf
btfsc
goto
liv4b,0
mod_e,0
status,cf
step5b
movlw d'170'
movwf ton
goto
finlivb
step5b
movf
subwf
btfsc
goto
liv5b,0
mod_e,0
status,cf
step6b
;se si
ton3→ ton
;errore < livello4
;se si
ton4→ ton
;errore < livello5
ton5 → ton
movlw d'210'
movwf ton
goto
finlivb
;se si
step6b
movlw d'255'
movwf ton
;se no motori al massimo
finlivb
return
;CONVERSIONE ADC
adconv
btfss
goto
bsf
goto
clr
bcf
conv
bcf
bcf
call
bsf
btfsc
goto
return
loop
selreg,0
clr
adcon0,3
conv
adcon0,3
adcon0,4
adcon0,5
delay_20u
adcon0,2
adcon0,2
loop
;sceglie an0 o an1
;scelto an0
;scelto an1
;ritardo di 20usec per dar modo al
;condensatore del s/h di caricarsi
;inizio conversione
;aspetta fine conversione
;SOTTOPROGRAMMA DI RITARDO
delay_20u
movlw d'1'
movwf na
lpa
movlw d'1'
movwf nb
lpb
movlw d'32'
movwf nc
lpc
decfsz nc,1
goto
lpc
decfsz nb,1
goto
lpb
decfsz na,1
goto
lpa
return
END
.
17
 Datasheet
DATASHEET DRIVER L298
1/2
18
DATASHEET DRIVER L298
2/2
19
DATASHEET PIC16F876
1/2
20
DATASHEET PIC16F876
2/2
21
DATASHEET GP2D120
1/1
22