Line sensor structure

Line sensor structure – štruktúra kódu

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

// Line senzor
const uint8_t SENSOR_COUNT = 4;
const uint8_t sensorPins[SENSOR_COUNT] = {26, 27, 28, 29};
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);

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

// Načíta hodnoty zo senzora – vráti true ak kanál vidí čiaru
bool onLine(uint8_t channel) {
  return analogRead(sensorPins[channel]) > THRESHOLD;
}

// Vráti pozíciu čiary – záporné = čiara vľavo, kladné = čiara vpravo, 0 = stred
int linePosition() {
  // Senzory: 0=ľavý krajný, 1=ľavý stred, 2=pravý stred, 3=pravý krajný
  // --- doplňte: vypočítajte pozíciu čiary podľa toho ktoré kanály vidia čiaru ---
  // Nápoveda: ak vidí iba kanál 0 = čiara je veľmi vľavo (-2)
  //           ak vidí iba kanál 3 = čiara je veľmi vpravo (+2)
  //           ak vidí kanály 1 a 2 = čiara je v strede (0)
}

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) {
    // --- doplňte: prekážka blízko – zastavte a počkajte ---
    return;
  }

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

  // 3. Sleduj čiaru
  if (pos == 0) {
    // --- doplňte: čiara v strede – ide rovno ---
  } else if (pos < 0) {
    // --- doplňte: čiara vľavo – zatočte doľava ---
  } else {
    // --- doplňte: čiara vpravo – zatočte doprava ---
  }

  // 4. Čo ak nevidíme čiaru vôbec?
  bool anyLine = false;
  for (uint8_t i = 0; i < SENSOR_COUNT; i++) {
    if (onLine(i)) anyLine = true;
  }
  if (!anyLine) {
    // --- doplňte: čiara zmizla – čo urobíme? ---
  }

  delay(20);
}