   setupIMU(); // set up hardware
   motor.setSpeed(10);
   altitude.attach(3);
-  
+
+  altitude.write(currentYBodyPosition);
   zeroGyro();
 
   blinkLED(20, 10); // Let the use know everything initialized

    correctionFactorXY += (Serial.read() - '0');
    if (negXY) correctionFactorXY = -correctionFactorXY;
    rotateStepperBy(correctionFactorXY);
    currentZBodyPosition += correctionFactorXY;
    currentZMotorPosition += 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);
    currentYBodyPosition += correctionFactorZ;
    currentYMotorPosition += correctionFactorZ;
    blinkLED(500, correctionFactorZ);
  }


  timer = currentTime;

  if(update_flag) {
    altitude.write(currentYBodyPosition);
    rotateStepperBy(currentZBodyPosition - currentZMotorPosition);
    currentZMotorPosition = currentZBodyPosition;
    sampleCounter = 0;
    update_flag = false;
