getting angles from mpu-6050

hi,
im writing a code for my mpu 6050, and i have some difficulties with it, my angles are not good and the deviation is really large

do you have some suggestions?

//wiring gyro: vcc->3.3v,Gnd<->ADO->gnd,SCL->A5,SDA->A4
//wiring BT +5v->5V,GND->GND,TX->D10,RX->D9

//power switch
//after BT
//add start commend
//add stop commend

#include <SoftwareSerial.h>
#include <Wire.h>

int i;
float gyroX, gyroY, gyroZ,geroXin,geroYin,geroZin;
float rotX, rotY, rotZ;
float anl =0.2,pre_time1 = 0, time1=0; //angle noise limit-anl
float LSB = 32.8*4; //2000/65536;//calibrat fs_sel=3 (250-131,500-65.5,1000-32.8,2000-16.4)
float dt;

const int RX_PIN = 10;
const int TX_PIN = 9;
const int BLUETOOTH_BAUD_RATE = 9600;

SoftwareSerial bluetooth(RX_PIN, TX_PIN);

void setup() {
Serial.begin(9600);
Wire.begin();
setupMPU();
Serial.begin(9600);
//bluetooth.begin(BLUETOOTH_BAUD_RATE);
}

void loop() {
//recordAccelRegisters();
recordGyroRegisters();
printData();

}

void setupMPU(){
Wire.beginTransmission(0b1101000); //This is the I2C address of the MPU (b1101000/b1101001 for AC0 low/high datasheet sec. 9.2)
Wire.write(0x6B); //Accessing the register 6B - Power Management (Sec. 4.28)
Wire.write(0b00000000); //Setting SLEEP register to 0. (Required; see Note on p. 9)
Wire.endTransmission();
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x1B); //Accessing the register 1B - Gyroscope Configuration (Sec. 4.4)
Wire.write(0x00010000); //Setting the gyro to full scale(250-(0x00000000), 500-(0x00001000),1000-(0x00010000),2000-(0x00011000))
Wire.endTransmission();
gyrocalibration();
}

void gyrocalibration()
{
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x43); //Starting register for Gyro Readings
Wire.endTransmission();
Wire.requestFrom(0b1101000,6); //Request Gyro Registers (43 - 48)
while(Wire.available() < 6);
Serial.println();
Serial.print(“loading”);
for(i=0;i<20;i++)
{
delay(500);
Serial.print(".");
}
Serial.println(“suprize”);
Serial.println();

geroYin = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
geroZin = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ

time1 = millis();
pre_time1 = millis();
}

void recordGyroRegisters() {
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x43); //Starting register for Gyro Readings
Wire.endTransmission();
Wire.requestFrom(0b1101000,6); //Request Gyro Registers (43 - 48)
while(Wire.available() < 6);
gyroY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
gyroZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ
time1 = millis();
dt = time1 - pre_time1;
pre_time1 = time1;
processGyroData();
}

void processGyroData() {

if(abs(gyroY-geroYin) * ( dt / 1000 ) / LSB >= anl ) //noise filter
{
rotY =rotY + (gyroY-geroYin)*( dt / 1000 ) / LSB; //caculate new angle[deg]
}
if(abs(gyroZ - geroZin) * ( dt / 1000 ) / LSB >= anl ) //noise filter
{
rotZ = rotZ + (gyroZ - geroZin) * ( dt / 1000 ) / LSB; //caculate new angle[deg]
}
}

void printData() {

// bluetooth.print(rotZ);
// bluetooth.print(",");
// bluetooth.println(rotY);
Serial.print(dt);
Serial.print(",");
Serial.print(abs(gyroY-geroYin) * ( dt / 1000 ));
Serial.print(",");
Serial.print(abs(gyroY-geroYin) / (32.8*4));
Serial.print(",");
Serial.print(( dt / 1000 ));
Serial.print(",");
Serial.print(rotZ);
Serial.print(",");
Serial.println(rotY);
}

combinedv3.ino (3.36 KB)

Please edit your post and add code tags. Instructions are in "How to use this forum".