#include <Servo.h>

#define TRIG 9      
#define ECHO 10     
#define SERVO_L 5   
#define SERVO_R 6   

int targetDistance = 30;  
int tolerance = 3;        

Servo leftServo;
Servo rightServo;

void setup() {
  Serial.begin(9600);
  
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);
  
  leftServo.attach(SERVO_L);
  rightServo.attach(SERVO_R);
  
  stop();
  delay(1000);
}

void loop() {
  int distance = measureDistance();

  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");
  
  if (distance < (targetDistance - tolerance)) {
    backward();
    Serial.println("BACKWARD");
  }
  else if (distance > (targetDistance + tolerance)) {
    forward();
    Serial.println("FORWARD");
  }
  else {  
    stop();
    Serial.println("STOP");
  }
  delay(100);  
}

int measureDistance() {
  digitalWrite(TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  
  long duration = pulseIn(ECHO, HIGH);
  int distance = duration * 0.034 / 2;
  
  return distance;
}


void forward() {
  leftServo.write(70);  
  rightServo.write(110); 
}

void backward() {
  leftServo.write(110);  
  rightServo.write(70); 
}

void stop() {
  leftServo.write(90);   
  rightServo.write(90); 
}

void turnRight() {
  leftServo.write(70);
  rightServo.write(70);
}

void turnLeft() {
  leftServo.write(110);
  rightServo.write(110);
}