• Home
• Robot
• Progetti
• Programmazione
• Info

FIRE – Aggiornamenti

Continua il lavoro sul nuovo Robot FIRE, lo sviluppo si è spostato ora sulla parte di controllo motori, che utilizzerà come encoder i sensori e i dischi dentati recuperati da un vecchio mouse, il cui principio di funzionamento è molto simile a quello che serve per il nostro scopo.

Il circuito è molto semplice, da una parte abbiamo un trasmettitore IR e dall’altra due ricevitori posti ad una certa distanza tra loro, ma contenuti nello stesso package, va da se che messi il TX e l’RX uno davanti all’altro i due ricevitori daranno in uscita due segnali a livello alto (i sensori usati hanno un’uscita TTL), mentre interponendo qualcosa tra i due sensori le uscite sono a livello basso. Ora se consideriamo che il disco dentato, ha appunto delle zone aperte, dove i raggi IR possono passare tranquillamente, e delle zone chiuse dove i raggi non passano, va da se che le uscite del ricevitore, se il disco gira, alterneranno valori alto e basso generando così un treno di onde quadre. Nelle figure che seguono (ingrandirle per vederle meglio) si possono vedere, le differenze dei segnali in uscita dai due ricevitori, che in seguito chiameremo canali, e si può notare la differenza nel canale B (rosso) quando il motore gira in un verso o nell’altro.

Questa differenza tra i due canali è utile per stabilire il verso di rotazione del motore (faccio presente che non utilizzando un encoder commerciale e quindi con caratteristiche specifiche, lo sfasamento tra i due canali può cambiare a seconda di come questi vengono montati), infatti se ipotiziamo di controllare il livello dei due segnali in seguito al fronte di salita (evidenziati dalla freccia) del canale A (blu), con il motore che gira in un verso avremo sempre le uscite dei due canali sono a livello alto (figura 1), ma se invece giriamo il motore nel verso opposto le cose cambiano, il canale B sarà sempre al livello opposto a quello di del canale A, sarà quindi sufficiente fare un controllo dei due livelli per determinare il verso di rotazione del motore.

Ma passiamo dalla teoria alla pratica, quelle che vedete di seguito sono le foto di dettaglio della struttura definitiva dei motori, come si può vedere è stata aggiunta un’altra staffa ad L per aumentare la stabilita dell’asse che sostiene il disco del encoder.

Il problema principale, della realizzazione hardware di questi encoder, è stato trovare la giusta posizione dei sensori rispetto al disco dentato. Essendo una soluzione “home made”, non abbiamo studiato più di tanto sulla capacità di replicabilità, ma ci siamo diretti più sulla funzionalità, pensando che stiamo comunque lavorando su un progetto il cui scopo finale è quello di studiare/analizzare nuove problematiche oltre naturalmente a trovare soluzioni per i nostri scopi. Solo come nota a margine, nel cassetto si sono già degli ottimi motoriduttori con encoder “serio” integrato che aspettano di essere usati.

Veniamo ora alla parte elettronica per la gestione degli encoder, per prima cosa per il controllo motori viene usato un PIC18F4431, montato momentaneamente su una Roboboard 2.0, probabilmente successivamente passeremo a realizzare una scheda apposita e con il più piccolo 18F2431, le cui uniche differenze sono nel package da 28 pin anziché 40 con il conseguente minore numero di porte, ma quelle disponibili sono sufficienti per i nostri scopi.

Come si può vedere dallo schema, della futura scheda con 18F2431, i canali A e B di entrambi gli encoder sono collegati alle porte KBI1, KBI2, KBI3 e KBI4, questi ingressi permettono di configurare un interrupt “on change”. Questa serie di PIC (18FXX31) ha tra le sue periferiche, quelle per la gestione del PWM avanzato, che mettono a disposizione per ogni PWM generato due segnali in opposizione di fase tra loro, questo semplifica ulteriormente il circuito, perché per pilotare il PonteH non è necessario aggiungere nessun altro integrato per invertire i segnali. RC0 viene utilizzato per abilitare/disabilitare il PonteH, al solo scopo di ridurre i consumi durante le fasi di standby, ricordo che i motori vengono controllati in modalità LAP, quindi per mantenerli fermi è necessario comunque che all’interno dei motori scorra corrente, invece disabilitando il ponte i motori non sono frenati ma i consumi vengono ridotti, cosa molto importante se si considera che il robot è alimentato a batterie.

