Operácie

Trojosí gyroskopický modul L3G4200D

Zo stránky SensorWiki

Autori: Ján Benedek
Martin Cintula
Michal Gáher
Študijný odbor: Aplikovaná mechatronika
Ročník: 2. Ing. (2014)

Zadanie

Naším zadaním bolo implementovať senzor L3G4200D - trojosí gyroskopický modul s mikropočítačom a získať z neho užitočné údaje. Gyroskop detekuje uhlovú rýchlosť natočenia vo všetkých troch osiach.

Senzor podporuje komunikáciu cez SPI a takisto aj cez I2C zbernicu. Pre náš projekt sme použili jednoduchšiu I2C zbernicu.

Následne sme vyčítané dáta mali zobraziť na PC vo virtualizačnom programe. Na tento účel poslúžil MATLAB.

Senzor je namontovaný na doske a predáva sa ako celok. Na doske sa nachádzajú prídavné obvody. Po privedení napájania a I2C komunikácie s pull-up rezistormi je zariadenie funkčné.


Literatúra:

Analýza

Senzor nám ponúka SPI a I2C zbernicu. Pre jednoduchosť sme použili I2C zbernicu. Čip podporuje protokol I2C, fast mode a takisto aj normal mode. Nemá žiadne špeciálne protokoly, preto je možné pre nás použiť I2C knižnicu od pána Flueriho.I2C

Casový diagram pre I2C:

Parametre senzora:

  • tri možnosti senzitivity
  • I2C/SPI zbernice
  • 16 bitový výstup dát
  • 8 bitový výstup teploty
  • Integrovaný dolno a horno priepustný filter
  • Power down and sleep mode

Pre našu aplikáciu nám nebude potrebné snímanie teploty. Pracovisko má stabilnú teplotu. Takisto nebudeme používať žiaden filter a využijeme obyčajný operation mode. Senzitivita je nastaviteľná a bude možné ju meniť.

Senzor je už z výroby nakalibrovaný, avšak pre našu aplikáciu v MATLABE budeme musieť urobiť našu vlastnú kalibráciu. Na určenie kalibračných konštánt potrebujeme systém, ktorý nám umožní senzor natáčať vo všetkých troch osiach v rámci 180 stupňov. Jednoduchý servo motor nám bude stačiť. Tu využijeme PWM moduláciu na riadenie servo motora (parallax servo motor).

Následne získane kalibračné hodnoty a uhlové rýchlosti nám pomôžu na určenie natočenia/uhla. Tu použijeme skript v MATLABE, ktorý nám bude integrovať v reálnom čase. Rovnice:

V prvom rade si treba uvedomiť, že integrácia je vlastne sumácia (ak Δt→0). Preto my budeme počítať uhly z uhlovej rýchlosti a sumovať tieto uhly, aby sme dostali pohyb senzora.

Rovnica na výpočet uhla: Je potrebné mať dve po sebe idúce dáta o uhlovej rýchlosti, tieto dve hodnoty sa spriemerujú a vynásobia časom Δt, ktorý ubehol medzi týmito dvoma dátami. Potom celú hodnotu prenásobíme kalibračnou konštantou a výsledok je, o aký uhol sa senzor pohol v danom čase (medzi dvoma čítaniami dát)

Piny na čipe:

My využijeme iba 5 z možných 8 pinov. Dva slúžia na napájanie, dva na I2C komunikáciu a posledný pin SD0 použijeme iba na zmenu adresy. Urobili sme to tak, lebo sme to videli v iných projektoch, ale čip by fungoval aj tak, len by sme museli v kóde zmeniť adresu.

Popis riešenia

Tento projekt rozložím do dvoch častí:

  • Zapojenie na vývojovej doske ACROB
  • Vizualizácia v MATLABE

Zapojenie čipu k mirkoprocesoru.

