SmartRod_test.ino

void loop()
{ 
  if (digitalRead(CHARGE_PIN) == CHARGE_OFF)
  {
    charging();
  }
  while (Bluefruit.connected())
  {
    noInterrupts();
    //readGyro();
    //readAccel();
	  readBoth();
    interrupts();
  }
}

void readGyro()
{
  Wire.beginTransmission(MPU_ADDRESS);
  Wire.write(GYRO_XOUT_H);
  Wire.endTransmission();
  Wire.requestFrom(MPU_ADDRESS, 6);
  angularVelocity[0] = Wire.read()<<8 | Wire.read();
  angularVelocity[1] = Wire.read()<<8 | Wire.read();
  angularVelocity[2] = Wire.read()<<8 | Wire.read();
}
  
void readAccel()
{
  Wire.beginTransmission(MPU_ADDRESS);
  Wire.write(ACCEL_XOUT_H);
  Wire.endTransmission();
  Wire.requestFrom(MPU_ADDRESS, 6);
  acceleration[0] = Wire.read()<<8 | Wire.read();
  acceleration[1] = Wire.read()<<8 | Wire.read();
  acceleration[2] = Wire.read()<<8 | Wire.read();
}

void readBoth()
{
  Wire.beginTransmission(MPU_ADDRESS);
  Wire.write(GYRO_XOUT_H);
  Wire.write(ACCEL_XOUT_H);
  Wire.endTransmission();
  Wire.requestFrom(MPU_ADDRESS, 12);
  if (Wire.available() <= 12)
  {
    angularVelocity[0] = Wire.read()<<8 | Wire.read();
    angularVelocity[1] = Wire.read()<<8 | Wire.read();
    angularVelocity[2] = Wire.read()<<8 | Wire.read();

    acceleration[0] = Wire.read()<<8 | Wire.read();
    acceleration[1] = Wire.read()<<8 | Wire.read();
    acceleration[2] = Wire.read()<<8 | Wire.read();
  }
}