IMU problem - I2C

Projects Programming
6

I think something with my I2C_read function is up

Try this less general version for now and see if it still prints out the same values for both bytes.

int I2C_read(byte slaveAddress, byte registerAddress, byte numberOfBytes)
{
    Wire.beginTransmission(slaveAddress);
    Wire.write(registerAddress);
    if(Wire.endTransmission()!=0)
      Serial.println("Reading Error!");
    Wire.requestFrom(slaveAddress, numberOfBytes); 
    //while(Wire.available() < numberOfBytes);
    //long temp = 0;
    int temp = 0;
    byte lsb = Wire.read();//0x28 register low byte
    Serial.println(lsb);
    byte msb = Wire.read();//0x29 register high byte
    Serial.println(msb);
    temp = msb << 8 | lsb; 
  
   // for(int i=1; i<=numberOfBytes; i++)
   // {
   //   temp += (long)Wire.read()<<(8*(numberOfBytes - i));
  //  }
    return temp;
}