//-------------------------------------------------------------------

//     DEFINITION DES FONCTIONS , DES VARIABLES, 
//          DES BIBLIOTHEQUES ET DE L'APPLI POUR LE BLUETOOTH

//-------------------------------------------------------------------

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include <avr/wdt.h>      //Bibliotheque du Watchdog pour Reset en cas de Freezing de données entre IMU et Arduino
  
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
      #include "Wire.h"
  #endif
  
#include <AFMotor.h>                                      

void IMU_Init();
void Moteurs_Init();
float IMU_GetAngle(int);
float PID_calculVitesseMoteurs(float,float,float,float,float,float, float);
float PI_Yaw(float,float);
void Moteurs_running(float,float,float);
void Moteurs_braking();

//////////////////////////////////////////////////////////////////////////////////////////////
//        RemoteXY include library          //
//////////////////////////////////////////////

// Si problème de connection avec le téléphone: s'assurer que l'appli est à jour, que la librairie est à jour dans Arduino
// et tout recompiler


// RemoteXY select connection mode and include library 
#define REMOTEXY_MODE__HARDSERIAL

#include <RemoteXY.h>

// RemoteXY connection settings 
#define REMOTEXY_SERIAL Serial3
#define REMOTEXY_SERIAL_SPEED 9600

// RemoteXY configurate  
#pragma pack(push, 1)
uint8_t RemoteXY_CONF[] =
  { 255,26,0,22,0,255,0,8,13,4,
  130,2,67,14,23,20,3,8,130,2,
  67,36,23,20,3,8,2,0,4,2,
  22,11,0,27,8,31,31,79,78,0,
  79,70,70,0,129,0,5,19,19,7,
  0,13,65,110,103,108,101,0,67,1,
  67,46,23,8,3,133,26,11,129,0,
  71,15,14,5,3,0,65,110,103,108,
  101,0,67,1,67,25,21,8,3,1,
  8,11,131,1,29,2,23,5,3,7,
  31,67,111,109,109,97,110,100,101,115,
  0,1,0,49,26,14,14,3,1,31,
  69,81,0,129,0,68,20,22,5,3,
  0,69,113,117,105,108,105,98,114,101,
  0,129,0,72,39,14,5,3,0,65,
  110,103,108,101,0,5,32,6,18,38,
  38,3,5,26,31,131,0,57,2,23,
  5,1,1,31,80,97,114,97,109,101,
  116,114,101,115,0,7,44,20,20,20,
  5,1,2,26,2,2,7,44,20,29,
  20,5,1,2,26,2,3,7,44,20,
  38,20,5,1,2,26,2,2,7,44,
  20,46,20,5,1,2,26,2,2,7,
  44,66,20,20,5,1,2,26,2,3,
  1,1,91,14,8,9,3,34,31,43,
  0,1,1,91,24,8,9,3,34,31,
  45,0 };
  
// this structure defines all the variables of your control interface 
struct {

    // input variable
  uint8_t switch_1; // =1 if switch ON and =0 if OFF 
  uint8_t button_Equilibre; // =1 if button pressed, else =0 
  int8_t joystick_1_x; // =-100..100 x-coordinate joystick position 
  int8_t joystick_1_y; // =-100..100 y-coordinate joystick position 
  float Kp_editField;
  float Ki_editField;
  float Kd_editField;
  float AWU_editField;
  float coeffDG_editField;
  uint8_t button_plus; // =1 if button pressed, else =0 
  uint8_t button_moins; // =1 if button pressed, else =0 

    // output variable
  char angleTheta[11];  // string UTF8 end zero 
  char angleEqui[11];  // string UTF8 end zero 

    // other variable
  uint8_t connect_flag;  // =1 if wire connected, else =0 

} RemoteXY;
#pragma pack(pop) 

/////////////////////////////////////////////
//           END RemoteXY include          // 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////


////////////////   VARIABLES Mesure + Controle  //////////////////////////////
float angleEquilibre = 0;

unsigned long currentTime;
unsigned long previousTime;
unsigned long tempsEcoule;

float Theta;
float VitesseAngulaire;
float previousTheta;                 //variable utilisée pour calcul de la vitesse angulaire
float Theta_Futur;
int tempoMesureMPU = 5;            //Mesure de l'angle Theta toutes les XX millisecondes
boolean dataAngleReady = false;     //Valeurs transmises par le gyroscope sont exploitables si TRUE
int mesureMPU_it = 0;

float Kp;
float Ki;
float Kd;
float AWU;

float vitesseMoteurs;
int orientation;

float Theta_Val[20];
int j;
/*
float theta_max;
float theta_min;
float Fluctuation_Theta = 0.5;
*/

float YAW = 0;
float YAW_Ref = 0;

float coeffMotDG = 0.8;
float angleEquilibreJoystick;
float previousJoystickY;


//unsigned long timeINIT;

/////////////////////////////////////////////////////////////////////////////////

//--------------------------------------------------------------

//                          SETUP

//---------------------------------------------------------------

void setup() {
  
    RemoteXY_Init (); 
    IMU_Init();
    Moteurs_Init();
    
//8888888888888888888888888888888888    
//    timeINIT = millis();
}

