#include <stdio.h>
#include <stdint.h>
#include "esp_system.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"
#include "driver/uart.h"
#include "sdkconfig.h"


#define TRIGGER_PIN 5
#define ECHO_PIN 1
#define BUZZER_PIN 4

void setup() ultrasonic_task ()
(void *pvParameters) {
  ultrasonic_sensor_t sensor = {
    .trigger_pin = TRIGGER_GPIO,
    .echo_pin = ECHO_GPIO
  };

  ultrasonic_init(&sensor);

  while (1) {
    uint32_t distance;
    if (ultrasonic_measure_cm(&sensor, MAX_SENSOR_DISTANCE_CM, &distance) == ESP_OK) {
      printf("Distance: %d cm\n", distance);
      // Here you can implement logic for navigation based on distance
      if (distance < 50) {
        gpio_set_level(BUZZER_GPIO, 1); // turn on buzzer
      } else {
        gpio_set_level(BUZZER_GPIO, 0); // turn off buzzer
      }
    } else {
      printf("Error in ultrasonic measurement\n");
    }
    vTaskDelay(1000 / portTICK_PERIOD_MS);
  }
}

void mpu6050_task(void *pvParameters) {
  // Initialize MPU6050 sensor here

  while (1) {
    // Read data from MPU6050 sensor
    // Implement your logic for navigation based on MPU6050 data
    vTaskDelay(1000 / portTICK_PERIOD_MS);
  }
}

void app_main(void) {
  // Configure GPIOs
  gpio_pad_select_gpio(BUZZER_GPIO);
  gpio_set_direction(BUZZER_GPIO, GPIO_MODE_OUTPUT);

  // Create tasks for ultrasonic sensor and MPU6050 sensor
  xTaskCreate(ultrasonic_task, "ultrasonic_task", 2048, NULL, 5, NULL);
  xTaskCreate(mpu6050_task, "mpu6050_task", 2048, NULL, 5, NULL);
}


This code assumes you have already set up the necessary libraries for the HC - SR04 ultrasonic sensor and the MPU6050 accelerometer and gyroscope sensor on the ESP32. You'll also need to connect the HC-SR04 trigger pin to GPIO 32, echo pin to GPIO 33, and the buzzer to GPIO 26. Adjust the pin numbers according to your actual connections.

This code creates two tasks : one for reading the distance from the HC - SR04 sensor and controlling the buzzer accordingly, and another for reading data from the MPU6050 sensor. You'll need to implement the logic for navigation based on the sensor readings inside these tasks.

  }

void loop() {

  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH);
  long distance = duration * 0.034 / 2;

  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  if (distance < 50) {
    tone(BUZZER_PIN, 1000);
    delay(1000);
  } else {
    noTone(BUZZER_PIN);
    delay(1000);
  }
  delay(100);
}