Progetto di un sistema di controllo per un robot basato su CAN bus

4. Il Progetto 44
ha a disposizione cinque nodi con stessa priorit a (gli Slave) e uno con priorit a maggiore
degli altri (il Master), ne deriva che il settaggio pi u semplice  e quello in tabella 1.
La priorit a viene indicata con un numero da 1 a 5, dove 5 rappresenta la massima e 1 la
minima. Questo,  e bene ricordarlo, viene considerato solo in fase di concorrenza nel bus.
L’ID del Master non  e indipendente dal messaggio, come per i Slave, ma bens  dipende
dal destinatario, poich e  e necessario mandare il Set-Point allo specico Slave. Per questa
ragione il Master deve avere un identicatore apposito, che sar a successivamente ltrato
dallo Slave destinatario. Si arriva quindi alla tabella 2:
Nodo ID Priorit a
Master
all
000 0000 0001 6
Master
1
000 0000 0010 5
Master
2
000 0000 0100 4
Master
3
000 0000 1000 3
Master
4
000 0001 0000 2
Master
5
000 0010 0000 1
Tabella 4.2: Standard ID del Master
Master
all
 e l’ID che bisogna usare per trasmettere lo stesso messaggio a tutti gli altri
nodi, in questo caso l’Arresto del motore. Anche in tale situazione vi  e una priorit a
intrinseca. Infatti il messaggio contenuto con il Master
all
, per come  e l’ID ha priorit a
maggiore (si ricorda che lo ’0’  e dominante). E’ necessario quindi, nella programma-
zione del rmware del dsPIC, creare delle speciche funzioni che permettano l’invio dei
Set-Point, passandogli come parametro il messaggio di 2 byte. Per quanto riguarda le
maschere ed i ltri del nodo Master, si  e scelto di impostarli in questo modo:
Maschera/Filtro ID
Maschera 0 100 0000 0000
Filtro 0 100 0000 0000
Filtro 1 100 0000 0000
Maschera 1 100 0000 0000
Filtro 2 100 0000 0000
Filtro 3 100 0000 0000
Filtro 4 100 0000 0000
Filtro 5 100 0000 0000
Tabella 4.3: Maschere e ltri del Master
Tramite il primo bit 1 dell’identicatore si ltrano automaticamente tutti gli ID degli
Slave, in questa maniera si ricevono tutti i messaggi destinati al Master.
Per gli altri nodi invece la situazione  e dierente. Ogni Slave deve poter ricevere il pro-
prio messaggio di Set-Point, e per questo viene settato un ltro in modo tale da ricevere
il Data Frame proveniente dal Master. Inoltre deve anche ricevere il messaggio di Arre-
sto, in tale situazione l’ID  e lo stesso per tutti i nodi, quindi anche il ltro apposito.
Le maschere devono essere congurate di conseguenza per la gestione opportuna dei l-
4. Il Progetto 45
Slave Maschera/Filtro ID
Slave 1 Maschera 0 100 0011 1111
Filtro 0 000 0000 0001
Filtro 1 000 0000 0010
Maschera 1 100 0011 1111
Filtro 2 000 0000 0001
Filtro 3 000 0000 0010
Filtro 4 000 0000 0010
Filtro 5 000 0000 0010
Slave 2 Maschera 0 100 0011 1111
Filtro 0 000 0000 0001
Filtro 1 000 0000 0100
Maschera 1 100 0011 1111
Filtro 2 000 0000 0001
Filtro 3 000 0000 0100
Filtro 4 000 0000 0100
Filtro 5 000 0000 0100
Slave 3 Maschera 0 100 0011 1111
Filtro 0 000 0000 0001
Filtro 1 000 0000 1000
Maschera 1 100 0011 1111
Filtro 2 000 0000 0001
Filtro 3 000 0000 1000
Filtro 4 000 0000 1000
Filtro 5 000 0000 1000
Slave 4 Maschera 0 100 0011 1111
Filtro 0 000 0000 0001
Filtro 1 000 0001 0000
Maschera 1 100 0011 1111
Filtro 2 000 0000 0001
Filtro 3 000 0001 0000
Filtro 4 000 0001 0000
Filtro 5 000 0001 0000
Slave 5 Maschera 0 100 0011 1111
Filtro 0 000 0000 0001
Filtro 1 000 0010 0000
Maschera 1 100 0011 1111
Filtro 2 000 0000 0001
Filtro 3 000 0010 0000
Filtro 4 000 0010 0000
Filtro 5 000 0010 0000
Tabella 4.4: Maschere e ltri degli Slave
4. Il Progetto 46
traggi. La congurazione  e ad ogni modo riassunta in tabella 4. Lo schema dei possibili
messaggi e dei relativi ID  e schematizzato in tabella 5.
Nodo ID Messaggio Byte
Master 000 00xx xxx0 Set-Point 2
Master 000 0000 0001 Arresto Motori 2
Slave
x
10x xx00 0000 Misura 2
Tabella 4.5: Tipologie di messaggi trasmessi
Il Master come detto pu o inviare due messaggi: l’arresto di tutti motori, spedendo a
tutti i nodi un messaggio da 2 byte (11111111 11111111) con ID 000 0000 0001, oppure
pu o inviare il Set-Point allo specico Slave. Gli Slave invece possono solo spedire le
Misure, ottenute tramite il modulo di controllo del motore.
Nel caso pi u semplice, in cui nella rete  e presente un solo Slave  e pi u logico gestire
tutto con un solo identicatore per il Master, poich e non vi  e necessit a di distinguere
i messaggi. Lo Slave infatti riceve ogni Frame, e in base al contenuto del campo Data
reagisce di conseguenza.
4.5 Il QEI
Nella robotica, ma non solo,  e spesso necessario realizzare la lettura di un encoder
incrementale che normalmente  e eettuata collegandolo ad un ingresso di un micro-
controllore dotato di interrupt on-change e implementando via software la decodica 2X
o 4X e la rilevazione del verso di rotazione. La scrittura del codice, specie nella decodica
4X non  e certo di semplice realizzazione. Invece, impiegando un dsPIC, a fare tutto ci
pensa il Quadrature Encoder Interface: una periferica direttamente integrata [6].
Figura 4.4: Funzionamento QEI (2X)
Il QEI lavora direttamente in hardware, ci o signica che oltre a non scrivere codice,
non sono impegnate risorse della CPU, che rimangono a disposizione per altre operazio-
ni. Inoltre, nella decodica fatta in maniera classica gli interrupt, nel caso di encoder ad
4. Il Progetto 47
alta risoluzione, sono scatenati a frequenze elevate e a seconda dei MIPS a disposizione,
diviene problematico riuscire a gestire la decodica assieme alle altre operazioni che la
MCU deve compiere. Questo vuol dire che se non si ha a disposizione il QEI, a parit a
di applicazione  e necessaria una MCU pi u potente e un programmatore pi u esperto. Vo-
lendo dunque acquisire la posizione e la direzione della rotazione del motore, si procede
con l’identicazione di alcuni parametri, descritti nella gura 4.
I canali QEA e QEB, corrispondenti ai relativi pin del dsPIC, sono pilotati dalla di-
rezione di rotazione del motore. Infatti se il canale A precede B la direzione  e quella
di normale rotazione, positiva; nel caso duale, la direzione  e opposta, negativa ( UPDN
rispecchia la situazione, assume valore logico alto o basso a seconda della direzione del
motore). Ad ogni clock (count clock) viene aggiornato il contatore della posizione a 16
bit POSCNT di una unit a. Questo campionamento pu o essere impostato a 2X o 4X
rispetto al periodo del segnale in quadratura presente nei canali A e B. Ci o si fa settando
il registro QEICONbits.QEIM. In questo casi si  e scelto di utilizzare una risoluzione
a 2X, per non rischiare di andare in overow [19].
La frequenza dei segnali in quadratura (f
q
) dipende dalla velocit a del motore utilizzato
(v
m
) e dalla frequenza dell’encoder (f
e
), in questo caso:
f
q
GM8
=
v
m
60
2f
e
=
7000rpm
60
2(5760) 1:4MHz
f
q
GM9
=
v
m
60
2f
e
=
5000rpm
60
2(7620) 1:3MHz
E’ possibile calcolare
8
i massimi campioni per giro:
MAX COUNT PER REV
GM8
= 2f
e
 1 = 2 5760 1 = 11519
