void SetSpeedMotor(double *PitchRollYaw, byte Vitesse_0, double Gain)
{
  // Calcul de  la partie variable de la vitesse 
  double P=PitchRollYaw[0];
  double R=PitchRollYaw[1];
  double Y=PitchRollYaw[2];
  
  double Vitesse=Vitesse_0+Gain*(P+R);  // Dépendance R&P: -180<R+P<180
  //double Vitesse=Vitesse_0+Gain*(R);  // Dépendance R: -90<R<90
  //double Vitesse=Vitesse_0+Gain*(P);  // Dépendance P: -90<P<90

  // Mise à jour de la vitesse du moteur 
  M1.write(Vitesse); // 0-180 
}

void ReadGY521_0( long *GyAccTempp)
{
  long  GATCorr_0[NumData]={0,0,0,0,0,0,0};
  ReadGY521(GyAccTemp,GATCorr_0);
}

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

#define NumData     7
#define pi          3.1415926535897932384626433832795  
#define MPU         0x68
#define PinM1       9



// Pinout 
/*
  VCC -> 3.3 V / 5 V (préférable)
  GND -> GND
  SCL -> A5
  SDA -> A4
  
  XDA -> NC (non connecté)
  XCL -> NC 
  ADO -> NC
  INT -> NC
 */



Servo M1;
long  GyAccTemp[NumData];
long  GATCorr[NumData]={0,0,0,0,0,0,0};
double PitchRoll[3]; 

void setup()
{
  // Init module GY-512 
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  
  // Init port série 
  Serial.begin(19200);

  // Affectation du pin au servomoteur  
  M1.attach(PinM1);
}
void loop()
{  
  // Lecture des données: Acce + Temp + Gyro
   
  //ReadGY521(GyAccTemp,GATCorr); // Lecture avec correction 
  ReadGY521_0(GyAccTemp);       // Lecture des données bruts 
 
  // Calcul du Pitch / Roll / Yaw atan(x/sqt(y²+z²)
  ComputeAngle(GyAccTemp, PitchRoll); //+-90
  
  // Mise à jour de la vitesse du moteur  
  SetSpeedMotor(PitchRoll,90, 0.5); // 0(0%) - 180(100%) 
}


void SetSpeedMotor(double *PitchRollYaw, byte Vitesse_0, double Gain)
{
  // Calcul de  la partie variable de la vitesse 
  double P=PitchRollYaw[0];
  double R=PitchRollYaw[1];
  double Y=PitchRollYaw[2];
  
  double Vitesse=Vitesse_0+Gain*(P+R);  // Dépendance R&P: -180<R+P<180
  //double Vitesse=Vitesse_0+Gain*(R);  // Dépendance R: -90<R<90
  //double Vitesse=Vitesse_0+Gain*(P);  // Dépendance P: -90<P<90

  // Mise à jour de la vitesse du moteur 
  M1.write(Vitesse); // 0-180 
}




// Lecture des données des capteurs avec Corrections
void ReadGY521( long *GyAccTempp, long *GATCorrr)
{
  // Init du module GY-521
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,14,true);

  // Lecture des données (3 axes accéléromètre + température + 3 axes gyroscope
  for  (long i=0;i<NumData;i++)
  {
    if(i!=3)
    {
      GyAccTempp[i]=(Wire.read()<<8|Wire.read()) - GATCorrr[i];
    }
    else
    {
      GyAccTempp[i]=(Wire.read()<<8|Wire.read()) - GATCorrr[i];
      GyAccTempp[i] = GyAccTempp[i]/340 + 36.53;
    }
  }
}

// Lecture des données des capteurs sans Corrections
void ReadGY521_0( long *GyAccTempp)
{
  long  GATCorr_0[NumData]={0,0,0,0,0,0,0};
  ReadGY521(GyAccTemp,GATCorr_0);
}

//Conversion des données accéléromètre en pitch/roll/yaw
void ComputeAngle(long *GyAccTempp,  double *PitchRol)
{  
  double x,y,z, X, Y, Z;
  x= (double)GyAccTempp[0];
  y= (double)GyAccTempp[1];
  z= (double)GyAccTempp[2];

  X=sqrt((y*y) + (z*z)); 
  Y=sqrt((x*x) + (z*z)); 
  Z=sqrt((x*x) + (y*y)); 
  
  PitchRol[0] = atan(x/X); //  pitch 
  PitchRol[1] = atan(y/Y); // roll
  PitchRol[2] = atan(z/Z); // yaw
  
  //Conversion Radian en degré
  PitchRol[0] = PitchRol[0] * (180.0/pi);
  PitchRol[1] = PitchRol[1] * (180.0/pi) ;
  PitchRol[2] = PitchRol[2] * (180.0/pi) ;
}