/ Bibliothèque pour la gestion des capteurs à ultrasons
#include <NewPing.h>
#include <Ultrasonic.h>
#include <PID_v1.h>

// Broches du capteur ultrason
#define TRIGGER_PIN A0
#define ECHO_PIN A1
// Définir la distance maximale à mesurer (en cm)
#define MAX_DISTANCE 200
#define MIN_DISTANCE 3
// Paramètres PID
double consigne = 20.0; // Distance cible en cm

double input, output, setpoint;

double Kp = 1.0, Ki = 0.0, Kd = 0.0; // Ajustez ces valeurs en fonction de vos besoins

PID myPID(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

// Créer une instance de la classe NewPing
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);



void setup() {
  pinMode(TRIGGER_PIN, OUTPUT); //Broche trigger en sortie
  pinMode(ECHO_PIN, INPUT);     // Broche ECHO en entrée
  

  // Initialiser le PID
  myPID.SetMode(AUTOMATIC);
  myPID.SetSampleTime(50); // Temps d'échantillonnage en ms
  myPID.SetOutputLimits(-255, 255); // Limite de la sortie du PID, adaptée à votre moteur
  
  // Définir la consigne initiale
  setpoint = consigne;
  Serial.begin(9600);

}

void loop() {
  mesure_capteur();

  // Calculer la sortie du PID
  myPID.Compute();

  // Appliquer la sortie du PID au moteur (exemple)
  // Vous devrez adapter cela en fonction de votre configuration matérielle
  //analogWrite(motorPin, abs(output));
  // Attendre un court moment avant la prochaine mesure
  delay(50);

}


double mesure_capteur(void){
  // Mesurer la distance en centimètres
  input = sonar.ping_cm();
  if (input >=MAX_DISTANCE || input <= MIN_DISTANCE){
    input= 6969;
  }else{
    return(input)
  }
  // Afficher les valeurs pour le débogage
  Serial.print("Input: ");
  Serial.print(input);
  Serial.print(" cm");
}