     //Serial.println("No valid compass calibration data");
 
   lastDisplay = lastRate = millis();
-  sampleCount = 0;
-  timer = millis();
 }
 
 RTVector3 q;
 
+unsigned long now;
+  
 void loop()
 {  
-  unsigned long now = millis();
-  unsigned long delta;
-  
+  now = millis();
+  tryReadIMU();
+  buzz();
+}
+
+void tryReadIMU()
+{
   if (imu->IMURead()) {                                // get the latest data if ready yet
     fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp());
-    sampleCount++;
-    if ((delta = now - lastRate) >= 1000) {
-      //Serial.print("Sample rate: "); Serial.print(sampleCount);
+    if ((now - lastRate) >= 1000) {
       imu->IMUGyroBiasValid();
         //Serial.println(", gyro bias valid");
       //else
         //Serial.println(", calculating gyro bias - don't move IMU!!");
         
-      sampleCount = 0;
       lastRate = now;
     }
-    //if ((now - lastDisplay) >= DISPLAY_INTERVAL) {
-      //lastDisplay = now;
-//      RTMath::display("Gyro:", (RTVector3&)imu->getGyro());                // gyro data
-//      RTMath::display("Accel:", (RTVector3&)imu->getAccel());              // accel data
-//      RTMath::display("Mag:", (RTVector3&)imu->getCompass());              // compass data
-//      RTMath::displayDegrees("", (RTVector3&)fusion.getFusionPose()); // fused output
-//      Serial.println();
-      q = (RTVector3&)fusion.getFusionPose();
-      
-      Serial.print(q.y() * 57.2957795);
-      Serial.println();
-    //}
-  }
-  
+
+    q = (RTVector3&)fusion.getFusionPose();
+    
+    Serial.print(q.y() * 57.2957795);
+    Serial.println();
+  }  
+}
+
+void buzz()
+{
   if (buzz && (now - buzzTime) >= abs((q.y() * 57.2957795))* 5){
     buzz = false;
     silentTime = now;

#include <Wire.h>
#include "I2Cdev.h"
#include "RTIMUSettings.h"
#include "RTIMU.h"

  Wire.begin();
  imu = RTIMU::createIMU(&settings);                        // create the imu object
  
  if ((errcode = imu->IMUInit()) < 0) {
    //  Display error somehow
  }
  
  //if (imu->getCalibrationValid())
