 
   // Setup hardware
   setupIMU();         // Gyro
-  motor.setSpeed(20); // Stepper
-  altitude.attach(3); // Servo
+  azimuth.attach(3);
 
   // Calibrate the gyro
-  zeroGyro();
+  calibratedOffset = zeroGyro(calibrationSamples);
 
   // Setup motor update interval timer
   MsTimer2::set(motorUpdateRate, updateMotor);

  }
}

void zeroGyro() {
  float ySum = 0;
  float zSum = 0;

  for(int i = 0; i < calibrationSamples; i++) {

    if(i%100==0) {
      digitalWrite(LED, HIGH);
    }
    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;
}


}

// 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) {