Ako som vyššie spomenul, pin SD0 je pripojený na Vcc a to zmení adresu čipu. Čip funguje aj bez SD0 pripojenia na Vcc, ale vtedy treba zmeniť adresu v kóde.

Na obrázku je jasne vidieť, ako máme pripojené 2 periférie. Servo motor nám slúži iba na kalibráciu. Senzor komunikuje cez I2C zbernicu a je potrebné k SDA a SCL pripojiť pull up rezistory, aby komunikácia fungovala správne.

Senzor je na začiatku potrebné nakonfigurovať. Nachádza sa tu 5 Control registerov, ktoré je potrebné nakonfigurovať podľa požiadaviek na čip.

  • CTRL_REG1 nastavuje zapnutie osí, a celkové zapnutie čipu
  • CTRL_REG2 nastavuje filtre v čipe (my nepoužívame)
  • CTRL_REG3 nastavuje prerušenia (tiež nepoužívame)
  • CTRL_REG4 nastavuje citlivosť/typ zápisu dát (endian/SPI enable)
  • CTRL_REG5 nastavuje prerušenia/filtre (nepoužívame)

Po úvodnej konfigurácií môžeme priamo pristupovať k dátam. Dáta sú uložené ako 16 bitové dáta v dvoch 8 bitových registroch. Tieto registre treba vyčítať a následne spojiť aby sme dostali finálnu hodnotu. Dáta sú zapísané ako dvojkový doplnok, takže nám dávajú aj negatívne hodnoty. Registre idú za sebou, preto nieje potrebné vkuse zapisovať adresu, z ktorej chcem čítať, ale akonáhle je prečítaný prvý bajt, môžeme pokračovať na ďalší a takto vieme vyčítať všetkých 6 8-bitových registrov a poskladať relevantné údaje. Adresa kde sa nachádzajú dáta začína 0xA8.

Čip priamo posiela údaje o uhlovej rýchlosti v jednotkách [dps]= degrees per second (stupeň za sekundu). Ďalšie spracovanie dát nastáva už v MATLABE. Dáta sú posielané po sériovej linke.

Vizualizácia

Celá vizualizácia sa deje v MATLABE. Najprv sme museli nadviazať komunikáciu s vývojovou doskou ACROB. Tak, ako sme používali serial connection cez program Terminal, takisto použijeme tento typ spojenia pre MATLAB. Aby sme sa uistili, že komunikácia je nadviazaná správne, používame tzv. handshake. Je to zrealizované tak, že ACROB posiela znak 'a' po sériovej linke a to v loope, takže pokým nenastane komunikácia s MATLABOM/iným sériovým komunikátorom, kód sa nepohne ďalej. Keď MATLAB prečíta znak 'a', pošle po sériovej linke späť znak '2'. Týmto si sú obe zariadenia isté, že komunikujú správne. MATLAB kód je tiež v loop, pokým neprijme znak 'a'.

Rozhodol som sa, že uhlovú rýchlosť budem integrovať a ukazovať natočenie šípky. Šípka bude vlastne kopírovať pohyb celého senzora. Aby sme dostali skutočnú polohu/natočenie, potrebujeme zistiť kalibračné konštanty. Toto je spravené cez servo motor. Servo motor točí celý senzor od 0 stupňov až po 180 konštantnou rýchlosťou a počas tohto pohybu neustále čítame dáta z čipu. Nakoľko je rýchlosť konštantná, dáta z čipu by mali dávať konštantnú uhlovú rýchlosť. Nakoľko vieme čas, ako dlho trvá dosiahnutie 180 stupňov, vieme aj o koľko stupňov sa senzor otočil a vieme aj uhlovú rýchlosť, potom je už jednoduché zistiť kalibračnú konštantu. Vzorec na výpočet kalibračnej konštanty je rovnaký ako hore uvedený vzorec na výpočet uhla. My vlastne vypočítame celkové premiestnenie/uhol a porovnáme to so 180 stupňami a z toho nám vyjde kalibračná konštanta.

