Tropjosí gyroskopický modul L3G4200D

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 maltab.

Senzor je namontovaný na doske a predáva sa to ako celok. Na doske sa nachádzajú nejake obvody a čipy. Výsledok je, že nám stačí priviesť napájanie a I2C komunikáciu s pull up rezistormi a celé to potom funguje.

Literatúra:

• Zoznam použitej literatúry, vrátane katalógových údajov (datasheet), internetových odkazov a pod.

• Product page
• Datasheet L3G4200D
• Gyroscope-3-Axis-L3G4200D-Guide
• Zdroj vizualizácie
• Arduino kód

Analýza

Senzor nám ponúka SPI a I2C zbernicu. Pre jednoduchosť sme použili I2C zbernicu. Chip podporuje protokol I2C, fast mode a takisto aj normal mode. Nemá žiadne špeciálne protokoli, 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 potreba snímania teploty. Pracovisko má stabilnú teplotu. Takisto nebudeme používať žiaden filter a využijeme obyčajný operation mód. 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 pomožu na určenie natočenia/uhla. Tu použijeme skript v matlabe ktorý nám bude integrovať v reálnom čase. Rovnice:

Piny na čipe:

My využijeme iba 5 z možných 8. 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é nekonfigurovať. Nachádza sa tu 5 Control registerov, ktoré je potrebné nakonfigurovať podľa požiadaviek na čip.

• CTRL_REG1 nastavuje zapnutie osí, a celkove 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 nasledne spojiť aby sme dostali finálnu hodnotu. Dáta su zapísane ako dvojkový doplnok, takže nám dávajú aj negatívne hodnoty. Registre idú za sebou, preto neni treba vkuse zapisovať adresu z ktorej chcem čítať, ale akonáhle je prečítaný prvý bajt, možme pokračovať na ďaľší 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 jendotká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ývojov 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 zralizované tak, že ACROB posiela charakter 'a' po sériovej linke a je to v loope, takže pokým nenastane komunikácia s matlabom/iným sériovým kuminikátorom, kód sa nepohne ďalej. Keď matlab prečíta charakter 'a', pošle po sériovej linke späť charakter '2'. Týmto si sú obe zaraidenia isté, že komunikujú správne. Matlab k´d je tiež v loop pokým nepríjme charakter 'a'.

Rozhodol som sa, že uhlovú rýchlosť budem integrovať a ukazovať natočenie šípky. Šípka sa 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 tohoto 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ňnov, vieme aj o koľko stupňnov sa seznor otočil a vieme aj uhlovú rýchlosť, je jednoduché potom zistiť kalibračnú konštantu.

Tento proces sa deje pre všetky tri osi, a vrámci získania presných hodnôt, proces sa deje 5 krát a kalibračná konštanta je zpreimerovaná. Po kalibráci sa spustí vizuálne okno kde je nakreslená šípka. šípka može byť reprezentovaná v 2D ale aj v 3D. 2D sa ukázala byť viac prehladné. Ší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 charakter 'G', potom senzor posila dáta normaálne, ak však matlab pošle charakter 'C', to znamená kalibrácia a kód riadi servo pohon a pošle dáta 50 krát. Pri charakteri 'G', dáta sú poslané raz treba neustále posielať charakter '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, v 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();	
}

Ďaľšie časti kódu sú už len I2C knižnica, a knižnica pre seriovú 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 charakter 'G' do čipu a čítame dáta z čipu. V ďaľších skiptoch potom už len integrujeme a reprezentujeme to na šípke. Ostatné kódy su 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

Ukážka Nezabudnite napísať čosi ako užívateľský návod. Z neho by malo byť jasné čo program robí, ako sa prejavuje a aké má užívateľské rozhranie (čo treba stlačiť, čo sa kde zobrazuje). Ak ste namerali nejaké signály, sem s nimi. Ak je výsledkom nejaký údaj na displeji, odfotografujte ho.

Kľúčové slová 'Category', ktoré sú na konci stránky nemeňte.