Controlling pan/tilt mechanism with GY-87 IMU module

#include <Servo.h>
#include "I2Cdev.h"
#include "MPU6050.h"
#include "HMC5883L.h"
#include "Wire.h"
Servo servobase;
Servo servoelbow;
int servoPos = 90; //
int compassCorrection = 0; // set to 0 (not used)
static const char LED = 6;
static const float ACCEL_SENS = 16384.0; // Accel Sensitivity with default +/- 2g scale
static const float GYRO_SENS  = 131.0;   // Gyro Sensitivity with default +/- 250 deg/s scale
HMC5883L mag;
int16_t mx, my, mz;
MPU6050 accelgyro ;
int16_t ax, ay, az ;
int16_t gx, gy, gz ;
void setup()
{
boolean state = HIGH;
unsigned int count = 0;
Serial.begin(9600);
  while (!Serial && (count < 30) )
  {
    delay(200); 
    digitalWrite(LED, state);
    state = !state;
    count++;
  }
  digitalWrite(LED, HIGH);
  Wire.begin();
  accelgyro.initialize();
  mag.initialize();
}
void loop()
{
  servobase.attach(9);
servoelbow.attach(6);
  static unsigned long ms = 0;
  static boolean state = HIGH;
if (millis() - ms > 100)
  {
 accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
 mag.getHeading(&mx, &my, &mz);
  float heading = atan2(my, mx);
  if(heading < 0) heading += 2 * M_PI;
  float headingDegrees = heading * 180/M_PI; 
  int headingDegreesInteger = headingDegrees;
if ((headingDegreesInteger - compassCorrection) >= 0)
  {
    headingDegreesInteger = headingDegreesInteger  - compassCorrection;
  }
  else
  {
    headingDegreesInteger = headingDegreesInteger - compassCorrection + 360;
  }
  if (headingDegreesInteger > 90 && headingDegreesInteger < 270)
  {
    if (headingDegreesInteger <= 180)
    {
      servoPos = 180;
    }
    else
    {
      servoPos = 10;
    }
  }
  else
  {
    if (headingDegreesInteger <= 90)
    {
      servoPos = map(headingDegreesInteger, 0, 90, 85, 10);
    }
    else
    {
      servoPos = map(headingDegreesInteger,360, 270, 85, 180); //(360,270,85,180)
    }
  }
  servobase.write(servoPos);
  Serial.println (servoPos);
  delay(10);
accelgyro.getMotion6 (&ax, &ay, &az, &gx, &gy, &gz);
ay = map (ay, -17000, 17000, 0, 180) ;
Serial.println (180-ay);
servoelbow.write (180-ay); 
delay(10);
    ms = millis();
    digitalWrite(LED, state);
    state = !state;
  }
}