Tento proces sa deje pre všetky tri osi, a v rámci získania presných hodnôt sa proces deje 5 krát a kalibračná konštanta je spriemerovaná. Po kalibrácii sa spustí vizuálne okno, kde je nakreslená šípka. Šípka môže byť reprezentovaná v 2D ale aj v 3D. 2D sa ukázalo byť viac prehľadné. Šípka sa nakláňa tak, ako senzor v 'x' a 'y' osi a natáča sa tak ako senzor v 'z' osi.

Ak sa zmení citlivosť senzora, je potrebné urobiť kalibráciu znova. Po získaní kalibračných konštánt, opätovná kalibrácia nie je potrebná.

V kóde sa nachádzajú dve spracovania dát, ak MATLAB pošle znak 'G', potom senzor posiela dáta normálne, ak však MATLAB pošle znak 'C', to znamená kalibráciu a kód riadi servo pohon a pošle dáta 50 krát. Pri znaku 'G', dáta sú poslané raz a treba neustále posielať znak 'G', aby sme čítali dáta.

Algoritmus a program

Tu popíšem kód, ktorý bol nahratý do mikropočítača. Kód použitý v MATLABE bude pripojený ako príloha, avšak je príliš rozsiahli na to aby som ho tu citoval.

Kód do mikroprocesoru bol robení v AVR Studio 4, v jazyku C. Vizualizácia bola robená v MATLABE v Matlabovskom kóde (MATLAB 2012).

Vývojový diagram na základe ktorého sme potom urobili kód:

Máme main, ktorý sa riadi vývojovým diagramom: 

/* A nezabudnite zdroják hojne komentovať  */
#include <avr/interrupt.h>  
#include <stdlib.h>
#include <avr/io.h>
#include <util/delay.h>
#include <util/twi.h>
#include <stdio.h>
#include "serial.h"  

//Definovanie citlivosti
#define L3G4200D_RANGE250      0x00
#define L3G4200D_RANGE500      0x10
#define L3G4200D_RANGE2000     0x30

 
void servo(int turn);


//pouzitie prikazov printf pre seriovu komunikaciu
FILE mystdout_Uart = FDEV_SETUP_STREAM(sendchar, NULL, _FDEV_SETUP_WRITE);
FILE mystdin_Uart = FDEV_SETUP_STREAM(NULL, recchar, _FDEV_SETUP_READ);


//inicializacne a pociatocne hodnoty a premenne
int16_t gxraw = 0;
int16_t gyraw = 0;
int16_t gzraw = 0;
int16_t range = L3G4200D_RANGE250;



int main(void)
{ 	

	//nastavenie portov pre servo
	DDRB   = 0x06;										//Nastavenie pinov PB1 a PB2
	PORTB = 0x01;										//Nastavenie výstupu na log. 0

	servo(100);										//otocenie servo do vychodzej polohy
	_delay_ms(100);										//pockanie na otocenie

	//inicializacia seriovej linky
	inituart();								
	stdout = &mystdout_Uart;				
	stdin = &mystdin_Uart;

	//inicializacia cipu
	l3g4200d_init(&range);
	_delay_ms(1500);


	//handshake s matlabom
	printf("a");
	int x = 0;
	while (x == 0)
	{
  		x=getchar(); 					//prijem charakteru po seriovej linke
	}



	//loop kde sa caka na charekter z matlabu a posielanie dat do matlabu
	while(1)
	{
		int mode=getchar();				//prijatie znaku z matlabu

		switch (mode)					//dve moznosti, bud kalibracia alebo posielanie dat
		{
			case 71:				//ASCI kod pre G -> posielanie dat
			l3g4200d_getrawdata(&gxraw, &gyraw, &gzraw);	//priajtie dat -> pozri chip.c
			printf("%d\n",gxraw);				//poslanie po seriovej linke
			printf("%d\n",gyraw);
			printf("%d\n",gzraw);
			break;

			case 67:				//ACSCI kod pre C -> kalibracia
			servo(542);				//otocenie serva o 180 stupnov
 			for (int i=1; i<=50; i++)		//zber dat pocas natacanie (50 vzoriek)
			{
				l3g4200d_getrawdata(&gxraw, &gyraw, &gzraw);	//priajtie dat -> pozri chip.c
				printf("%d\n",gxraw);		//poslanie po seriovej linke
				printf("%d\n",gyraw);
				printf("%d\n",gzraw);
			}
			_delay_ms(200);			
			servo(100);				//natocenie serva spat
			_delay_ms(200);
			break;
		}
	}
	return 0;
}