Passiamo ora ad analizzare il codice, in particolare per prima cosa vediamo la parte più importante ovvero l’ISR (Interrupt Service Routine).  In questa prima bozza del codice il PIC conta il numero di click dell’encoder di ogni ruota, misura il periodo dell’onda quadra generata dal canale B degli encoder e determina il verso di rotazione dei motori.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96

void InterruptHandlerHigh (void)
{
	if(INTCONbits.RBIF==1)
	{
		// Controlla quale canale di quale encoder ha scatenanto l'interrupt
		// ed aggoirna in contatore di impulsi associato
		if(ENC_A_SX!=encAsxOldStatus)
		{
			clickSX++;
			encAsxOldStatus=ENC_A_SX;
			// Se il valore della variabile è 1 allora c'è stato un fronte di
			// salita, quindi posso leggere il timer per misurare il periodo
			if(ENC_A_SX==1)
			{
				// Cattura il valore attuale del Timer, leggo prima la parte
				// bassa per evitare che vari ulteriormente la lettura.
				tmr5_now_sx.byteL=TMR5L;
				tmr5_now_sx.byteH=TMR5H;
				// Salvo il numero di overflow e azzero il contatore
				tmr5_ovf_value_sx=tmr5_overflow_sx;
				tmr5_overflow_sx=0;
				// Imposto il flag di controllo per il calcolo della velocità
				flag_calcola_speed_sx=1;
			}
		}
		if(ENC_B_SX!=encBsxOldStatus)
		{
			clickSX++;
			encBsxOldStatus=ENC_B_SX;
			// Determina il verso di rotazione
			if(ENC_A_SX==1 && ENC_B_SX==1)
			{
				Rdir_mot_sx=1;	//Forward
			}
			else if(ENC_A_SX==0 && ENC_B_SX==1)
			{
				Rdir_mot_sx=0;
			}
			//*******************
			LedVerde=!LedVerde;
			//*******************
		}
		if(ENC_A_DX!=encAdxOldStatus)
		{
			clickDX++;
			encAdxOldStatus=ENC_A_DX;
			// Se il valore della variabile è 1 allora c'è stato un fronte di
			// salita, quindi posso leggere il timer per misurare il periodo
			if(ENC_A_DX==1)
			{
				// Cattura il valore attuale del Timer, leggo prima la parte
				// bassa per evitare che vari ulteriormente la lettura.
				tmr5_now_dx.byteL=TMR5L;
				tmr5_now_dx.byteH=TMR5H;
				// Salvo il numero di overflow e azzero il contatore
				tmr5_ovf_value_dx=tmr5_overflow_dx;
				tmr5_overflow_dx=0;
				// Imposto il flag di controllo per il calcolo della velocità
				flag_calcola_speed_dx=1;
			}
		}
		if(ENC_B_DX!=encBdxOldStatus)
		{
			clickDX++;
			encBdxOldStatus=ENC_B_DX;
			// Determina il verso di rotazione
			//*!ATTENZIONE!Questo encoder è momntato nello stesso verso
			//* dell'altro, quindi la logica di controllo va invertita
			if(ENC_A_DX==1 && ENC_B_DX==1)
			{
				Rdir_mot_dx=0;	// Backward
			}
			else if(ENC_A_DX==0 && ENC_B_DX==1)
			{
				Rdir_mot_dx=1;	// Forward
			}
			//*******************
			LedRosso=!LedRosso;
			//*******************
		}
		INTCONbits.RBIF=0;
	}
 
	if(PIR3bits.TMR5IF==1)
	{
		//*********************
		LedGiallo=!LedGiallo;
		//*********************
		// Incrementa i contatori di overflow
		tmr5_overflow_dx++;
		tmr5_overflow_sx++;
		PIR3bits.TMR5IF=0;
	}
 
	return;
}

