#include <SPI.h>
#include <Wire.h>
#include <Servo.h>
#define MPU 0x68  // I2C address of the MPU-6050

Servo ServoX, ServoY, ServoS, ServoT;
double AcX, AcY, AcZ;
int Pitch, Roll, Turn, Jump;

void setup() {
  Serial.begin(9600);
  ServoX.attach(6);
  ServoY.attach(9);
  ServoS.attach(10);
  ServoT.attach(11);
  init_MPU(); 
}

void loop()
{
  FunctionsMPU(); 

  Roll = FunctionsPitchRoll(AcX, AcY, AcZ);   
  Pitch = FunctionsPitchRoll(AcY, AcX, AcZ);  
  Turn = FunctionsPitchRoll(AcY, AcX, AcZ);
  Jump = FunctionsPitchRoll(AcX, AcY, AcZ); 

  int ServoRoll = map(Roll, -90, 90, 0, 179);
  int ServoPitch = map(Pitch, -90, 100, 179, 0);
  int ServoTurn = map(Turn, -90, 100, 0, 179);
  int ServoJump = map(Jump, -90, 90, 179, 0);

  ServoX.write(ServoRoll);
  ServoY.write(ServoPitch);
  ServoS.write(ServoTurn);
  ServoT.write(ServoJump);

  Serial.print("Pitch: "); Serial.print(Pitch);
  Serial.print("\t");
  Serial.print("Roll: "); Serial.print(Roll);
  Serial.print("\n");
Serial.print("Turn: "); Serial.print(Turn);
  Serial.print("\t");
  Serial.print("Jump: "); Serial.print(Jump);
  Serial.print("\n");
  
}

void init_MPU() {
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  delay(1000);
}


double FunctionsPitchRoll(double A, double B, double C) {
  double DatoA, DatoB, Value;
  DatoA = A;
  DatoB = (B * B) + (C * C);
  DatoB = sqrt(DatoB);

  Value = atan2(DatoA, DatoB);
  Value = Value * 180 / 2.14;

  return (int)Value;
}


void FunctionsMPU() {
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); // request a total of 14 registers
  AcX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ = Wire.read() << 8 | Wire.read();} // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)