BMA250 Accelerometer - how to sleep

I figured out how to do this using the suspend feature. I didn't realize I could wake the BMA250 from suspend sleep with an I2C command. So this turned out to be pretty easy. But I have a strange problem that has me stumped. I have a test sketch that works. It puts the accelerometer to sleep for 5 seconds, wakes it up for a reading then back to sleep. I had a few Serial.println() statements in the code for debugging. When I started to take the print statements out, it stopped working properly - it just stays in sleep mode and keeps returning the same value for x y z. I tried putting a delay where the print statement was thinking maybe that was the cause, but that didn't help.

Here is the working sketch with the Serial.println() statements still in place.

#include <Wire.h>
#define BMA250_I2CADDR      0x18
#define BMA250_RANGE        0x03   // 0x03 = 2g, 0x05 = 4g, 0x08 = 8g, 0x0C = 16g
#define BMA250_BW           0x08   // 7.81Hz (update time of 64ms) - slowest speed
#define BMA250_POWERMODE    0x11   // Power Mode register
#define BMA250_WAKEUP          0
#define BMA250_SLEEP           1

int AccelX;
int AccelY;
int AccelZ;


// Function Prototypes
void setup();
void loop();
void BMA250Init();
int BMA250ReadAccel(); 
uint8_t getPwrModeRegister(); 
bool setSleepMode(bool suspendMode);


void setup()
{
  Wire.begin();
  Serial.begin(9600);
  BMA250Init();
}


void loop()
{
  static uint32_t tmrSleep = millis();
  
  
  if ((long)(millis() - tmrSleep) > 0)
  {
    tmrSleep = millis() + 5000;
    setSleepMode(BMA250_WAKEUP);
    delay(2); // give accelerometer time to wake-up

    BMA250ReadAccel();
  
    // Print out the accelerometer data
    Serial.print("x: ");
    Serial.print(AccelX);
    Serial.print("\ty: ");
    Serial.print(AccelY);
    Serial.print("\tz:");
    Serial.print(AccelZ);
    Serial.println("");    
    Serial.println("Going to sleep now");
    setSleepMode(BMA250_SLEEP);
  }
}


void BMA250Init()
{
  // Setup the range measurement setting
  Wire.beginTransmission(BMA250_I2CADDR);
  Wire.write(0x0F);   // G-force range register
  Wire.write(BMA250_RANGE);
  Wire.endTransmission();
  
  // Setup the bandwidth
  Wire.beginTransmission(BMA250_I2CADDR);
  Wire.write(0x10);  // bandwidth register
  Wire.write(BMA250_BW);
  Wire.endTransmission();
} // BMA250Init


int BMA250ReadAccel()
{
  uint8_t ReadBuff[8];
  
  // Read the 6 data bytes from the BMA250
  Wire.beginTransmission(BMA250_I2CADDR);
  Wire.write(0x02);  // starting register for accel data
  Wire.endTransmission();
  Wire.requestFrom(BMA250_I2CADDR,6);
  
  for(int i = 0; i < 6;i++)
  { ReadBuff[i] = Wire.read(); }
  
  AccelX = ReadBuff[1] << 8;
  AccelX |= ReadBuff[0];
  AccelX >>= 6;
  
  AccelY = ReadBuff[3] << 8;
  AccelY |= ReadBuff[2];
  AccelY >>= 6;
  
  AccelZ = ReadBuff[5] << 8;
  AccelZ |= ReadBuff[4];
  AccelZ >>= 6;  
} // BMA250ReadAccel()



bool setSleepMode(bool suspendMode )
{
  uint8_t powerModeBuf;
  
  // Read current register value
  powerModeBuf = getPwrModeRegister();
  Serial.print("Read Register:  ");
  Serial.println(powerModeBuf, BIN);
  
  // Set the suspend bit
  if (suspendMode == BMA250_SLEEP) 
  { powerModeBuf |= 1 << 7; }  // set bit 7
  else
  { powerModeBuf &= ~(1 << 7); }  // clear bit 7


  Serial.print("Write Register: ");
  Serial.println(powerModeBuf, BIN);

  // Write register back to accelerometer
  Wire.beginTransmission(BMA250_I2CADDR);
  Wire.write(BMA250_POWERMODE);  // Powermode register
  Wire.write(powerModeBuf);
  Wire.endTransmission();

  return suspendMode;

} // setSleepMode()



// Power Mode register 0x11
// bit 7 - suspend
uint8_t getPwrModeRegister()
{
  // Read the power mode register byte 0x11 
  Wire.beginTransmission(BMA250_I2CADDR);
  Wire.write(BMA250_POWERMODE); // power mode register
  Wire.endTransmission();
  Wire.requestFrom(BMA250_I2CADDR,1);
  return Wire.read();

} // getPwrModeRegister()