#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/gpio.h"
#include "servo.h"
#include "ultrasonic_distance.h"
#define COLLISION_DISTANCE 80 // Adjust this according to your setup
int main() {
stdio_init_all();
gpio_init(TRIGGER_PIN);
gpio_set_dir(TRIGGER_PIN, GPIO_OUT);
gpio_init(ECHO_PIN);
gpio_set_dir(ECHO_PIN, GPIO_IN);
servo_init();
while (1) {
float distance = measure_distance();
if (distance > 0) {
if (distance > COLLISION_DISTANCE) {
printf("Distance: %f cm. Closing side mirror...\n", distance);
servo_set_angle(0); // Close the side mirror
} else {
printf("Distance: %f cm. Side mirror open.\n", distance);
servo_set_angle(90); // Open the side mirror
}
} else {
printf("Error: Unable to measure distance.\n");
}
sleep_ms(1000);
}
return 0;
}