#include <Arduino.h>

// Define the pins for the first ultrasonic sensor
const int trigPin1 = 2; // GPIO pin for triggering the first ultrasonic sensor
const int echoPin1 = 4; // GPIO pin for receiving the echo from the first sensor

// Define the pins for the second ultrasonic sensor
const int trigPin2 = 5; // GPIO pin for triggering the second ultrasonic sensor
const int echoPin2 = 13; // GPIO pin for receiving the echo from the second sensor

void setup() {
  Serial.begin(115200);

  // Set up the first ultrasonic sensor
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);

  // Set up the second ultrasonic sensor
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
}

void loop() {
  // Measure distance for the first sensor
  float distance1 = measureDistance(trigPin1, echoPin1);

  // Measure distance for the second sensor
  float distance2 = measureDistance(trigPin2, echoPin2);

  // Print the distances to the Serial Monitor
  Serial.print("Distance 1: ");
  Serial.print(distance1);
  Serial.println(" cm");

  Serial.print("Distance 2: ");
  Serial.print(distance2);
  Serial.println(" cm");

  // Add a delay before the next measurement
  delay(1000);
}

float measureDistance(int trigPin, int echoPin) {
  // Trigger ultrasonic sensor
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Read the duration of the pulse on the echo pin
  long duration = pulseIn(echoPin, HIGH);

  // Calculate distance in centimeters
  return duration * 0.034 / 2;
}