MPU6050 and Timer1 conflict

Hi, I'm building self balanced robot and I want to use MPU6050 (GY-521 board) instead of my analog sensors. I have Arduino UNO R2.

The problem is with Timer1 because I use this timer for interrupt ( reading values from sensor and do PID() function). I think, that FreeIMU library using the same timer and could change his settings.

//-------- MPU6050 ----------
#include <I2Cdev.h>
#include <MPU60X0.h>
#include <EEPROM.h>


//#define DEBUG
#include "DebugUtils.h"
#include "CommunicationUtils.h"
#include "FreeIMU.h"
#include <Wire.h>
#include <SPI.h>

int raw_values[9];
float ypr[3]; // yaw pitch roll

// Set the FreeIMU object
FreeIMU my3IMU = FreeIMU();


float reading = 0;

void setup(){
 
  Serial.begin(115200);
  Serial.println("A");
  Wire.begin();
  my3IMU.init(); // the parameter enable or disable fast mode
  Serial.println("B");
  noInterrupts();
  Serial.println("C);
//set timer1 interrupt or 256Hz
  TCCR1A = 0;// set entire TCCR1A register to 0
  TCCR1B = 0;// same for TCCR1B
  TCNT1  = 0;//initialize counter value to 0
  // set compare match register for 1hz increments
  OCR1A = 0xF423;// = (16*10^6) / (1*4) - 1 (must be <65536) //256Hz
  // turn on CTC mode
  TCCR1B |= (1 << WGM12);
  TCCR1B |= (1 << CS10); // for 1 for 256Hz or 512 Hz

  // enable timer compare interrupt
  TIMSK1 |= (1 << OCIE1A);
 interrupts();
}
void loop(){
Serial.println("D");
}

ISR(TIMER1_COMPA_vect){//timer1 256Hz 

  Serial.println("E");
 my3IMU.getYawPitchRoll(ypr);
 reading = ypr[2];
  Serial.println("F");
}

The result of code is:

A
B

How to fix it?

How to fix it?

Do you really expect interrupt-driven functions to execute while you have interrupts disabled?

ok, I enabled it, but I still have a problem, because my3IMU.getYawPitchRoll(ypr) function takes 4000 microsecond (3000 in fast mode), why so long ?

how about modify the code like this.

void IMU_Get()
{
my3IMU.getYawPitchRoll(ypr);
reading = ypr[2];
Serial.println("F");
}

int timer_sw;
void loop(){
Serial.println("D");
if (timer_sw)
{
IMU_Get();
timer_sw = 0;
}
}

ISR(TIMER1_COMPA_vect){//timer1 256Hz
Serial.println("E");
timer_sw = 1;
}

///////////////////////////////////

///////////////////////////////////

ISR(TIMER1_COMPA_vect){//timer1 256Hz
  Serial.println("E");
  timer_sw = 1;
}

Interrupts are disabled during an interrupt service routine. Doing things in the ISR that rely on interrupts being enabled is stupid. Stop doing it.

It is no way to use Wire.xxxx function in ISR. Please refer the related document from the blog or take a look my works of 2WD balance robot here. http://regishsu.blogspot.tw/
Thanks.