   // Setup motor update interval timer
   MsTimer2::set(motorUpdateRate, updateMotor);
   MsTimer2::start();
+
+  ref = millis();
 }
 
 void loop() {

  // Update orientation data with measurements from gyro
  // Just using angular rates from gyro and time deltas to track position using a Reimann sum.
  int currentTime = millis();
  int deltaT = currentTime - timer;
  positionChange += deltaT * readGyro() / 1000; // Gyro data is given in 1000ths of a degree, so we divide by 1000
  timer = currentTime;

  // Update motor position

    Finds the calibration constant of the
    gyro.
    @param number of calibration samples
    @returns degrees of gyro drift per second
*/
float zeroGyro(int calibrationSamples) {
  float gyroSum = 0;
  digitalWrite(LED, HIGH);
  for (int i = 0; i < calibrationSamples; i++) {
    gyroSum += readGyro();
  }
  blinkLED(300, 5);
  Serial.println(gyroSum / calibrationSamples);
  return gyroSum / calibrationSamples;
}

/**
