Car with lasers

Car with lasers

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

VL53L0X sensor;

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

  if (!sensor.init()) {
    Serial.println("Senzor nenájdený!");
    while (1);
  }
  sensor.setTimeout(500);
  sensor.startContinuous();

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

int getDistance() {
  int dist = sensor.readRangeContinuousMillimeters();
  if (sensor.timeoutOccurred()) return -1;  // chyba merania
  return dist;
}

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

void loop() {
  int dist = getDistance();

  Serial.print("Vzdialenosť: ");
  Serial.print(dist);
  Serial.println(" mm");

  if (dist == -1) {
    // --- doplňte: chyba merania – čo urobíme? ---
  } else if (dist > 300) {
    // --- doplňte: voľno ---
  } else if (dist > 100) {
    // --- doplňte: blíži sa prekážka ---
  } else {
    // --- doplňte: prekážka blízko ---
  }

  delay(50);
}