Roomba structure

#include <Servo.h>

const int TRIG        = 6;
const int ECHO        = 7;
#define SERVO_LAVY    12
#define SERVO_PRAVY   13
#define SERVO_HLAVA   11

Servo servoLavy;
Servo servoPravy;
Servo servoHlava;

void setup() {
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);
  servoLavy.attach(SERVO_LAVY, 500, 2500);
  servoPravy.attach(SERVO_PRAVY, 500, 2500);
  servoHlava.attach(SERVO_HLAVA, 500, 2500);
  stoj();
  servoHlava.write(90); // hlava rovno
  Serial.begin(9600);
}

long measurePulse() {
  // --- skopírujte z predchádzajúcej úlohy ---
}

float getDistance() {
  // --- doplňte: zavolajte measurePulse() a prepočítajte na cm ---
}

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

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

  if (dist > 20) {
    // --- voľno: ide rovno ---
  } else {
    stoj();

    // Pozri sa doprava
    servoHlava.write(0);
    delay(500);
    float distRight = getDistance();

    // Pozri sa doľava
    servoHlava.write(180);
    delay(500);
    float distLeft = getDistance();

    // Vráť hlavu rovno
    servoHlava.write(90);
    delay(300);

    if (distRight > distLeft) {
      // --- doplňte: otočte sa doprava ---
    } else if (distLeft > 20) {
      // --- doplňte: otočte sa doľava ---
    } else {
      // --- doplňte: všade prekážka – cúvnite ---
    }
  }

  delay(50);
}