// Définition capteur ultrason
#define Broche_Echo 29 // Broche Echo du HC-SR04 sur D7 //
#define Broche_Trigger 23 // Broche Trigger du HC-SR04 sur D8 //

// Definition des variables
int MesureMaxi = 300; // Distance maxi a mesurer //
int MesureMini = 3; // Distance mini a mesurer //
long Duree;
long Distance_mesuree;

int bouton_urgence = 0;

double distance_cible_atterrissage = 80; // Distance d'atterrissage à atteindre en cm

double kp_atterrissage = 1;
double ki_atterrissage = 0;
double kd_atterrissage = 0;


void setup(){
  pinMode(Broche_Trigger, OUTPUT); // Broche Trigger en sortie //
  pinMode(Broche_Echo, INPUT); // Broche Echo en entree //
  Serial.begin (9600);
}

void loop() {
  mesure_capteur_ultrason();
  Serial.print(phase_de_vol_verticale_atterrissage_1(mesure_capteur_ultrason(),kp_atterrissage,ki_atterrissage,kd_atterrissage,distance_cible_atterrissage));
  Serial.print("\t");
  delay(1000);
}

double mesure_capteur_ultrason(void) {
  // Initialisation
  digitalWrite(Broche_Trigger, LOW);  
  delayMicroseconds(2);
  digitalWrite(Broche_Trigger, HIGH);  
  delayMicroseconds(10);
  digitalWrite(Broche_Trigger, LOW);  
  Duree = pulseIn(Broche_Echo, HIGH);
  Distance_mesuree = Duree * 0.034 / 2; 

  if (Distance_mesuree >= MesureMaxi || Distance_mesuree <= MesureMini) {
    Distance_mesuree = 10000;
  } else {
    return (Distance_mesuree);
  }
}


double phase_de_vol_verticale_atterrissage_1(double capteur, double kp, double ki, double kd, double distance) {
  
  if (capteur == 10000) {
    return (bouton_urgence = 1);
  }
  else {
    double v = calculer_PID(mesure_capteur_ultrason(),distance_cible_atterrissage,kp_atterrissage,ki_atterrissage,kd_atterrissage);
    return (v);
  }
}

double phase_de_vol_verticale_atterrissage_2 (double capteur){
  if (capteur == 10000) {
    return (bouton_urgence = 1);
  }
  else {
    double Fausse_variable_hauteur = -5.00;
    return(Fausse_variable_hauteur);
  }
}

double calculer_PID(double distance_actuelle, double consigne, double kp, double ki, double kd) {
  static double erreur_precedente = 0;
  static double somme_erreurs = 0;
  double erreur = consigne - distance_actuelle;
  double P = kp * erreur;
  somme_erreurs += erreur;
  double I = ki * somme_erreurs;
  double D = kd * (erreur - erreur_precedente);
  double sortie_pid = P + I + D;
  erreur_precedente = erreur;
  return sortie_pid;
}