   }
 }
 
-void zeroGyro() {
-  float ySum = 0;
+int zeroGyro(int calibrationSamples) {
   float zSum = 0;
-
   for(int i = 0; i < calibrationSamples; i++) {
-
-    if(i%100==0) {
-      digitalWrite(LED, HIGH);
+    if(i % 1000 == 0) {
+      blinkLED(500, 1);
     }
-    else if(i%100==10) {
-      digitalWrite(LED, LOW);
-    }
-    
     imu.readGyro();
-    float y = imu.calcGyro(imu.gy);
     float z = imu.calcGyro(imu.gz);
-    ySum += y;
     zSum += z;
     delay(10);
   }
-
-  calibratedYOffset = ySum/calibrationSamples;
-  calibratedZOffset = zSum/calibrationSamples;
+  return zSum / calibrationSamples;
 }
 

}

// For stepper motors
int rotateStepperBy(float deg) {
  int steps = deg/stepperStepSize;
  motor.step(steps);  
  return steps;
}

float readGyroZ() {
  if (imu.gyroAvailable()) {
    imu.readGyro();
  }
  return imu.calcGyro(imu.gz) - calibratedZOffset;
}

float readGyroY() {
  if (imu.gyroAvailable()) {
    imu.readGyro();
  }
  return imu.calcGyro(imu.gy) - calibratedYOffset;
}

void blinkLED(int del, int n) {


  // Accept corrections from serial
  if (Serial.available() == 4) {
    int correctionFactor = 0;
    byte negative = 0;
    if (Serial.read() == '1') negative = 1;
    for (int i = 100; i >= 1; i /= 10) {
      correctionFactor += (Serial.read() - '0') * 100;
    }
    if (negative) correctionFactor = -correctionFactor;
    rotateServoBy(correctionFactor);
  }

  // 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
  if(update_flag) {
    // Update stepper position (Z/AZM)
    rotateServoBy(-positionChange);
    positionChange = 0;
    // Reset flag
    update_flag = false;
  }
