Moving Laser Radar

#include <Arduino.h> // Bibliothque principale Arduino
#include <Wire.h> // Pour la communication I2C
#include <VL53L0X.h> // Pour le capteur de distance VL53L0X
#include <Adafruit_GFX.h> // Bibliothque graphique pour l'cran
#include <Adafruit_ILI9341.h> // Bibliothque pour le contrleur d'cran ILI9341
#include <Servo.h> // Pour contrler les servomoteurs

#define TFT_CS 10 // Broche Chip Select de l'cran
#define TFT_DC 8  // Broche Data/Command de l'cran
#define TFT_RST 9 // Broche Reset de l'cran

const int SERVO = 4; // Broche de contrle du servomoteur

Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST); // Objet cran
Servo servoX; // Objet servo
VL53L0X sensor; // Objet capteur de distance

// Fonction pour dessiner un arc de cercle (radar) sur l'cran
void drawRadarArc(int x0, int y0, int radius, int startAngle, int endAngle, uint16_t color) {
  for (int angle = startAngle; angle <= endAngle; angle++) { // Parcours des angles de l'arc
    float rad = angle * 3.14159265 / 180.0; // Conversion en radians
    int x = x0 + radius * cos(rad); // Calcul de la position x du point
    int y = y0 - radius * sin(rad); // Calcul de la position y du point
    tft.drawPixel(x, y, color); // Dessin du pixel sur l'cran
  }
}

void setup() {
  Serial.begin(115200); // Initialisation de la communication srie
  Wire.begin(); // Initialisation du bus I2C
  if (!sensor.init()) { // Initialisation du capteur de distance
    Serial.println("Erreur : Capteur VL53L0X non dtect !"); // Message d'erreur si capteur absent
    while (1); // Boucle infinie si erreur
  }
  sensor.setTimeout(500); // Dfinition du timeout du capteur
  sensor.startContinuous(); // Dmarrage des mesures continues du capteur
  servoX.attach(SERVO); // Attache le servomoteur  la broche dfinie
  tft.begin(); // Initialisation de l'cran
  tft.fillScreen(ILI9341_BLACK); // Efface l'cran (fond noir)
  Serial.println("Initialisation termine"); // Message de fin d'initialisation
}

void loop() {
  const int x_centre = 120; // Coordonne x du centre du radar
  const int y_centre = 160; // Coordonne y du centre du radar
  const int rayon = 100; // Rayon du radar (en pixels)
  const int distance_max = 1000; // Distance maximale mesure (en mm)

  drawRadarArc(x_centre, y_centre, rayon, 160, 380, ILI9341_GREEN); // Dessine l'arc du radar
  tft.fillCircle(x_centre, y_centre, 3, ILI9341_GREEN); // Dessine le centre du radar

  int prev_x = x_centre + rayon * cos(160 * 3.14159265 / 180.0); // Position x initiale du trait radar
  int prev_y = y_centre - rayon * sin(160 * 3.14159265 / 180.0); // Position y initiale du trait radar

  // Balayage du radar de gauche  droite
  for (int angle = 160; angle <= 380; angle++) {
    if (angle > 160) { // Efface le trait prcdent sauf au dbut
      tft.drawLine(x_centre, y_centre, prev_x, prev_y, ILI9341_BLACK); // Efface le trait prcdent
      drawRadarArc(x_centre, y_centre, rayon, 160, 380, ILI9341_GREEN); // Redessine l'arc du radar
    }
    float rad = angle * 3.14159265 / 180.0; // Conversion de l'angle en radians

    int x_arc = x_centre + rayon * cos(rad); // Calcul de la position x du bout du trait
    int y_arc = y_centre - rayon * sin(rad); // Calcul de la position y du bout du trait
    tft.drawLine(x_centre, y_centre, x_arc, y_arc, ILI9341_GREEN); // Dessine le trait du radar
    prev_x = x_arc; // Sauvegarde la position x pour le prochain effacement
    prev_y = y_arc; // Sauvegarde la position y pour le prochain effacement

    int servoAngle = map(angle, 160, 380, 0, 180); // Calcule l'angle du servo correspondant  l'angle du radar
    servoX.write(servoAngle); // Positionne le servomoteur

    uint16_t distance = sensor.readRangeContinuousMillimeters(); // Mesure la distance avec le capteur
    if (!sensor.timeoutOccurred() && distance > 50 && distance <= distance_max) { // Si la mesure est valide
      Serial.println("Distance = " + String(distance) + " mm");
      float ratio = (float)distance / distance_max; // Calcule le ratio distance/rayon
      int r_obj = (int)(ratio * rayon); // Calcule la distance  afficher sur le radar (en pixels)
      int x_obj = x_centre + r_obj * cos(rad); // Position x du point rouge
      int y_obj = y_centre - r_obj * sin(rad); // Position y du point rouge
      tft.fillCircle(x_obj, y_obj, 2, ILI9341_RED); // Dessine le point rouge (obstacle)
    }
    delay(15); // Petite pause pour la fluidit du balayage
  }
  tft.drawLine(x_centre, y_centre, prev_x, prev_y, ILI9341_BLACK); // Efface le dernier trait
  drawRadarArc(x_centre, y_centre, rayon, 160, 380, ILI9341_GREEN); // Redessine l'arc du radar

  // Balayage du radar de droite  gauche
  for (int angle = 380; angle >= 160; angle--) {
    if (angle < 380) { // Efface le trait prcdent sauf au dbut
      tft.drawLine(x_centre, y_centre, prev_x, prev_y, ILI9341_BLACK); // Efface le trait prcdent
      drawRadarArc(x_centre, y_centre, rayon, 160, 380, ILI9341_GREEN); // Redessine l'arc du radar
    }
    float rad = angle * 3.14159265 / 180.0; // Conversion de l'angle en radians
    int x_arc = x_centre + rayon * cos(rad); // Calcul de la position x du bout du trait
    int y_arc = y_centre - rayon * sin(rad); // Calcul de la position y du bout du trait
    tft.drawLine(x_centre, y_centre, x_arc, y_arc, ILI9341_GREEN); // Dessine le trait du radar
    prev_x = x_arc; // Sauvegarde la position x pour le prochain effacement
    prev_y = y_arc; // Sauvegarde la position y pour le prochain effacement

    int servoAngle = map(angle, 160, 380, 0, 180); // Calcule l'angle du servo correspondant  l'angle du radar
    servoX.write(servoAngle); // Positionne le servomoteur

    uint16_t distance = sensor.readRangeContinuousMillimeters(); // Mesure la distance avec le capteur
    if (!sensor.timeoutOccurred() && distance > 50 && distance <= distance_max) { // Si la mesure est valide
      Serial.println("Distance = " + String(distance) + " mm");
      float ratio = (float)distance / distance_max; // Calcule le ratio distance/rayon
      int r_obj = (int)(ratio * rayon); // Calcule la distance  afficher sur le radar (en pixels)
      int x_obj = x_centre + r_obj * cos(rad); // Position x du point rouge
      int y_obj = y_centre - r_obj * sin(rad); // Position y du point rouge
      tft.fillCircle(x_obj, y_obj, 2, ILI9341_RED); // Dessine le point rouge (obstacle)
    }
    delay(15); // Petite pause pour la fluidit du balayage
  }
  tft.drawLine(x_centre, y_centre, prev_x, prev_y, ILI9341_BLACK); // Efface le dernier trait
  drawRadarArc(x_centre, y_centre, rayon, 160, 380, ILI9341_GREEN); // Redessine l'arc du radar
}