MAX COUNT PER REV
GM9
= 2f
e
 1 = 2 7620 1 = 15239
La risoluzione quindi  e:
Resolution
GM8
=
360
o
11520
= 0:031
o
Resolution
GM9
=
360
o
15240
= 0:0236
o
Questa  e necessaria per convertire i valori contenuti nella variabile POSCNT in numeri
frazionari da poter poi utilizzare per il controllo. Ne deriva che:
AngPos[0]
GM8
=POSCNT
32768
11520
AngPos[0]
GM9
=POSCNT
32768
15240
8
Si riportano i calcoli per entrambe le tipologie di motori, le serie GM8 e GM9. Per distinguere le
variabili si riporta come pedice il motore a cui si riferiscono.
4. Il Progetto 48
dove AngPos non  e altro che un array contenente la copia convertita di POSCNT in
numero frazionario. La tabella successiva riassume il tutto:
Variabile Angolare Intera Frazionaria Risoluzione
AngPos[0]
GM8
[0
o
; 357:1
o
] [0; 32765] [0:0; 0:9999] 0:031
o
AngPos[0]
GM9
[0
o
; 359:64
o
] [0; 32766] [0:0; 0:9999] 0:0236
o
Tabella 4.6: Valori della variabile AngPos
Per contare invece il numero di giri si utilizza il registro MAXCNT e un contatore
lap count. La procedura  e la seguente:
1. Si inizializza MAXCNT con MAX COUNT PER REV, in modo tale da coprire
tutto il giro dell’encoder.
2. Ogni volta che si arriva a questo valore tramite POSCNT vuol dire che  e stato
percorso un giro del motore, e lap count pu o essere aumentato di uno.
Per conoscere invece la direzione del motore si sfrutta l’interrupt previsto per il
modulo QEI ( QEIInterrupt). In gura 5 viene riassunta la procedura.
Figura 4.5: Utilizzo di MAXCNT (4X)
Il registro POSCNT viene resettato a 0 nelle seguenti situazioni (in modo da capire
posizione e direzione di percorrenza):
 Nel caso in cui l’encoder viaggi nella direzione positiva (avanti, canale A precede
B) e il valore di POSCNT aumenta no ad arrivare a MAXCNT. Al prossimo
impulso che incrementa il contatore di posizione, verr a impostato a 0. Un interrupt
viene generato.
4. Il Progetto 49
 Se l’encoder viaggia in direzione contraria (indietro, canale B precede A) e il va-
