I would like to read a sensor connected via I2C using Timer3 and Arduino Mega. Values are read from the sensor in the interrupt service routine and timer3 is set to trigger an interrupt at 200Hz.

Step I: testing the timer

I tested the timer using a counter and incrementing its value in the ISR. The output is displayed every second. It works!

200
201
200
201

CODE

// Timer variable
volatile int cont=0;

void setup()
{
  Wire.begin();
  Serial.begin(115200);       
   
  setupL3G4200D(2000); // Configure L3G4200  - 250, 500 or 2000 deg/sec
  delay(1500); //wait for the sensor to be ready   

  // Initialize Timer
  cli();
  TCCR3A = 0;
  TCCR3B = 0;
  
  // Set compare match register to the desired timer count
  OCR3A=77; //16*10^6/(200Hz*1024)-1 = 77 -> 200 Hz 
  //OCR3A=193; //16*10^6/(80Hz*1024)-1 = 194 -> 80 Hz 
  
  TCCR3B |= (1 << WGM32);
  // Set CS10 and CS12 bits for 1024 prescaler:
  TCCR3B |= (1 << CS30) | (1 << CS32);
  // enable timer compare interrupt:
  TIMSK3 |= (1 << OCIE3B);
  // enable global interrupts:
  sei(); 
  
  k = micros();
}

void loop()
{
  delay(1000);
  Serial.println(cont);
  cont=0; 
}

ISR(TIMER3_COMPB_vect)
{  
  cont++;
}

Step II: Reading values from the sensor in the ISR

Now I add the I2C function in the Interrupt Service Routine, but the program is blocked after the calcBias() function which reads from the sensor, measures and saves the acquisition time in the samplingTime variable.

CODE

// Timer variables
volatile float phi=0;
volatile float theta=0;
volatile float psi=0;
volatile int x = 0;
volatile int y = 0;
volatile int z = 0;
volatile int cont=0;

void setup()
{
  Wire.begin();
  Serial.begin(115200);
   
  setupL3G4200D(2000); // Configure L3G4200  - 250, 500 or 2000 deg/sec
  delay(1500); //wait for the sensor to be ready   
  
  calcBias():
  Serial.print("Readings take: (us)");
  Serial.println(samplingTime);

  // Initialize Timer
  cli();
  TCCR3A = 0;
  TCCR3B = 0;
  
  // Set compare match register to the desired timer count
  OCR3A=77; //16*10^6/(200Hz*1024)-1 = 77 -> 200 Hz 
  //OCR3A=193; //16*10^6/(80Hz*1024)-1 = 194 -> 80 Hz 
  
  TCCR3B |= (1 << WGM32);
  // Set CS10 and CS12 bits for 1024 prescaler:
  TCCR3B |= (1 << CS30) | (1 << CS32);
  // enable timer compare interrupt:
  TIMSK3 |= (1 << OCIE3B);

  Serial.println("Enabling interrupts..");
  // enable global interrupts:
  sei(); 
  
  k = micros();
}

void loop()
{
  delay(1000);
  Serial.println(cont);
  Serial.println(z);
  cont=0; 
}


ISR(TIMER3_COMPB_vect)
{  
  // Update x, y, and z with new values 2.5ms     
  getGyroValues(); 
  cont++;
}

void getGyroValues()
{   

  //starting samplingTimer
  samplingTime = micros();  

  // Gets the angular velocity from the sensor
  byte zMSB = readRegister(L3G4200D_Address, 0x2D);
  byte zLSB = readRegister(L3G4200D_Address, 0x2C);
  z = ((zMSB << 8) | zLSB);

  samplingTime = micros()- samplingTime;
}

void calcBias()
{  
  Serial.println("Bias");
  
  // Reads sensors and display three values
  getGyroValues(); 
    
  [...]      

  Serial.println(bx);
  Serial.println(by);
  Serial.println(bz);        
}

void writeRegister(int deviceAddress, byte address, byte val) 
{
    Wire.beginTransmission(deviceAddress); // start transmission to device 
    Wire.write(address);       // send register address
    Wire.write(val);         // send value to write
    Wire.endTransmission();     // end transmission
}

int readRegister(int deviceAddress, byte address)
{
    int v;
    Wire.beginTransmission(deviceAddress);
    Wire.write(address); // register to read
    Wire.endTransmission();

    Wire.requestFrom(deviceAddress, 1); // read a byte

    while(!Wire.available()) 
    {
        // waiting
       // Serial.println("No Data");
    }

    v = Wire.read();
    return v;
}

I thought that the interrupt frequency was to high and I tried with 80 Hz and 20 Hz. Same results.

Here is the output:

starting up L3G4200D
Bias
4.00
0.00
-3.00
Read