La phase de programmation est une étape cruciale dans le développement de ce robot auto-stabilisé, car elle implique l’implémentation de l’algorithme PID (Proportionnel-Intégral-Dérivé) sur le microcontrôleur ESP32. L’objectif principal de cette phase est de mettre en place une méthode de stabilisation permettant au robot de rester en équilibre de façon stable et réactive. Cela se fait en temps réel, en s’appuyant sur la fusion de données provenant de capteurs inertiels (accéléromètre triaxial et gyroscope) via un filtre complémentaire pour une estimation précise de l’angle d’inclinaison. Le programme ESP32, avec son contrôle en boucle fermée, sera le cœur du système, assurant une bonne communication entre les différents éléments et permettant le pilotage du robot ainsi que la récupération des informations des capteurs.

Vous trouverez ci-dessous le programme de notre projet:

#include

#include

#include

#include

// Définition des broches des moteurs

const int MOTOR1_PWM = D10;

const int MOTOR1_DIR = D9;

const int MOTOR2_PWM = D8;

const int MOTOR2_DIR = D7;

MPU6050 mpu;

// PID pour la stabilisation

double setpoint = 171.05; 

double input, output;

double Kp = 120, Ki = 0.001, Kd = 2.5; 

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

// Variables pour le filtre complémentaire

float angle = 0; // Angle calculé

float gyroRate = 0; // Vitesse angulaire

unsigned long lastTime = 0;

// Variables pour la calibration

float gyroBiasY = 0.0f;

// Fonctions utilitaires

    void calibrateMPU() {
    
      Serial.println("Calibration du gyroscope...");
      
      const int numSamples = 1000;
      
      long sumY = 0;

      for (int i = 0; i < numSamples; i++) {
        
        int16_t gx, gy, gz;
        
        mpu.getRotation(&gx, &gy, &gz);
        
        sumY += gy;
        
        delay(2);
         }

        gyroBiasY = sumY / (float)numSamples;
          
        Serial.println("Calibration terminée !");
        }
        
        void setMotor(int pwm, bool forward, int motorDirPin, int motorPwmPin) {
        
          pwm = constrain(pwm, 0, 255);
          
          digitalWrite(motorDirPin, forward ? LOW : HIGH);
          
          analogWrite(motorPwmPin, pwm);
        }
       
        float getFilteredAngle() {
        
          int16_t ax, ay, az, gx, gy, gz;
          
          mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

// Calcul de l’angle avec l’accéléromètre

 float accelAngle = atan2((float)ay, (float)az) * 180.0f / PI;

// Calcul de la vitesse angulaire corrigée

  gyroRate = ((float)gy - gyroBiasY) / 131.0f; // Conversion en degrés/s

// Filtre complémentaire

 unsigned long currentTime = millis();
 
  float dt = (lastTime == 0) ? 0.01f : (currentTime - lastTime) / 1000.0f; // 10 ms par défaut au premier appel
  
  lastTime = currentTime;
  
  if (dt <= 0.0f || dt > 0.2f) dt = 0.01f; // Sécurité sur dt

// Filtre complémentaire (98% gyro, 2% accéléro)

  angle = 0.98f * (angle + gyroRate * dt) + 0.02f * accelAngle;

  return angle;
 
  //return gy;
}











    void setup() {

      Serial.begin(115200);

 
      pinMode(MOTOR1_PWM, OUTPUT);
    
      pinMode(MOTOR1_DIR, OUTPUT);
      
      pinMode(MOTOR2_PWM, OUTPUT);
      
      pinMode(MOTOR2_DIR, OUTPUT);
    
      Wire.begin();
      
      mpu.initialize();
      
      if (!mpu.testConnection()) {
      
        Serial.println("Erreur : MPU6050 non détecté !");
        
        while (1);
          }
    
      calibrateMPU();
     /*
      
      while(1){
      
        Serial.println(getFilteredAngle(),DEC);
        
        setMotor(100,true,MOTOR1_DIR,MOTOR1_PWM);
        
        setMotor(100,true,MOTOR2_DIR,MOTOR2_PWM);
        
        delay(500);
          }
    */
      pid.SetMode(AUTOMATIC);
      
      pid.SetOutputLimits(-90, 90);
    
      Serial.println("Initialisation terminée !");
        }

void loop() {

// Lecture de l’angle filtré

  input = getFilteredAngle();

// Calcul PID pid.Compute();

// Sécurité : coupe les moteurs si le robot est trop penché if (abs(input - setpoint) > 40) {

    setMotor(0, true, MOTOR1_DIR, MOTOR1_PWM);
    
    setMotor(0, true, MOTOR2_DIR, MOTOR2_PWM);
     
    setpoint = getFilteredAngle(); // Réinitialise la consigne à la position actuelle
} else {
   

// Les deux moteurs reçoivent la même commande, sens selon le signe de output

    int pwm = abs((int)output);
    
    
  bool forward = (output < 0);
  
    setMotor(pwm, forward, MOTOR1_DIR, MOTOR1_PWM);
    
    setMotor(pwm, forward, MOTOR2_DIR, MOTOR2_PWM);
  }

// Affichage pour le débogage

  Serial.print("Angle: ");
  
  Serial.print(input);
  
  Serial.print(" | PID Output: ");
  
  Serial.println(output);

delay(20); // Fréquence de mise à jour
}