//servo funkcia na natacanie	
void servo(int turn)
{
    TCCR1A = 0b10100010;					//mod: PWM, Phase Correct, 9-bit
    TCCR1B = 0b00011011;					//delicka 1:1024 
    ICR1 = 0x1387;						//nastavenie topu/periody PWM - 20ms
    TCNT1 = 0x00;						//vynulovanie casovaca
    OCR1A = turn;						//nastavenie kedy ma togglovat
}

Potom máme vytvorený súbor chip.c kde sa nachádzajú dve funkcie, inicializácia čipu a čítanie dát z čipu: 

#include <stdlib.h>
#include <avr/io.h>
#include <util/delay.h>
#include "chip.h"
#include "i2cmaster.h"

//funckia na vycitanie dat
void l3g4200d_getrawdata(int16_t *gxraw, int16_t *gyraw, int16_t *gzraw) 
{
	uint8_t i = 0;	
	uint8_t buff[6];					//buffer pre 8 bitove data

	i2c_start(L3G4200D_ADDR | I2C_WRITE);			//start I2C + poslanie adresy cipa a R/W
	i2c_write(L3G4200D_OUT_X_L | (1 << 7));			//Definovanie adresy v cipe
	i2c_rep_start(L3G4200D_ADDR | I2C_READ);		//Vycitanie z cipu

	//loop pre vycitanie vsetkych dat zo vsetkych osi
	for(i=0; i<6; i++) {
		if(i==6-1)
			buff[i] = i2c_readNak();
		else
			buff[i] = i2c_readAck();
	}
	i2c_stop();						//stop I2C

	//zkompletizovanie 16 bitovych dat
	*gxraw = ((buff[1] << 8) | buff[0]);									
	*gyraw = ((buff[3] << 8) | buff[2]);
	*gzraw = ((buff[5] << 8) | buff[4]);
}

//inicializacna funckia
void l3g4200d_init(int16_t *range) {
	
	i2c_init();						//zapnutie I2C
	_delay_us(10);
	
	//enable chip
	i2c_start(L3G4200D_ADDR | I2C_WRITE);
	i2c_write(L3G4200D_CTRL_REG1);
	i2c_write(0x0F); 
	i2c_stop();

	//v podstate nepotrebne - len pre istotu
	i2c_start(L3G4200D_ADDR | I2C_WRITE);
	i2c_write(L3G4200D_CTRL_REG2);
	i2c_write(0x00);
        i2c_stop();

	//v podstate nepotrebne - len pre istotu
	i2c_start(L3G4200D_ADDR | I2C_WRITE);
	i2c_write(L3G4200D_CTRL_REG3);
	i2c_write(0x00);
	i2c_stop();

	//set range
	i2c_start(L3G4200D_ADDR | I2C_WRITE);
	i2c_write(L3G4200D_CTRL_REG4);
	i2c_write(*range);
	i2c_stop();

	//v podstate nepotrebne - len pre istotu
	i2c_start(L3G4200D_ADDR | I2C_WRITE);
	i2c_write(L3G4200D_CTRL_REG5);
	i2c_write(0x00);
	i2c_stop();	
}

Ďalšie časti kódu sú už len I2C knižnica, a knižnica pre sériovú komunikáciu.

