/*Programme à compléter lorsque **** */
#include <MPU6050.h>      //MPU6050 library
#include <Wire.h>        //IIC communication library

MPU6050 mpu;     //Instantiate an MPU6050 object; name mpu
int16_t ax, ay, az, gx, gy, gz;     //Define three-axis acceleration, three-axis gyroscope variables

float angle_acc,angle_gyro;   //angle variable
float dt=0.005,K=0.40;    // dt durée de la boucle cadencée
                          // K=0.40 : 40% angle accéléromètre et 60% angle Gyroscope

const int N=100;         // Filtre moyenneur sur 100 mesures
float Fifo[N],moyenne;

float Gyro_y,Gyro_x;   //Angular velocity variable
// Offset gyroscope déterminé dans programme offset
int offset_gx=*****,offset_gy=****, offset_gz=****;
int  offset_ax=****, offset_ay=***, offset_az=*****;

unsigned long t0,i;


void setup() 
{
  // Join the I2C bus
  Wire.begin();                   //Join the I2C bus sequence
  Serial.begin(9600);            //open serial monitor and set the baud rate to 9600
  delay(1500);
  mpu.initialize();              //initialize MPU6050
  delay(2);
}

void loop() 
{
  t0=micros();// top chrono pour un cycle de dt = 0,005 s soit 5000 µs

  // Lecture mpu6050 et calcul angle_acc   
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);     //IIC to get MPU6050 six-axis data  ax ay az gx gy gz
  ay=ay+offset_ay;az=az+offset_az;                    // correction d'offset
  angle_acc = *1*;           //*1* Formule de calcul de l'angle de tangage. Signe selon convention : + = avant

  // Le filtre moyenneur
  Fifo[i%N]=*2*;       // *2* Mise en place d'une file Fifo de N éléments angle_acc 
  *3*;                // *3* et *4* Calcul de la somme des N éléments de la file Fifo
  *4*;
  moyenne=moyenne/N;

  // Calcul angle gyro avec filtre complémentaire
  Gyro_x =*5*;   // *5* Correction offset + Calcul vitesse en °/s (sensibilité : 131 LSB/(°/s))
  angle_gyro=*6*;
  if (i<=N) angle_gyro=moyenne;// au début on utilise que angle_acc
              
  if (i%20==0) {// Transmission toutes les 20*5ms = 100ms
    Serial.print(angle_acc); // Pour exploitation par Labview transmission au format
    Serial.print(";");       // angle_acc;moyenne;angle_gyro
    Serial.print(moyenne);
    Serial.print(";");
    Serial.println(angle_gyro);
    }
  
  // Possibilité à Labview de modifier K
  if (Serial.available()) {
    K=Serial.parseFloat();
    while (Serial.available()) Serial.read();}
    
  while (*7*);  // *7* attente pour boucle cadencée à dt=5000µs
  i++;// incrémentation du compteur de nombre de cycles
}