#include <L3G4200D.h>
#include <lcd.c>
#include <math.h>

#define WHO_AM_I 0x0F
#define CTRL_REG1 0x20
#define CTRL_REG2 0x21
#define CTRL_REG3 0x22
#define CTRL_REG4 0x23
#define CTRL_REG5 0x24
#define CTRL_REG6 0x25
#define FIFO_CTRL_REG 0x2E
#define FIFO_SRC_REG 0x2F
#define OUT_X_L 0x28
#define OUT_X_H 0x29
#define OUT_Y_L 0x2A
#define OUT_Y_H 0x2B
#define OUT_Z_L 0x2C
#define OUT_Z_H 0x2D
#define STATUS_REG 0x27
#define REFERENCE 0x25
#define INT1_CFG 0x30
#define INT1_SRC 0x31
#define INT1_THS_XH 0x32
#define INT1_THS_XL 0x33
#define INT1_THS_YH 0x34
#define INT1_THS_YL 0x35
#define INT1_THS_ZH 0x36
#define INT1_THS_ZL 0x37
#define INT1_DURATION 0x38
/*#define x_eksen 0
#define y_eksen 1
#define z_eksen 2*/

enum eksenler {
x_eksen = 0,
y_eksen = 1,
z_eksen = 2,
};

signed int8 xMSB=0;
signed int8 xLSB=0;
signed int8 yMSB=0;
signed int8 yLSB=0;
signed int8 zMSB=0;
signed int8 zLSB=0;

signed int16 Rm[3]={0};
float32 Ro[3]={0};
float32 Rth[3]={0};
float32 Rt[3]={0};

void init(); //Fonksiyon prototipleri
void Setup_Gyro();
void Gyroscope_Write(unsigned int8 yazilacakAdres,unsigned int8 deger);
signed int8 Gyroscope_Read(unsigned int8 okunacakAdres);
void Gyroscope_Values_Raw();
void Gyroscope_Values();
void Calibrate_Gyro();

void main() //Ana Fonksiyon
{
init();
Setup_Gyro();
Calibrate_Gyro();
while(true)
{
Gyroscope_Values();
delay_ms(100);
printf(lcd_putc,"\f");
lcd_gotoxy(1,1);
printf(lcd_putc,"X:%Ld",Rt[x_eksen]);
lcd_gotoxy(1,2);
printf(lcd_putc,"Y:%Ld",Rt[y_eksen]);
lcd_gotoxy(9,1);
printf(lcd_putc,"Z:%Ld",Rt[z_Eksen]);

}
}
//#############################################################################
void init() //Denetleyici ayarları
{
setup_psp(PSP_DISABLED);
setup_timer_0(RTCC_OFF);
setup_timer_1(T1_DISABLED);
setup_timer_2(T2_DISABLED,0,1);
setup_timer_3(T3_DISABLED);
setup_adc_ports(NO_ANALOGS);
setup_adc(ADC_OFF);
setup_CCP1(CCP_OFF);
setup_CCP2(CCP_OFF);
setup_comparator(NC_NC_NC_NC);
lcd_init();
lcd_gotoxy(1,1);
printf(lcd_putc,"LCD confirmed");
delay_ms(500);
printf(lcd_putc,"\f");
}
//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
void Setup_Gyro()
{

Gyroscope_Write(CTRL_REG2,0b00100000); //Normal mode (reset reading HP_RESET_FILTER) ,High pass filter cut off frecuency:8
Gyroscope_Write(CTRL_REG3,0b00001000);// I2_DRDY,I2_WTM,I2_ORun,I2_Empty etkin ,push-pull--FIFO etkin olmadığı durumda push-pull hariç diğerleri geçersiz
Gyroscope_Write(CTRL_REG4,0b10010000);//BDU:1,500 dps,self-test disabled(normal mode)
Gyroscope_Write(CTRL_REG5,0b00001010);//HPF disabled,FIFO disabled
/*Gyroscope_Write(REFERENCE,0b00000000);
Gyroscope_Write(INT1_THS_XH,0b00000000);
Gyroscope_Write(INT1_THS_XL,0b00000000);
Gyroscope_Write(INT1_THS_YH,0b00000000);
Gyroscope_Write(INT1_THS_YL,0b00000000);
Gyroscope_Write(INT1_THS_ZH,0b00000000);
Gyroscope_Write(INT1_THS_ZL,0b00000000);
Gyroscope_Write(INT1_DURATION,0b00000000);
Gyroscope_Write(INT1_CFG,0b00000000);*/
Gyroscope_Write(CTRL_REG1,0b00011111); // 100Hz,25 cut-off/ X,Y,Z etkin
delay_ms(252);

}
//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
void Calibrate_Gyro()
{
float32 X[50]={0};
float32 Y[50]={0};
float32 Z[50]={0};
float32 temp[3]={0};

//50 adet çiğ değer toplanıyor ve ayrıca bu değerler hafızada tutuluyor.
for(unsigned int8 i=0;i<50;i++)
{
Gyroscope_Values();
X[i]=Rm[x_eksen]; Y[i]=Rm[y_eksen]; Z[i]=Rm[z_eksen];
// her döngüde farklı değerler farklı eksen dizilerine veriler atandı.
temp[x_eksen] = temp[x_eksen] + Rm[x_eksen];
temp[y_eksen] = temp[y_eksen] + Rm[y_eksen];
temp[z_eksen] = temp[z_eksen] + Rm[z_eksen];
//zero-rate için veri toplanıyor
}
//zero-rate belirleniyor
Ro[x_eksen] = temp[x_eksen] / 50;
Ro[y_eksen] = temp[y_eksen] / 50;
Ro[z_eksen] = temp[z_eksen] / 50;

for(int k=0;k<50;k++)
{//threshold için veri toplanıyor
Rth[x_eksen] = Rth[x_eksen] + pow((X[k] - Ro[x_eksen]),2);
Rth[y_eksen] = Rth[y_eksen] + pow((Y[k] - Ro[y_eksen]),2);
Rth[z_eksen] = Rth[z_eksen] + pow((Z[k] - Ro[z_eksen]),2);
}

Rth[x_eksen] = 3*sqrt(Rth[x_eksen] / 50);
Rth[y_eksen] = 3*sqrt(Rth[y_eksen] / 50);
Rth[z_eksen] = 3*sqrt(Rth[z_eksen] / 50);
}
//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
void Gyroscope_Write(unsigned int8 yazilacakAdres,unsigned int8 deger)
{
i2c_start();
i2c_write(0xD2);
i2c_write(yazilacakAdres);
i2c_write(deger);
i2c_stop();
delay_ms(20);
}
//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
signed int8 Gyroscope_Read(unsigned int8 okunacakAdres)
{
signed int8 gonder=0;
i2c_start();
i2c_write(0xD2);
i2c_write(okunacakAdres);
i2c_start();
i2c_write(0xD3);

gonder = i2c_read(0);

i2c_stop();
delay_ms(10);
return(gonder);
}
//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&

