Line sensor bonus structure

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

// Multiplexer
const int MUX_Z  = 26;
const int MUX_S0 = 6;
const int MUX_S1 = 7;
const int MUX_S2 = 8;

// Line senzor
const uint8_t SENSOR_COUNT = 8;
const int IR_PIN    = 15;
const int THRESHOLD = 400;

// VL53L0X
VL53L0X distSensor;

// BNO055
Adafruit_BNO055 bno;
bool bnoActive = false;

// Servá
#define SERVO_LAVY  12
#define SERVO_PRAVY 13

Servo servoLavy;
Servo servoPravy;

// Logovanie
long lastLog = 0;
const int LOG_INTERVAL = 100;  // logujeme každých 100ms
File logFile;

void setup() {
  Serial.begin(115200);
  analogReadResolution(12);

  // Multiplexer
  pinMode(MUX_S0, OUTPUT);
  pinMode(MUX_S1, OUTPUT);
  pinMode(MUX_S2, OUTPUT);

  // IR LED
  pinMode(IR_PIN, OUTPUT);
  digitalWrite(IR_PIN, HIGH);

  // VL53L0X
  Wire.setSDA(4);
  Wire.setSCL(5);
  Wire.begin();
  if (!distSensor.init()) {
    Serial.println("VL53L0X nenájdený!");
    while (1);
  }
  distSensor.setTimeout(500);
  distSensor.startContinuous();

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

  // LittleFS
  if (!LittleFS.begin()) {
    Serial.println("LittleFS zlyhalo!");
    while (1);
  }
  // Otvoríme súbor na zápis – každé spustenie prepíše predchádzajúci záznam
  logFile = LittleFS.open("/data.csv", "w");
  if (logFile) {
    logFile.println("cas_ms,uhol_z,pozicia_ciary,prekazka_mm");
    Serial.println("Logovanie OK");
  } else {
    Serial.println("Chyba otvorenia súboru!");
  }

  // Servá
  servoLavy.attach(SERVO_LAVY, 500, 2500);
  servoPravy.attach(SERVO_PRAVY, 500, 2500);
  stoj();

  Serial.println("OK – čakám na kalibráciu gyroskopu...");
}

void selectChannel(uint8_t channel) {
  digitalWrite(MUX_S0, (channel >> 0) & 1);
  digitalWrite(MUX_S1, (channel >> 1) & 1);
  digitalWrite(MUX_S2, (channel >> 2) & 1);
}

bool onLine(uint8_t channel) {
  selectChannel(channel);
  delayMicroseconds(10);
  return analogRead(MUX_Z) > THRESHOLD;
}

int linePosition() {
  int result = 0;
  for (uint8_t i = 0; i < SENSOR_COUNT; i++) {
    if (onLine(i)) result += (1 << i);
  }
  return result;
}

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

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

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 logData(int pos, int dist) {
  if (!bnoActive || !isCalibrated()) return;
  if (millis() - lastLog < LOG_INTERVAL) return;
  if (!logFile) return;

  logFile.print(millis());        logFile.print(",");
  logFile.print(getAngleZ());     logFile.print(",");
  logFile.print(pos);             logFile.print(",");
  logFile.println(dist);
  logFile.flush();  // uistíme sa že dáta sú zapísané na flash

  lastLog = millis();
}

void printLog() {
  // Zatvori súbor a znova otvor na čítanie
  logFile.close();
  File r = LittleFS.open("/data.csv", "r");
  if (!r) return;
  Serial.println("=== /data.csv ===");
  while (r.available()) {
    Serial.println(r.readStringUntil('\n'));
  }
  r.close();
  Serial.println("=================");
}

void loop() {
  // Čakaj na kalibráciu gyroskopu
  if (bnoActive && !isCalibrated()) {
    Serial.println("Kalibrujem BNO055...");
    delay(100);
    return;
  }

  // Príkaz cez sériový monitor: 'P' = vypíš súbor
  if (Serial.available()) {
    char c = Serial.read();
    if (c == 'P') printLog();
  }

  // 1. Skontroluj prekážku
  int dist = distSensor.readRangeContinuousMillimeters();
  if (!distSensor.timeoutOccurred() && dist < 100) {
    stoj();
    logData(0, dist);
    // --- doplňte: objíďte alebo počkajte ---
    return;
  }

  // 2. Zisti pozíciu čiary
  int pos = linePosition();

  // 3. Sleduj čiaru
  if (pos == 24 || pos == 36) {
    // --- doplňte: čiara v strede ---
  } else if (pos > 0 && pos < 16) {
    // --- doplňte: čiara vľavo ---
  } else if (pos >= 64) {
    // --- doplňte: čiara vpravo ---
  }

  if (pos == 0) {
    // --- doplňte: čiara zmizla ---
  }

  if (pos == 255) {
    // --- doplňte (voliteľné): križovatka ---
  }

  // 4. Zaloguj dáta
  logData(pos, dist);

  delay(20);
}