Code test du capteur gyrscopique L3G4200D avec arduino nano

Description :

Ce projet a pour but de vérifier le bon fonctionnement du capteur de rotation L3G4200D en utilisant une carte Arduino Nano.

Le L3G4200D est un capteur qui mesure la vitesse de rotation sur 3 axes (X, Y et Z). Ce test permet de s’assurer que le capteur communique bien avec l’Arduino et qu’il renvoie des valeurs cohérentes lorsqu’on le fait bouger.

Le gyroscope L3G4200D a un petit défaut naturel appelé la dérive gyroscopique (ou Drift).

Le bouton sert à réinitialiser la précision du système dès que l’on constate que les angles commencent à s’écarter de la réalité à cause du temps qui passe.

Cette dérive peut aussi être atténuée dans le code (voir commentaires dans le code).

Vidéo de démonstration :

Schéma de câblage :

Code Arduino :

#include <Wire.h> // Inclut la bibliothèque pour la communication I2C

#define L3G4200D_ADDR 0x68 // Adresse I2C du capteur L3G4200D
#define BOUTON_PIN 2       // Définit la broche D2 pour le bouton de reset

// --- Variables globales ---
float angleX = 0, angleY = 0, angleZ = 0; // Stockage des angles finaux
float biasX = 0, biasY = 0, biasZ = 0;    // Stockage du décalage (erreur) au repos
unsigned long lastTime;                   // Stocke le temps de la dernière lecture

// Facteur de conversion : pour l'échelle 2000 dps, 1 unité brute = 0.070 degrés/sec
const float sensitivity = 0.070; 

void setup() {
  Serial.begin(115200); // Démarre la communication série rapide
  Wire.begin();         // Initialise le bus I2C
  Wire.setClock(400000); // Augmente la vitesse I2C à 400kHz

  // Configure la broche 2 en entrée avec résistance de pull-up interne
  // Le bouton doit être branché entre D2 et le GND
  pinMode(BOUTON_PIN, INPUT_PULLUP);

  // Configuration du capteur L3G4200D
  // Registre 0x20 : Active le mode normal et les 3 axes à 800Hz
  writeReg(0x20, 0b11111111); 
  // Registre 0x23 : Définit l'échelle de mesure à +/- 2000 dps
  writeReg(0x23, 0b00110000); 

  Serial.println("INITIALISATION...");
  // Effectue la toute première calibration au démarrage
  calibrerGyro(); 
  
  lastTime = micros(); // Initialise le chronomètre en microsecondes
}

void loop() {
  // --- Gestion du bouton de Reset ---
  // Si le bouton est pressé, la broche D2 passe à l'état bas (LOW)
  if (digitalRead(BOUTON_PIN) == LOW) {
    Serial.println("RESET EN COURS...");
    angleX = 0; // Remet l'angle X à zéro
    angleY = 0; // Remet l'angle Y à zéro
    angleZ = 0; // Remet l'angle Z à zéro
    calibrerGyro(); // Recalcule le biais au repos pour stopper la dérive
    lastTime = micros(); // Relance le chronomètre
    delay(300); // Anti-rebond : attend que vous relâchiez le bouton
  }

  // --- Calcul du temps écoulé (dt) ---
  unsigned long currentTime = micros(); // Temps actuel
  float dt = (currentTime - lastTime) / 1000000.0; // Différence en secondes
  lastTime = currentTime; // Sauvegarde pour la prochaine boucle

  // --- Lecture des données ---
  int16_t rawX, rawY, rawZ; // Variables pour les données brutes
  readAllRaw(rawX, rawY, rawZ); // Récupère les données du capteur

  // Conversion en Vitesse Angulaire (degrés par seconde)
  // On soustrait le biais mesuré au repos pour corriger le drift
  float gx = (rawX - biasX) * sensitivity;
  float gy = (rawY - biasY) * sensitivity;
  float gz = (rawZ - biasZ) * sensitivity;

  // --- Filtre de zone morte (Deadzone) ---
  // Si la rotation est inférieure à 0.8 deg/sec, on force à 0 pour éviter la dérive
  // Augmentez 0.5 à 1.5 ou 2.0 pour voir si ça stabilise au repos
  if (abs(gx) < 1.6) gx = 0;
  if (abs(gy) < 2.1) gy = 0;
  if (abs(gz) < 2.0) gz = 0;

  // --- Calcul de l'angle (Intégration) ---
  // Angle actuel = Angle précédent + (Vitesse * Temps)
  angleX += gx * dt;
  angleY += gy * dt;
  angleZ += gz * dt;

  // --- Affichage des résultats ---
  // Format compatible avec le Traceur Série de l'Arduino IDE
  Serial.print("Roll_X:"); Serial.print(angleX); Serial.print("\t");
  Serial.print("Pitch_Y:"); Serial.print(angleY); Serial.print("\t");
  Serial.print("Yaw_Z:"); Serial.println(angleZ);

  delay(5); // Petite pause pour stabiliser la boucle
}

// Fonction pour mesurer le "zéro" du capteur quand il est immobile
void calibrerGyro() {
  long sX = 0, sY = 0, sZ = 0;
  // On prend 400 échantillons pour faire une moyenne précise
  for (int i = 0; i < 400; i++) {
    int16_t x, y, z;
    readAllRaw(x, y, z);
    sX += x; sY += y; sZ += z;
    delay(2);
  }
  // Calcule la valeur moyenne du bruit sur chaque axe
  biasX = sX / 400.0;
  biasY = sY / 400.0;
  biasZ = sZ / 400.0;
  Serial.println("CALIBRATION TERMINEE.");
}

// Fonction pour lire les 3 axes d'un seul coup
void readAllRaw(int16_t &x, int16_t &y, int16_t &z) {
  Wire.beginTransmission(L3G4200D_ADDR);
  // 0x28 est le premier registre. Le bit 0x80 active la lecture multiple.
  Wire.write(0x28 | 0x80); 
  Wire.endTransmission();
  
  Wire.requestFrom(L3G4200D_ADDR, 6); // Demande les 6 octets (2 par axe)
  if(Wire.available() >= 6) {
    x = Wire.read() | (Wire.read() << 8); // Combine octet bas et haut pour X
    y = Wire.read() | (Wire.read() << 8); // Combine octet bas et haut pour Y
    z = Wire.read() | (Wire.read() << 8); // Combine octet bas et haut pour Z
  }
}

// Fonction simplifiée pour écrire une configuration dans le capteur
void writeReg(byte reg, byte val) {
  Wire.beginTransmission(L3G4200D_ADDR);
  Wire.write(reg);   // Adresse du registre
  Wire.write(val);   // Valeur à envoyer
  Wire.endTransmission();
}