#include <Servo.h>

Servo myservoR;
Servo myservoL;

const int trigPin = 13;
const int echoPin = 12;

void setup() {
  Serial.begin(9600);
  myservoR.attach(9);
  myservoL.attach(6);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
}

void loop() {
  // Perform distance measurement
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10); // Adjusted to meet the HC-SR04 requirements
  digitalWrite(trigPin, LOW);
  int duration = pulseIn(echoPin, HIGH);
  int distance = microsecondsToCentimeters(duration);

  // Control the robot based on the distance
  if (distance < 100) {  // Adjust this threshold based on your requirements
    // Move forward
    myservoR.write(80);
    myservoL.write(100);
  } else {
    // Stop
    myservoR.write(90);
    myservoL.write(90);
  }
}

// Function to calculate distance from duration
int microsecondsToCentimeters(int microseconds) {
  return microseconds / 29 / 2;
}