Lidar structure

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <Servo.h>
#include <VL53L0X.h>

Adafruit_BNO055 bno;
VL53L0X distSensor;
bool active = false;

#define SERVO_LAVY   12
#define SERVO_PRAVY  13
#define SERVO_HLAVA  11  // 180° servo pre lidar skenovanie

Servo servoLavy;
Servo servoPravy;
Servo servoHlava;

// Výsledky skenovania
int scanAngles[]    = {0, 30, 60, 90, 120, 150, 180};
int scanDistances[] = {0,  0,  0,  0,   0,   0,   0};
int scanCount = 7;

void setup() {
  Serial.begin(115200);
  Wire.setSDA(4);
  Wire.setSCL(5);
  Wire.begin();
  delay(1000);

  // Inicializácia BNO055
  int adresa = 0;
  for (byte addr = 1; addr < 127; addr++) {
    Wire.beginTransmission(addr);
    if (Wire.endTransmission() == 0) adresa = addr;
  }
  if (adresa > 0) {
    bno = Adafruit_BNO055(55, adresa, &Wire);
    if (bno.begin()) active = true;
  }

  // Inicializácia VL53L0X
  if (!distSensor.init()) {
    Serial.println("VL53L0X nenájdený!");
    while (1);
  }
  distSensor.setTimeout(500);
  distSensor.startContinuous();

  servoLavy.attach(SERVO_LAVY, 500, 2500);
  servoPravy.attach(SERVO_PRAVY, 500, 2500);
  servoHlava.attach(SERVO_HLAVA, 500, 2500);
  stoj();
  servoHlava.write(90);
}

float getAngleZ() {
  imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
  return euler.z();
}

bool isCalibrated() {
  uint8_t system, gyro, accel, mag;
  bno.getCalibration(&system, &gyro, &accel, &mag);
  return system >= 1;
}

void stoj()   { servoLavy.write(90); servoPravy.write(90); }
void vpred()  { /* --- skopírujte z predchádzajúcej úlohy --- */ }
void vlavo()  { /* --- skopírujte z predchádzajúcej úlohy --- */ }
void vpravo() { /* --- skopírujte z predchádzajúcej úlohy --- */ }
void otoc(float cielUhol) { /* --- skopírujte z predchádzajúcej úlohy --- */ }

// Preskúma terén – otočí hlavu z 0° na 180° a zmeria vzdialenosť v každom uhle
void scan() {
  Serial.println("Skenujem...");
  for (int i = 0; i < scanCount; i++) {
    servoHlava.write(scanAngles[i]);
    delay(400);  // počkáme kým sa servo ustáli
    scanDistances[i] = distSensor.readRangeContinuousMillimeters();
    Serial.print("Uhol: "); Serial.print(scanAngles[i]);
    Serial.print("°  Vzdialenosť: "); Serial.print(scanDistances[i]);
    Serial.println(" mm");
  }
  servoHlava.write(90);  // vrátime hlavu rovno
}

// Vráti uhol kde je najbližší objekt
int nearestAngle() {
  // --- doplňte: prejdite pole scanDistances[] a nájdite index s najmenšou hodnotou ---
  // --- vráťte scanAngles[index] ---
}

void loop() {
  if (!active || !isCalibrated()) {
    Serial.println("Kalibrujem...");
    delay(100);
    return;
  }

  // 1. Preskúmaj terén
  scan();

  // 2. Zisti smer k najbližšiemu objektu
  int cielUhol = nearestAngle();
  Serial.print("Najbližší objekt na uhle: ");
  Serial.println(cielUhol);

  // 3. Otočte sa k nemu
  // Uhol 90° = rovno, 0° = vľavo, 180° = vpravo
  if (cielUhol < 80) {
    // --- doplňte: otočte sa doľava o príslušný uhol ---
  } else if (cielUhol > 100) {
    // --- doplňte: otočte sa doprava o príslušný uhol ---
  }

  // 4. Choďte k nemu (použite logiku poslušného psíčka)
  // --- doplňte ---

  delay(50);
}