http://blog.tkjelectronics.dk/2012/03/the-balancing-robot/#comment-285666

Help_for_Dustan.ino

// Define these variables
int16_t accY;
int16_t accZ;
int16_t gyroX;
unsigned long timer;

// Put this in your setup()
Wire.begin();  
i2cWrite(0x6B,0x00); // Disable sleep mode  
if(i2cRead(0x75,1)[0] != 0x68) { // Read "WHO_AM_I" register
  Serial.println(F("Error reading sensor"));
  while(1); 
}

// And this in your loop
uint8_t* data = i2cRead(0x3D,8);  
accY = ((data[0] << 8) | data[1]);
accZ = ((data[2] << 8) | data[3]);
gyroX = ((data[6] << 8) | data[7]);

double accAngle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
double gyroRate = (double)gyroX/131.0;

pitch = kalman.getAngle(accAngle, gyroRate, (double)(micros()-timer)/1000000);
timer = micros();


// You also need the following functions
void calibrate() {
  uint8_t* data = i2cRead(0x3D,4);  
  accY = ((data[0] << 8) | data[1]);
  accZ = ((data[2] << 8) | data[3]);
  double accAngle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;

  if(accAngle < 180) // Check which side is lying down
    kalman.setAngle(90); // It starts at 90 degress and 270 when facing the other way
  else
    kalman.setAngle(270);
}

void i2cWrite(uint8_t registerAddress, uint8_t data){
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.write(data);
  Wire.endTransmission(); // Send stop
}
uint8_t* i2cRead(uint8_t registerAddress, uint8_t nbytes) {
  uint8_t data[nbytes];  
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.endTransmission(false); // Don't release the bus
  Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading
  for(uint8_t i = 0; i < nbytes; i++)
    data[i] = Wire.read();
  return data;
}