Line sensor structure

#include <Wire.h>
#include <VL53L0X.h>
#include <Servo.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;

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

Servo servoLavy;
Servo servoPravy;

void setup() {
  Serial.begin(115200);
  analogReadResolution(12);  // 12-bitové ADC (0–4095)

  // 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();

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

  Serial.println("OK");
}

// Prepne multiplexer na daný kanál (0–7)
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;
}

// Vráti bitovú masku pozície čiary (0–255)
// Každý bit zodpovedá jednému senzoru: bit0=ľavý krajný ... bit7=pravý krajný
// Príklady:
//   0b00000000 =   0 → žiadna čiara
//   0b00000001 =   1 → čiara úplne vľavo
//   0b00011000 =  24 → čiara v strede
//   0b11111111 = 255 → celá čiara (križovatka)
int linePosition() {
  int result = 0;
  for (uint8_t i = 0; i < SENSOR_COUNT; i++) {
    if (onLine(i)) result += (1 << i);
  }
  return result;
}

void stoj()   { servoLavy.write(90); servoPravy.write(90); }
void vpred()  { /* --- doplňte --- */ }
void vlavo()  { /* --- doplňte --- */ }
void vpravo() { /* --- doplňte --- */ }

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

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

  Serial.println(pos, BIN);  // vypíše binárny zápis – užitočné pri ladení

  // 3. Sleduj čiaru
  // Nápoveda: stred = 0b00011000 = 24
  //           vľavo = hodnoty kde sú nastavené iba nižšie bity (pos < 16)
  //           vpravo = hodnoty kde sú nastavené iba vyššie bity (pos >= 64)
  if (pos == 24 || pos == 36) {
    // --- doplňte: čiara v strede – ide rovno ---
  } else if (pos > 0 && pos < 16) {
    // --- doplňte: čiara vľavo – zatočte doľava ---
  } else if (pos >= 64) {
    // --- doplňte: čiara vpravo – zatočte doprava ---
  }

  // 4. Čo ak nevidíme čiaru vôbec?
  if (pos == 0) {
    // --- doplňte: čiara zmizla – čo urobíme? ---
  }

  // 5. Križovatka – všetky senzory vidia čiaru
  if (pos == 255) {
    // --- doplňte (voliteľné): križovatka ---
  }

  delay(20);
}