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

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

// Declare global variables
LSM9DS1 imu; // imu chip
int stepperPins[] = {9, 10, 11, 12};
int stepperSteps = 200;
Stepper motor(stepperSteps, stepperPins[0], stepperPins[1], stepperPins[2], stepperPins[3]);
Servo altitude;

int motorUpdateRate = 50; // gyro sample rate
int maxSteps = 10; // maximum number of steps per motor update
int sampleCounter = 0; // keep track of what to sample
int timer = millis();
float runningSumAlt = 0;
float runningSumZ = 0;
float currentYBodyPosition = 0;
float currentZBodyPosition = 0;
float currentYMotorPosition = 0;
float currentZMotorPosition = 0;
int calibrationSamples = 1000;
float calibratedYOffset = 0;
float calibratedZOffset = 0;

float currentServoVal = 90;

bool update_flag = false; // interrupt flag for executing motor update
const int LED = 13; // indicator LED

void setup() {
  Serial.begin(115200);
  pinMode(LED, OUTPUT);
  setupIMU(); // set up hardware
  motor.setSpeed(10);
  altitude.attach(3);
  
  zeroGyro();

  blinkLED(20, 10); // Let the use know everything initialized

  MsTimer2::set(motorUpdateRate, updateMotor); // accepts ms argument
  MsTimer2::start(); // set up and start a timer interrupt  
}

void loop() {
  if (Serial.available() == 8) {
    int correctionFactorXY = 0;
    int correctionFactorZ = 0;

}

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

float readGyroZ() {
