Moving Laser Radar

#include <Arduino.h>
#include <Wire.h>
#include <VL53L0X.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <Servo.h>

#define TFT_CS 10
#define TFT_DC 8
#define TFT_RST 9

const int servoXPin = 4;

Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);
Servo servoX;
VL53L0X sensor;

// Draw a circular arc (radar shape)
void drawRadarArc(int x0, int y0, int radius, int startAngle, int endAngle, uint16_t color)
{
  for (int angle = startAngle; angle <= endAngle; angle++)
  {
    float rad = angle * 3.14159265 / 180.0;
    int x = x0 + radius * cos(rad);
    int y = y0 - radius * sin(rad);
    tft.drawPixel(x, y, color);
  }
}

void setup()
{
  Serial.begin(115200);
  Wire.begin();
  tft.begin();

  // Check if distance sensor is connected
  if (!sensor.init())
  {
    Serial.println("Error: Missing sensor!");
    while (1)
    {
      tft.fillScreen(ILI9341_BLACK);
      tft.setTextColor(ILI9341_WHITE);
      tft.setTextSize(2);
      tft.setRotation(2);
      tft.setCursor(30, 120);
      tft.println("Missing sensor!");
    }
  }

  tft.fillScreen(ILI9341_BLACK);
  Serial.println("Sensor ready!");
  sensor.setTimeout(500);
  sensor.startContinuous();
  servoX.attach(servoXPin);
  tft.fillScreen(ILI9341_BLACK);
  Serial.println("Initialization complete");
}

void loop()
{
  const int x_centre = 120;
  const int y_centre = 160;
  const int rayon = 100;
  const int distance_max = 1000;

  drawRadarArc(x_centre, y_centre, rayon, 160, 380, ILI9341_GREEN);
  tft.fillCircle(x_centre, y_centre, 3, ILI9341_GREEN); // Radar center point

  int prev_x = x_centre + rayon * cos(160 * 3.14159265 / 180.0);
  int prev_y = y_centre - rayon * sin(160 * 3.14159265 / 180.0);

  // Radar sweep: left to right
  for (int angle = 160; angle <= 380; angle++)
  {
    if (angle > 160)
    {
      tft.drawLine(x_centre, y_centre, prev_x, prev_y, ILI9341_BLACK); // Clear previous line
      drawRadarArc(x_centre, y_centre, rayon, 160, 380, ILI9341_GREEN);
    }

    float rad = angle * 3.14159265 / 180.0;
    int x_arc = x_centre + rayon * cos(rad);
    int y_arc = y_centre - rayon * sin(rad);
    tft.drawLine(x_centre, y_centre, x_arc, y_arc, ILI9341_GREEN);
    prev_x = x_arc;
    prev_y = y_arc;

    int servoAngle = map(angle, 160, 380, 0, 180);
    servoX.write(servoAngle); // Move servo accordingly

    uint16_t distance = sensor.readRangeContinuousMillimeters();

    // Display distance only if valid
    if (!sensor.timeoutOccurred() && distance > 50 && distance <= distance_max)
    {
      float ratio = (float)distance / distance_max;
      int r_obj = (int)(ratio * rayon);
      int x_obj = x_centre + r_obj * cos(rad);
      int y_obj = y_centre - r_obj * sin(rad);
      int oldRotation = tft.getRotation();

      tft.setTextSize(2);
      tft.setTextColor(ILI9341_WHITE);
      tft.setRotation(2);
      tft.fillRect(30, 250, 190, 15, ILI9341_BLACK); // Clear old text
      tft.setCursor(30, 250);
      tft.println("Distance = " + String(distance) + "mm");
      tft.setRotation(oldRotation);
      tft.fillCircle(x_obj, y_obj, 2, ILI9341_RED); // Draw detected object
    }
    delay(15); // Smooth sweep
  }

  tft.drawLine(x_centre, y_centre, prev_x, prev_y, ILI9341_BLACK);
  drawRadarArc(x_centre, y_centre, rayon, 160, 380, ILI9341_GREEN);

  // Radar sweep: right to left
  for (int angle = 380; angle >= 160; angle--)
  {
    if (angle < 380)
    {
      tft.drawLine(x_centre, y_centre, prev_x, prev_y, ILI9341_BLACK);
      drawRadarArc(x_centre, y_centre, rayon, 160, 380, ILI9341_GREEN);
    }

    float rad = angle * 3.14159265 / 180.0;
    int x_arc = x_centre + rayon * cos(rad);
    int y_arc = y_centre - rayon * sin(rad);
    tft.drawLine(x_centre, y_centre, x_arc, y_arc, ILI9341_GREEN);
    prev_x = x_arc;
    prev_y = y_arc;

    int servoAngle = map(angle, 160, 380, 0, 180);
    servoX.write(servoAngle);

    uint16_t distance = sensor.readRangeContinuousMillimeters();

    if (!sensor.timeoutOccurred() && distance > 50 && distance <= distance_max)
    {
      float ratio = (float)distance / distance_max;
      int r_obj = (int)(ratio * rayon);
      int x_obj = x_centre + r_obj * cos(rad);
      int y_obj = y_centre - r_obj * sin(rad);
      int oldRotation = tft.getRotation();

      tft.setTextSize(2);
      tft.setTextColor(ILI9341_WHITE);
      tft.setRotation(2);
      tft.fillRect(30, 250, 190, 15, ILI9341_BLACK);
      tft.setCursor(30, 250);
      tft.println("Distance = " + String(distance) + "mm");
      tft.setRotation(oldRotation);
      tft.fillCircle(x_obj, y_obj, 2, ILI9341_RED);
    }
    delay(15);
  }

  tft.drawLine(x_centre, y_centre, prev_x, prev_y, ILI9341_BLACK);
  drawRadarArc(x_centre, y_centre, rayon, 160, 380, ILI9341_GREEN);
}