 }
 
 // For stepper motors
-int rotateStepperBy(float deg) {
-  int steps = deg/stepperStepSize;
-  motor.step(steps);  
-  return steps;
+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 readGyroZ() {
+float readGyro() {
   if (imu.gyroAvailable()) {
     imu.readGyro();
   }
-  return imu.calcGyro(imu.gz) - calibratedZOffset;
-}
-
-float readGyroY() {
-  if (imu.gyroAvailable()) {
-    imu.readGyro();
-  }
-  return imu.calcGyro(imu.gy) - calibratedYOffset;
+  return imu.calcGyro(imu.gz) - calibratedOffset;
 }
 
 void blinkLED(int del, int n) {

  }
}

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


// ================ SYSTEM PARAMETERS ===================
// Sample rates
int motorUpdateRate = 50; // gyro sample rate
int calibrationSamples = 1000;

// Initial calibration value
float calibratedOffset = 0;

// Indicator LED
const int LED = 13;

// These values will need to be experimentally found using
// a servo
const float RPM = 1.5;
const float CLOCK_WISE_SPEED = 40;
const float COUNTER_CLOCK_WISE_SPEED = 100;
const float ZERO_SPEED = 75;
// =======================================================

// ============ Initialize electronic interfaces =========
Servo azimuth;
LSM9DS1 imu;

// ============ Runtime variables ========================
// Orientation data
float positionChange;

// Misc.
int timer = millis();
