Gyroscope structure

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

Adafruit_BNO055 bno;
bool active = false;

#define SERVO_LAVY  12
#define SERVO_PRAVY 13

Servo servoLavy;
Servo servoPravy;

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

  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;
      Serial.println("BNO055 OK – čakám na kalibráciu...");
    }
  }

  servoLavy.attach(SERVO_LAVY, 500, 2500);
  servoPravy.attach(SERVO_PRAVY, 500, 2500);
  stoj();
}

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()  { /* --- doplňte --- */ }
void vlavo()  { /* --- doplňte --- */ }
void vpravo() { /* --- doplňte --- */ }

// Otočí autíčko o požadovaný uhol (kladný = doprava, záporný = doľava)
void otoc(float cielUhol) {
  float startUhol = getAngleZ();
  float otocene = 0;

  while (abs(otocene) < abs(cielUhol)) {
    float aktualny = getAngleZ();
    otocene = aktualny - startUhol;

    // --- doplňte: otáčajte doprava alebo doľava podľa znamienka cielUhol ---

    delay(10);
  }
  stoj();
}

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

  float uhol = getAngleZ();

  // Korekcia smeru – ak sa autíčko odchýli od 0°, skoriguj
  if (uhol > 2) {
    // --- doplňte: odchýlka doprava – skoriguj doľava ---
  } else if (uhol < -2) {
    // --- doplňte: odchýlka doľava – skoriguj doprava ---
  } else {
    vpred();
  }

  // Príkazy cez sériový monitor: 'R' = otoc doprava, 'L' = otoc doľava
  if (Serial.available()) {
    char c = Serial.read();
    if (c == 'R') otoc(90);
    if (c == 'L') otoc(-90);
  }

  delay(50);
}