Pre ilustráciu uvediem Matlabovský kód na spustenie sériovej linky s čipom a takisto aj pre vyčítanie dát z ACROB-u:

setupSerial.m => 

function [s,flag] = setupSerial(comPort)

flag = 1;
s = serial(comPort);
set(s,'DataBits', 8 );
set(s,'StopBits', 1 );
set(s,'BaudRate', 9600 );
set(s,'Parity', 'none');
fopen(s);
a='b';
while (a ~='a')
    a=fread(s,1,'uchar');
end
if (a=='a')
    disp('serial read');
end
fprintf(s,'%d',2);
mbox = msgbox('Serial Communication setup.'); uiwait(mbox);
end

Funkcia setupSerial je zavolaná len raz na začiatku.

readGyro.m => 

function [gx, gy, gz] = readGyro(gyroConnection, isCalibration)

    if(~isCalibration)
        fprintf(gyroConnection.s,'G');
    end

    gx = fscanf(gyroConnection.s,'%d');
    gy = fscanf(gyroConnection.s,'%d');
    gz = fscanf(gyroConnection.s,'%d');
end

V tejto readGyro funkcii posielame znak 'G' do čipu a čítame dáta z čipu. V ďalších skriptoch potom už len integrujeme a reprezentujeme to na šípke. Ostatné kódy sú v prílohe.

Zdrojový kód: main.c,chip.c,serial.c,I2C.c,chip.h,serial.h a I2C.h

Matlab kód: Vizualizacia.m,Kalibracia.m,Vypocet_uhlov.m,Vycitaj_data.m,CloseSerial.m a setupSerial.m

Overenie

Pre spustenie celého experimentu potrebujete mať pripojenú dosku ACROb k PC, otvorený MATLAB s m.filom, GyroVis.m a pripevnený senzor na servo motore, najskôr v x osi.

Než spustíte sériovú linku, overte si na ktorom COM porte sa nachádza ACROB doska. Ak je to iný port než COM=5, tak to zmeňte v GyroVis.m. Potom môžete spustiť GyroVis.m. Počkajte pokým vám nevyhodí okno so Serial Communication Setup. Kliknite na OK. Toto znamená, že MATLAB je spojený s ACROB doskou a môže začať kalibrácia. Ako som už spomenul, treba mať senzor upevnený na servo motore, v smere osi x. MATLAB vám oznámi, že sa začína kalibrácia v x osi. Stlačte OK a servo motor sa začne točiť. Uistite sa, že motor je fixovaný, aby prípadný pohyb motora nespôsobil chybný výpočet. Keď skončí kalibrácia v osi x, MATLAB vám oznámi, že začína kalibrácia v osi y. Upevnite najprv senzor aby sa točil v osi y a potom stlačte OK. To isté sa opakuje pre os z.

Po úspešnej kalibrácii vám MATLAB napíše, že kalibrácia bola úspešná. Kliknite na OK. V hlavnom command window vám vypíše kalibračné konštanty. Teraz je vizualizácia prístupná. Môžete si vybrať, či chcete, aby sa šípka hýbala vo všetkých osiach alebo len v niektorých. Nakoľko senzor iba počíta uhly, nevie v akej pozícii sa nachádza na začiatku. Preto je dobré chytiť senzor a dať ho do vodorovnej polohy a kliknúť na reset. To vynuluje uhly na nulu, teda začiatočnú pozíciu a potom už šípka opisuje pohyb senzora.

Keď chcete skončiť, kliknite na Stop/Close serial Port. Je to potrebné, aby sa ukončila sériová komunikácia s čipom.

Pre ukážku ako senzor naozaj funuguje, pozrite si nasledujúce video: Ukážka

Fotogaléria






Zdroj: „https://senzor.robotika.sk/sensorwiki/index.php?title=Trojosí_gyroskopický_modul_L3G4200D&oldid=5476“