I primi controlli che vengono fatti nella ISR, sono gli “if” visibili alla riga 3 e alla riga 84, questi controlli sui flag di interrupt vengono usati per stabilire quale periferica l’ha scatenato.  Prendiamo in esame l’Interrupt Flag del Timer 5, questo viene generato ogni volta che il registro del contatore del timer va in owerflow, quindi ogni volta che avviene questo evento vengono incrementate le due variabili di controllo “tmr5overflow_xx”, che verranno poi utilizzate in un’altra funzione per misurare la durata del periodo. Alla riga 82 viene fatta un’operazione importantissima ovvero il reset del flag dell’Interrupt del Timer5, questa operazione deve essere fatta manualmente al termine della gestione dell’ISR, altrimenti appena usciti dalla routine questa verrà richiamata immediatamente come se fosse stato generato un nuovo interrupt.
L’altro flag RBIF, viene posto a livello alto quando uno degli ingressi delle porte KBIx cambia di stato, a queste porte come già visto sono collegati i canali dei due encoder, quindi dopo il controllo della riga 3 vengono fatti altri quattro controlli, uno per ogni canale degli encoder, dove viene verificato se il livello logico attuale dell’encoder è diverso da quello memorizzato l’ultima volta che c’è stato un cambiamento (righe 7, 26, 43, 62).
Prendiamo in analisi la porzione di codice che segue l’istruzione “if” di riga 7, alla riga 9 viene incrementata la variabile “clickSX” che viene utilizzata per contare il numero di click totale del motore sinistro, adesso questa informazione non è utilizzata, ma servirà poi per la parte di odometria che svilupperemo in seguito. Nella riga successiva (10) viene aggiornata la variabile encAsxOldStatus con il valore attuale del livello dell’encoder.
Nella riga 13 troviamo un altro “if” in questo caso controlla se il livello attuale del canale A dell’encoder in esame è a livello alto, questo permette di stabilire se quello che stiamo analizzando è stato un cambio di stato dal livello logico basso ad alto, quindi se è seguito ad un fronte di salita. In questo modo il codice che segue viene eseguito solo a seguito di ogni fronte di salita, e quindi ad ogni periodo delle onde quadre generate dall’encoder, questo ulteriore controllo serve appunto per effettuare la misurazione della durata del periodo, quindi a seguito di ogni fronte di salita viene salvato il valore del Timer5 e il numero di volte che questo è andato in overflow nelle apposite variabili di appoggio, viene impostata ad uno la variabile “flag_calcola_speed_sx”, che viene utilizzata nella funzione “misura_velocità”, che vedremo in seguito, per stabilire se è il momento di fare i conti per misurare la durata del periodo.
Alla riga 26 viene controllato se il cambio di stato è avvenuto sul canale B dell’encoder collegato al motore sinistro, le istruzioni nelle due righe che seguono sono equivalenti a quelle già viste per le righe 9 e 10. Le righe che seguono invece servono a stabilire il verso di rotazione del motore, come già indicato nei grafici visti in precedenza.
La parte di codice che segue (dalla riga 43 alla 80) è in pratica la replica dello stesso codice, solo che questa volta è relativo al motore destro e presenta un’unica differenza nella parte in cui viene stabilito il verso di rotazione, questo perché trattandosi di encoder home made non sono stati fatti per girare entrambi allo stesso modo. Infine alla riga 81 viene azzerato il flag dell’interrupt, per renderlo disponibile ad essere attivato al prossimo cambio di stato di uno dei quattro pin.

Il codice che segue riporta la funzione utilizzata per calcolare la durata del periodo di un’onda quadra, misurato come abbiamo visto in precedenza sul canale B dei due encoder.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51

