   }
 }
 
-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;
 }
 


  // Accept corrections from serial
  if (Serial.available() == 4) {
    int correctionFactorXY = 0;
    int correctionFactorZ = 0;
    byte negXY = 0;
    byte negZ = 0;
    if (Serial.read() == '1') negXY = 1;
    correctionFactorXY += (Serial.read() - '0') * 100;
    correctionFactorXY += (Serial.read() - '0') * 10;
    correctionFactorXY += (Serial.read() - '0');
    if (negXY) correctionFactorXY = -correctionFactorXY;
    rotateStepperBy(correctionFactorXY);
    /*
    currentZBodyPosition += correctionFactorXY;
    currentZMotorPosition += correctionFactorXY;
    if (Serial.read() == '1') negZ = 1;
    correctionFactorZ += (Serial.read() - '0') * 100;
    correctionFactorZ += (Serial.read() - '0') * 10;
    correctionFactorZ += (Serial.read() - '0');
    if (negZ) correctionFactorZ = -correctionFactorZ;
    altitude.write(currentYBodyPosition + correctionFactorZ);
    currentYBodyPosition += correctionFactorZ;
    */
  }

  // 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;
  currentZBodyPosition += deltaT * readGyroZ() / 1000; // Gyro data is given in 1000ths of a degree, so we divide by 1000
  currentYBodyPosition += deltaT * readGyroY() / 1000;
  timer = currentTime;

  // Update motor positions
  if(update_flag) {
    // Debug output
    // Update servo position (Y/ALT)
    altitude.write(currentYBodyPosition);

    // Update stepper position (Z/AZM)
    float difference = currentZBodyPosition - currentZMotorPosition; // we want currentZMotorPosition + currentZBodyPosition = 0
    int steps = rotateStepperBy(-difference);
    currentZMotorPosition += -steps * stepperStepSize;

    // Reset flag
    update_flag = false;
  }

}

// For stepper motors
void rotateServoBy(float deg) {
  float wait = abs((deg / 360) / RPM);
  if (deg < 0) azimuth.write(COUNTER_CLOCK_WISE_SPEED);
  else azimuth.write(CLOCK_WISE_SPEED);
  delay(wait);
  azimuth.write(ZERO_SPEED);
}

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

void blinkLED(int del, int n) {
