#include <Wire.h>
#include <Servo.h>
#include <MPU6050.h>


Servo Xaxle,Yaxle;
MPU6050 sensor;


int16_t ax, ay, az;
int16_t gx, gy, gz;


int ax0=0;
int ay0=0;


void setup()
{
  Wire.begin();//Iniciar a conexão serial I2c
  Serial.begin(115200);
  Xaxle.attach(5);//pino para o servo no roll
  Yaxle.attach(6);//pino para o servo no Yall

  sensor.initialize();//Iniciar o sensor 
  motions();
}


void loop()
{
  sensor.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  ax=map(ax,-17000,17000,0,180);
  movement_X();
  Serial.println(ay);
  ay=map(ay,-17000,17000,0,180);
  Serial.println("y axle value :");
  Serial.println(ay);
  movement_y();
  
}


void motions()
{
  sensor.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  ax0=map(ax,-17000,17000,0,180);
  ay0=map(ay,-17000,17000,0,180);
}


void movement_X()
{
  if(ax > ax0)
  {
    for(int i= ax0; i<= ax; i++)
    {
      Xaxle.write(i);
      delay(5);
    }
  }
  else
  {
    for(int i= ax0; i >= ax; i--)
    {
      Xaxle.write(i);
      delay(5);
    }
  }
  ax0=ax;
}

void movement_y()
{
  if(ay > ay0)
  {
    for(int j= ay0; j<= ay; j++)
    {
      Yaxle.write(j);
      delay(5);
    }
  }
  else
  {
    for(int j= ay0; j >= ay; j--)
    {
      Yaxle.write(j);
      delay(5);
    }
  }
  ay0=ay;
}