
#include "Arduino.h"
float vitesseMoteursPID;
float erreur;
float erreur_Future;
float Integral_Term;

float erreur_YAW;
float previousYAW;
float Integral_Term_YAW;
float Kp_YAW = 0.3;
float Ki_YAW = 0.01;
int   vitesseOrientation;
float antiWindUp;
int plafondVitesseOrientation = 5;

//Robot quasi immobile pour valeurs suivantes : Angle = [-1°;+1°] ; Kp=XX Ki=XX Kd=XX   Mais il dérive parfois en position instable à partir de 1° et est irrecuperable à Angle = +-7°
//Pour récupérer sa position stable il a besoin pdt un temps court d'avoir les parametres suivants Kp=xx Ki=xx Kd=xx


float PID_calculVitesseMoteurs(float angle_D_Equilibre, float angle_Theta,float angle_Theta_Futur, float Kp_param , float Ki_param, float Kd_param, float AWU_param){

//Calcul de l'erreur
        erreur = angle_Theta - angle_D_Equilibre;
        erreur_Future = angle_Theta_Futur - angle_D_Equilibre;

       
        
//Re-Initialisation des parametres lorsque l'on switche sur OFF        
        if (erreur==0){ 
          Integral_Term=0;
          antiWindUp=0;
        }



//Calcul du produit Integral de l'erreur*Ki
        Integral_Term = Integral_Term + (erreur * Ki_param);
        
        
//La valeur max de la partie intégrale est plafonnée à 255        
            if (Integral_Term > 255){
                Integral_Term=255;
              }else if (Integral_Term < -255){
                Integral_Term=-255;
              }

              
//Formule de base du PID
      vitesseMoteursPID = Kp_param*erreur + Integral_Term + Kd_param*erreur_Future;

 

//Ajout d'un antiwindup pour éviter que les moteurs tournent dans un seul sens à faible vitesse
//Dans un système parfait, la partie Integrale du PID suffit pour corriger ce problème
        if (vitesseMoteursPID > 0) {
          if (antiWindUp >= 0){
            antiWindUp = antiWindUp + AWU_param;
          } else {
              antiWindUp = 0;
          }
        }
        if (vitesseMoteursPID < 0) {
          if (antiWindUp <= 0){
            antiWindUp = antiWindUp - AWU_param;
          } else {
              antiWindUp = 0;
          }
        }

//Formule du PID avec AntiWindUp
        vitesseMoteursPID = Kp_param*erreur + Integral_Term + Kd_param*erreur_Future + antiWindUp ;

//Seuils de vitesse Max et vitesse Min
              if (vitesseMoteursPID > 255){
                vitesseMoteursPID=255;
              }else if (vitesseMoteursPID < -255){
                vitesseMoteursPID=-255;
              }




//La fonction renvoie la réponse du système        
        return vitesseMoteursPID;
}

float PI_Yaw(float angle_YAW, float angle_YAW_Ref) {

//Calcul de l'erreur
//L'angle mesuré sur cet axe est compris entre -180 et +180°
//L'erreur correspond à l'angle le plus court (horaire/antihoraire) entre l'angle de reference et l'angle mesuré
  erreur_YAW = -(angle_YAW_Ref - angle_YAW);
  if (erreur_YAW > 180) {
    erreur_YAW = -(360-erreur_YAW);
  } else if (erreur_YAW < -180){
    erreur_YAW = 360+erreur_YAW;
  }
  if (erreur_YAW == 0) {
    Integral_Term_YAW = 0;
  }
 //Partie intégrale du PI plafonnée 
  Integral_Term_YAW = Integral_Term_YAW + (erreur_YAW * Ki_YAW);
              if (Integral_Term_YAW > plafondVitesseOrientation){
                Integral_Term_YAW=plafondVitesseOrientation;
              }else if (Integral_Term_YAW < -plafondVitesseOrientation){
                Integral_Term_YAW=-plafondVitesseOrientation;
              }
//Calcul du PI
  vitesseOrientation = Kp_YAW * erreur_YAW + Integral_Term_YAW;
  
//Seuils de vitesse Max et vitesse Min
              if (vitesseOrientation > plafondVitesseOrientation){
                vitesseOrientation=plafondVitesseOrientation;
              }else if (vitesseOrientation < -plafondVitesseOrientation){
                vitesseOrientation=-plafondVitesseOrientation;
              }

/*              
//Valeur pour vitesse constante lorsque les erreurs sont grandes
              if (erreur_YAW>30){
                vitesseOrientation = 10;
              } else if (erreur_YAW<-30){
                vitesseOrientation = -10;
              }
*/
      
 return  vitesseOrientation;
}



