     XYZbias[0] = sumx/20;
     XYZbias[1] = sumy/20;
     XYZbias[2] = sumz/20;
+    
+    count2 = millis();
 }
 
 void loop()

    count = millis();
    }
    
    if ( ( millis() - count2 ) > 15 )
    {
      pos = pos + dpos;
      if (pos >= 180) dpos = -1;
      else if (pos <= 0) dpos = 1;
      myservo.write(pos);
      count2 = millis();
    }
}


  float gxm = dof.calcGyro(GX.mean()) - gbias[0];
  float gym = dof.calcGyro(GY.mean()) - gbias[1];
  float gzm = dof.calcGyro(GZ.mean()) - gbias[2];
  //MadgwickQuaternionUpdate(axm, aym, azm, gxm*PI/180.0f, gym*PI/180.0f, gzm*PI/180.0f, mx, my, mz);
   MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);
 //MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);

    // Serial print and/or display at 0.5 s rate independent of data rates
