/*
Ten program ustawia piny, mierzy odległość za pomocą czujnika ultradźwiękowego i steruje silnikami w zależności od wykrytej odległości. Jeśli odległość do przeszkody jest mniejsza niż 20 cm, robot zatrzymuje się, cofa, a następnie skręca w prawo. W przeciwnym razie porusza się do przodu.
Upewnij się, że odpowiednio podłączyłeś wszystkie komponenty zgodnie z Twoim schematem. Powodzenia!
*/
// Definicje pinów
const int trigPin = 6;
const int echoPin = 7;
const int motorA1 = 9;
const int motorA2 = 8;
const int motorB1 = 5;
const int motorB2 = 4;
const int enableA = 10;
const int enableB = 3;

// Funkcja do pomiaru odległości
long measureDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  long duration = pulseIn(echoPin, HIGH);
  long distance = duration * 0.034 / 2;
  return distance;
}

// Funkcje do sterowania silnikami
void moveForward() {
  digitalWrite(motorA1, HIGH);
  digitalWrite(motorA2, LOW);
  digitalWrite(motorB1, HIGH);
  digitalWrite(motorB2, LOW);
}

void moveBackward() {
  digitalWrite(motorA1, LOW);
  digitalWrite(motorA2, HIGH);
  digitalWrite(motorB1, LOW);
  digitalWrite(motorB2, HIGH);
}

void turnLeft() {
  digitalWrite(motorA1, LOW);
  digitalWrite(motorA2, HIGH);
  digitalWrite(motorB1, HIGH);
  digitalWrite(motorB2, LOW);
}

void turnRight() {
  digitalWrite(motorA1, HIGH);
  digitalWrite(motorA2, LOW);
  digitalWrite(motorB1, LOW);
  digitalWrite(motorB2, HIGH);
}

void stopMotors() {
  digitalWrite(motorA1, LOW);
  digitalWrite(motorA2, LOW);
  digitalWrite(motorB1, LOW);
  digitalWrite(motorB2, LOW);
}

void setup() {
  // Ustawienie pinów
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(motorA1, OUTPUT);
  pinMode(motorA2, OUTPUT);
  pinMode(motorB1, OUTPUT);
  pinMode(motorB2, OUTPUT);
  pinMode(enableA, OUTPUT);
  pinMode(enableB, OUTPUT);
  
  // Włączenie silników
  analogWrite(enableA, 255);
  analogWrite(enableB, 255);
}

void loop() {
  long distance = measureDistance();
  
  if (distance < 20) {
    stopMotors();
    delay(500);
    moveBackward();
    delay(500);
    turnRight();
    delay(500);
  } else {
    moveForward();
  }
}