lore di POSCNT  e gi a 0. Al successivo impulso che decrementa il contatore di
posizione, verr a impostato uguale a MAXCNT. Un interrupt viene generato.
Tramite questi comportamenti  e possibile dunque conoscere la direzione che viene
presa dal motore. La variabile direction tiene conto proprio di questo; il valore logico
0 corrisponde alla prima situazione, 1 all’altra.
4.6 Il Regolatore
Per questo progetto si  e scelto un regolatore proporzionale, senza applicare tutta la teoria
dei PID. La formula usata per il controllo quindi  e:
u =K
p
e
dove u  e il segnale che appunto verr a usato poi per il PWM (il segnale di controllo),
e  e l’errore di posizione, cio e la dierenza tra la posizione desiderata e quella misurata
dall’encoder, secondo la notazione dello schema di controllo di gura 1, capitolo 3:
e =y
des
 y
mis
mentre K
p
 e molto semplicemente una costante proporzionale adatta allo scopo del
controllo. Tutto questo  e implementato via software nel rmware del micro-controllore,
senza particolari accorgimenti, nel main degli Slave.
4.7 Il PWM
Per comandare il Ponte H descritto in precedenza si fa uso del modulo PWM. A livello
software si ha la necessit a di impostare alcuni parametri, utili al micro-controllore per
gestire il modulo di potenza, come la frequenza del dente di sega, il duty cycle...
L’asse dei tempi  e rappresentato da un timer a 15 bit, PTMR, con la possibilit a di
avere un prescaler e un postscaler. Questo registro, e quindi il tempo, si pu o disabilitare
attraverso il bit PTCONbits.PTEN, ma anche congurare in quattro diverse modalit a:
1. Free Running Mode (Treno di dente di sega)
2. Single Event Mode (Unico dente di sega)
3. Continuous Up/Down Count Mode (Segnale triangolare)
4. Continuous Up/Down Count Mode with interrupts for double-updates
4. Il Progetto 50
In questa sede si utilizza solamente la prima modalit a ( PTCONbits.PTMOD=0),
per questo le altre non verranno analizzate. In tale utilizzo la base dei tempi scorre in
avanti no ad arrivare al punto in cui corrisponde al valore del registro PTPER.
PTMR viene resettato a 0 al successivo dente di sega e l’asse dei tempi continua a
scorrere in avanti no a che il bit PTCONbits.PTEN non viene resettato ad 0. Il
valore di PTPER viene automaticamente caricato quando l’asse dei tempi del PWM  e
disabilitato (PTCONbits.PTEN=0), la situazione viene rappresentata in gura 6.
Figura 4.6: Aggiornamento del periodo di PWM
E’ fondamentale quindi il calcolo della frequenza del PWM:
PTPER =
F
cy
F
PWM
 PTMR
prescaler
 1 =
10000000
19500
 1 511
doveF
PWM
pu o assumere valori no a 22 kHz di norma. La risoluzione in bit, data dal-
l’oscillatore (F
cy
) e dalla frequenza del PWM (F
PWM
), si determina tramite la seguente
formula:
Resolution =
log(
2T
PWM
Tcy
)
log 2
 log(
0:0001
0:0000001
)
log 2
=
log 1000
log 2
=
3
0:3
= 10bit
Nel dsPIC utilizzato  e presente un generatore di duty cycle per ogni canale PWM,
in particolare i registri sono PDC1, PDC2...
Nel caso in cui il duty cycle sia diverso da zero e gli aggiornamenti non siano istanta-
nei (IUE=0), l’uscita del generatore di PWM relativo si allinea all’inizio del periodo
(PTMR=0). Ogni canale  e invece inattivo quando il valore di PTMR corrisponde al
duty cycle.
Nel caso in cui invece un PDCx sia nullo, l’uscita del corrispondente pin di PWM  e
inattiva per l’intero periodo. In aggiunta l’output  e attivo per l’intero periodo se il duty
cycle  e maggiore del valore in PTPER.