 int stepperPins[] = {9, 10, 11, 12};
 int stepperSteps = 200;
 Stepper motor(stepperSteps, stepperPins[0], stepperPins[1], stepperPins[2], stepperPins[3]);
+Servo altitude;
 
 int n = 10; // gyro sample
 int k = 200; // servo sample
 int j = 0; // keep track of what to sample
-float runningSum = 0;
+float runningSumXY = 0;
+float runningSumZ = 0;
+
+float currentServoVal = 90;
 
 bool int_flag = false; // interrupt flag for executing gyro read every 100 ms
 const int LED = 13; // indicator LED
-
-char ch[4]; // for receiving commands from Raspberry Pi 3
 
 void setup() {
   Serial.begin(115200);
   pinMode(LED, OUTPUT);
   setupIMU(); // set up hardware
   motor.setSpeed(10);
+  altitude.attach(3);
 
   blinkLED(20, 10); // Let the use know everything initialized
 
   MsTimer2::set(n, a); // accepts ms argument
-  MsTimer2::start(); // set up and start a timer interrupt
-  
+  MsTimer2::start(); // set up and start a timer interrupt  
 }
 
 void loop() {
-  if (Serial.available() == 4) {
-    int correctionFactor = 0;
-    int neg = 0;
-    if (Serial.read() == '1') neg = 1;
-    correctionFactor += (Serial.read() - '0') * 100;
-    correctionFactor += (Serial.read() - '0') * 10;
-    correctionFactor += (Serial.read() - '0');
-    if (neg) correctionFactor -= 2 * correctionFactor;
-    rotateStepperBy(correctionFactor);
+  if (Serial.available() == 8) {
+    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);
+    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(currentServoVal + correctionFactorZ);
   }
   if (int_flag) {
     j++;
     j %= k / n + 1;
-    runningSum += n * readGyro() / 1000;
+    runningSumXY += n * readGyroZ() / 1000;
+    runningSumZ += n * readGyroY() / 1000;
     if (j == 0) {
-      rotateStepperBy(-runningSum); // runningSum is the total theta change
-      runningSum = 0;
+      rotateStepperBy(-runningSumXY); // runningSumXY is the total theta change on the Z axis of rotation
+      runningSumXY = 0;
+      altitude.write(currentServoVal + runningSumZ); // runningSumZ is the total theta change on the Y axis of rotation
+      currentServoVal += runningSumZ;
+      runningSumZ = 0;
     }
   }
 }

  motor.step(steps);  
}

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

void blinkLED(int del, int n) {

/**********************************************************************
 * Arduino Uno Program - Controls Servo and Reads IMU                 *
 * Project: Eclipse Tracking (2017)                                   *
 * Version: 1.0 (3/27/2017)                                           *
 * Max Bowman / Jeremy Seeman / George Moe                            *
 **********************************************************************/
#include <Stepper.h>
#include <Servo.h>
#include <Wire.h>
#include <MsTimer2.h>
#include <SparkFunLSM9DS1.h>
