     currentServoVal += 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;
+  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
     Serial.println("SUM: BODY_Y = " + String(currentYBodyPosition) + " | BODY_Z = " + String(currentZBodyPosition) + " | MOTOR_Y: " + String(currentYMotorPosition) + " | MOTOR_Z: " + String(currentZMotorPosition));
+
+    // Update servo position (Y/ALT)
     altitude.write(currentYBodyPosition);
-    rotateStepperBy(currentZBodyPosition - currentZMotorPosition);
     currentYMotorPosition = currentYBodyPosition;
-    currentZMotorPosition = currentZBodyPosition;
-    sampleCounter = 0;
+
+    // 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 rotateStepperBy(float deg) {
  int steps = deg*5/9;
  motor.step(steps);  
}

float readGyroZ() {

#include <SparkFunLSM9DS1.h>
#include "config.h"

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

int calibrationSamples = 1000;

// Initial calibration values
float calibratedYOffset = 0;
float calibratedZOffset = 0;

// Stepper motor
int stepperPins[] = {9, 10, 11, 12};
int stepperSteps = 200;
float stepperStepSize = 9.0/5;

// Servo motor
float currentServoVal = 90;

// Indicator LED
const int LED = 13;
// =======================================================

// ============ Initialize electronic interfaces =========
// Stepper
Stepper motor(stepperSteps, stepperPins[0], stepperPins[1], stepperPins[2], stepperPins[3]);

// Servo
Servo altitude;

// Gyro
LSM9DS1 imu;

// ============ Runtime variables ========================
// Orientation data
float currentYBodyPosition = 0;
float currentZBodyPosition = 0;
float currentYMotorPosition = 0;
float currentZMotorPosition = 0;

// Misc.
int timer = millis();
bool update_flag = false; // interrupt flag for executing motor update

// Initialize everything
void setup() {
  // Initialize I/O
  Serial.begin(115200);
  pinMode(LED, OUTPUT);

  // Setup hardware
  setupIMU();         // Gyro
  motor.setSpeed(20); // Stepper
  altitude.attach(3); // Servo

  // Calibrate the gyro
  zeroGyro();

  // Setup motor update interval timer
  MsTimer2::set(motorUpdateRate, updateMotor);
  MsTimer2::start();

  // Blink LED to indicate that initilization has completed!
  blinkLED(20, 10);
}

void loop() {

  // Accept corrections from serial
  if (Serial.available() == 8) {
    int correctionFactorXY = 0;
    int correctionFactorZ = 0;