void misura_velocita(void)
{
	unsigned char icp=0;
 
	// Si è concluso un periodo intero per il canale A del motore SX
	if(flag_calcola_speed_sx==1)
	{
		// Memorizzo nella variabile unsigned int i due registri H e L del TMR5
		tmr5_now_sx.Word=((unsigned int)tmr5_now_sx.byteH<<8)+tmr5_now_sx.byteL;
 		// Controlla se ci sono stati un overflow del timer durante il conteggio,
 		// per stabilire come calcolare il valore del periodo.
 		if(tmr5_ovf_value_sx>0)
		{
			lcd_put_uchar(tmr5_ovf_value_sx,3,5);
			// Se c'è stato overflow la durata del periodo è calolata sottraendo
			// al valore massimo del TMR5 il valore della lettura precedente,
			// aggiungendo per ogni altro overflow-1 il valore massimo di TMR5 e
			// aggiungendo in fine il valore attuale sempre di TMR5+1.
			durata_periodo_sx=((unsigned short long)(0xFFFF*(tmr5_ovf_value_sx-1)))+(0xFFFF-tmr5_old_sx)+tmr5_now_sx.Word+1;
			// Imposto il valore attuale del timer come old per il prossimo calcolo
			tmr5_old_sx=tmr5_now_sx.Word;
		}
		else
		{
			// Se non c'è stato overflow la durata del periodo è la differenza
			// tra la lettura precedente e quella attuale.
			durata_periodo_sx=tmr5_now_sx.Word-tmr5_old_sx;
			// Imposto il valore attuale del timer come old per il prossimo calcolo
			tmr5_old_sx=tmr5_now_sx.Word;
		}
		flag_calcola_speed_sx=0;
	}
	// Si è concluso un periodo intero per il canale A del motore DX
	if(flag_calcola_speed_dx==1)
	{
		// Per i commenti vedi la sezione speculare sopra
		tmr5_now_dx.Word=((unsigned int)tmr5_now_dx.byteH<<8)+tmr5_now_dx.byteL;
		if(tmr5_ovf_value_dx>0)
		{
			lcd_put_uchar(tmr5_ovf_value_dx,3,17);
			durata_periodo_dx=((unsigned short long)(0xFFFF*(tmr5_ovf_value_dx-1)))+(0xFFFF-tmr5_old_dx)+tmr5_now_dx.Word+1;
			tmr5_old_dx=tmr5_now_dx.Word;
		}
		else
		{
			durata_periodo_dx=tmr5_now_dx.Word-tmr5_old_dx;
			tmr5_old_dx=tmr5_now_dx.Word;
		}
		flag_calcola_speed_dx=0;
	}
}

Il primo controllo che viene fatto  sulle variabili “flag_calcola_speed_xx”, già viste nella gestione della ISR, per stabilire se ci sono i dati necessari per il calcolo.  Per prima cosa alla riga 9 vengono memorizzati i valori presi dai due registri a 8 bit che compongono il contatore del Timer5, in una variabile a 16 bit da utilizzare per i calcoli. Alla riga 12 viene controllato se durante la durata del periodo dell’onda sul canale B c’è stato uno o più overflow del registro del Timer5, nel caso questo sia vero viene applicata la formula nella riga 19 per calcolare la durata del periodo.  Il calcolo risulta molto più semplice quando non c’è stato overflow, in questo caso è sufficiente sottrarre al valore attuale del Timer quello della misura precedente. La misura per l’altro motore funziona allo stesso modo, in entrambi i casi è necessario ricordarsi di azzerare la variabile flag_calcola_speed_xx per evitare letture sbagliate.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

while(1)
{
	// Mostra il numero di Click per ruota
	lcd_put_uint(clickSX,2,3);
	lcd_put_uint(clickDX,2,15);
	// Esegue la misura della velocità sui due motori
	misura_velocita();
	// Mostra il valore del periodo calcolato
	lcd_put_uslong(durata_periodo_sx,1,0);
	lcd_put_uslong(durata_periodo_dx,1,12);
	// Mosta la direzione di rotazione dei motori impostata e quella verificata tramite gli encoder
	...
	...
	...
}

Questo box contiene una parte del codice eseguito nel loop principale presente nel main, come si può facilmente intuire vengono eseguite le istruzioni per visualizzare sul display le informazioni dei motori visualizzando i dati impostati e quelli misurati. Nella riga 7 viene richiamata la funzione “misura_velocita()” vista in precedenza.

Le informazioni sul display (20×4) sono divise in due colonne destra e sinistra, dove vengono visualizzate le informazioni dei rispettivi motori destro e sinistro. La prima riga indica il valore impostato nel registro del PWM e la direzione impostata per i motori, la seconda invece indica il periodo misurato tramite il Timer5, la terza indica il numero di click letti dall’encoder e l’ultima riga indica quante volte è andato in overflow il registro del Timer5.

Concludo il lungo articolo dicendo che ora abbiamo tutte le informazioni necessarie per poter implementare un controllo PID sui motori e per poi passare alla fase di controllo dell’odometria.

Stay Tuned!

(Ancora nessun voto)

 Loading ...

Ti è piaciuto questo articolo? Perché non lasci un commento e continui la discussione, oppure iscriviti ai feed e riceverai gli articoli sul tuo feed reader.