//---------------------------------------------------------------------------------------------------------------------------------------------------------------------
//-----                                                     -----
//                    PROGRAMME PRINCIPAL
//-----                                                     -----
//---------------------------------------------------------------

void loop() {

//Watch Dog pour faire un reset si bug pendant 2Secondes);
  wdt_enable(WDTO_2S);
  RemoteXY_Handler ();
      
//Affichage des valeurs d'angle
  dtostrf(angleEquilibre, 0, 2, RemoteXY.angleEqui);
  dtostrf(Theta, 0, 2, RemoteXY.angleTheta);

//Recup des valeurs de Kp,Ki,Kd
            Kp = 75;
            Ki = 1.8;
            Kd = 8;
            AWU = 1.5;
            
  if (RemoteXY.Kp_editField != 0){ Kp = RemoteXY.Kp_editField;} 
  if (RemoteXY.Ki_editField != 0){Ki = RemoteXY.Ki_editField;} 
  if (RemoteXY.Kd_editField != 0){Kd = RemoteXY.Kd_editField;} 
  if (RemoteXY.AWU_editField != 0){AWU = RemoteXY.AWU_editField;}  //pour tests


if (RemoteXY.coeffDG_editField != 0){coeffMotDG = RemoteXY.coeffDG_editField;}  

//Actions si bouton EQ + et - sont préssés
  if (RemoteXY.button_Equilibre==1){
     angleEquilibre = Theta;
  }
  if (RemoteXY.button_plus==1){
     angleEquilibre = angleEquilibre + 0.0005;
  }
  if (RemoteXY.button_moins==1){
     angleEquilibre = angleEquilibre - 0.0005;
  }

currentTime = millis();
tempsEcoule = currentTime - previousTime;

//----------------------------------------------------------------------------------------------------------- 
//Mesure des angles Theta et YAW
      if (tempsEcoule  >= tempoMesureMPU ){
            
            Theta = IMU_GetAngle(1);
            YAW = IMU_GetAngle(2);
          
           VitesseAngulaire  = (Theta - previousTheta)*1000/tempsEcoule;
           Theta_Futur =  2*Theta - previousTheta;
          
            mesureMPU_it++;
          
           //Suppression des Points Fous et Validations des Données
                        if ( (abs(VitesseAngulaire)> 720) ||  (Theta==previousTheta) ) {
                          dataAngleReady = false;
                          mesureMPU_it = 0;
                        }
                        
                        if (mesureMPU_it >= 2) {
                          dataAngleReady = true;                          
                          Theta_Val[j]=Theta; //Tableau dans lequel on enregistre les 20 dernières valeurs de Theta, sert à calculer la fluctuation d'angle
                          j++;
                        }
            
            if (j>19){
              j=0;
            }
            previousTheta = Theta;
            previousTime = millis();
      }
         
      wdt_reset();
//----------------------------------------------------------------------------------------------------------- 
// Calcul de la fluctuation d'angle Theta
/*
      theta_max = Theta_Val[0];
      theta_min = Theta_Val[0];
      for (int theta_it=1;theta_it<=19;theta_it++) {
        theta_max = max(Theta_Val[theta_it],theta_max);
        theta_min = min(Theta_Val[theta_it],theta_min);
      }

      Fluctuation_Theta = theta_max-theta_min;
      dtostrf(Fluctuation_Theta, 0, 2, RemoteXY.Fluct_Ang);
*/
      
//----------------------------------------------------------------------------------------------------------- 
// Actions à effectuer lorsque l'on active le pendule
  
           if (RemoteXY.switch_1==1){               
          if (dataAngleReady == true) { //Le calcul a lieu seulement si les données sont considérées comme valides
            
              angleEquilibreJoystick = angleEquilibre + ((float)(RemoteXY.joystick_1_y))/250;

             //Calcul de la vitesse des moteurs en utilisant un PID
             vitesseMoteurs = PID_calculVitesseMoteurs(angleEquilibreJoystick,Theta,Theta_Futur,Kp,Ki,Kd,AWU);
             
             YAW_Ref = YAW_Ref + ((float)(RemoteXY.joystick_1_x))/1000;

              if (YAW_Ref > 180) {
                YAW_Ref = YAW_Ref - 360;
              } else if (YAW_Ref < -180){
                YAW_Ref = YAW_Ref + 360;
              }
              
              orientation = PI_Yaw(YAW,YAW_Ref);

              if (RemoteXY.joystick_1_x != 0) {
                orientation = orientation * 3;
              }
     
          }        
                  Moteurs_running(vitesseMoteurs, orientation, coeffMotDG);        
      }   
      else {
//Si le switch est sur OFF les moteurs freinent et toutes les variables pour les calculs sont remises à zéro
                  Moteurs_braking();
                  vitesseMoteurs = PID_calculVitesseMoteurs(0,0,0,0,0,0,0);

                  orientation = PI_Yaw(0,0);
                  YAW_Ref = YAW;  //Reference determinée juste avant d'activer le switch sur ON

      }
}