void Gyroscope_Values_Raw()
{
unsigned int8 cikis=0,hpfReset,c=0;
cikis = Gyroscope_Read(STATUS_REG);
delay_ms(5);

while(bit_test(cikis,3)==0)
{
cikis = Gyroscope_Read(STATUS_REG);
}

//if(bit_test(cikis,7)==1) return;

xMSB = Gyroscope_Read(OUT_X_H);
xLSB = Gyroscope_Read(OUT_X_L);
yMSB = Gyroscope_Read(OUT_Y_H);
yLSB = Gyroscope_Read(OUT_Y_L);
zMSB = Gyroscope_Read(OUT_Z_H);
zLSB = Gyroscope_Read(OUT_Z_L);
hpfReset = Gyroscope_Read(REFERENCE);
Rm[x_eksen] = make16(xMSB,xLSB);
Rm[y_eksen] = make16(yMSB,yLSB);
Rm[z_eksen] = make16(zMSB,zLSB);
delay_ms(10);
}
//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
void Gyroscope_Values()
{
Rt[x_eksen]=0; Rt[y_eksen]=0; Rt[z_eksen]=0;
float32 deltaR[3]={0};
Gyroscope_Values_Raw();
for (int j = 0; j<3; j++)
{
deltaR[j] = Rm[j] - Ro[j];

if (abs(deltaR[j]) < Rth[j])
deltaR[j] = 0;

Rt[j] = deltaR[j] * 0.0175 * 0.01;//0.01 ile açısal yer değiştirme bulunur.
}
}

//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
/*void Gyroscope_Values_Raw(signed int16 array[])
{
Gyroscope_Write(FIFO_CTRL_REG,0b00101010);//wtm:10 fifo mod
unsigned int8 cikis=0;
cikis = Gyroscope_Read(FIFO_SRC_REG);
while(bit_test(cikis,7)==0)
cikis = Gyroscope_Read(FIFO_SRC_REG);

while(bit_test(cikis,5)==0) //empty bit 0 iken verileri alr
{
xMSB = Gyroscope_Read(OUT_X_H);
xLSB = Gyroscope_Read(OUT_X_L);
yMSB = Gyroscope_Read(OUT_Y_H);
yLSB = Gyroscope_Read(OUT_Y_L);
zMSB = Gyroscope_Read(OUT_Z_H);
zLSB = Gyroscope_Read(OUT_Z_L);

array[0] = make16(xMSB,xLSB);
array[1] = make16(yMSB,yLSB);
array[2] = make16(zMSB,zLSB);

cikis = Gyroscope_Read(FIFO_SRC_REG);// empty bitini öğrenmek için güncelleme

delay_ms(5);
}

Gyroscope_Write(FIFO_CTRL_REG,0b00001010);
}*/

Hey guys ,I have a problem ,accordign to datasheet if STATUS_REG 7. bit=0 in this case some values overwritten before I read previously ,How I can block this .Actually I dont know my problem is related with this but I should notify .Now I have some values very big that despite I do multiplication with like 0.00000001 ,I m moving with application notes.
Thanks for your helps .

multiplication

multiplication