#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;
}


main.c

diagram.json

#ifndef SERVO_H
#define SERVO_H

#include "hardware/pwm.h"
#include "hardware/gpio.h"

#define SERVO_PIN 2
#define SERVO_MIN_DUTY_CYCLE 500
#define SERVO_MAX_DUTY_CYCLE 2500

void servo_init();
void servo_set_angle(float angle);

#endif /* SERVO_H */


servo.h

#include "hardware/pwm.h"
#include "hardware/gpio.h"

#define SERVO_PIN 2
#define SERVO_MIN_DUTY_CYCLE 500
#define SERVO_MAX_DUTY_CYCLE 2500
#define SERVO_RANGE 1000 // Define SERVO_RANGE here

void servo_init() {
    gpio_set_function(SERVO_PIN, GPIO_FUNC_PWM);
    uint slice_num = pwm_gpio_to_slice_num(SERVO_PIN);
    pwm_set_wrap(slice_num, SERVO_RANGE);
    pwm_set_clkdiv(slice_num, 125.0); // Set PWM frequency
}

void servo_set_angle(float angle) {
    uint16_t duty_cycle = SERVO_MIN_DUTY_CYCLE + (SERVO_MAX_DUTY_CYCLE - SERVO_MIN_DUTY_CYCLE) * angle / 180.0;
    pwm_set_chan_level(pwm_gpio_to_slice_num(SERVO_PIN), pwm_gpio_to_channel(SERVO_PIN), duty_cycle);
}


servo.c

#ifndef ULTRASONIC_DISTANCE_H
#define ULTRASONIC_DISTANCE_H

#include "hardware/gpio.h"
#include "hardware/timer.h"

#define TRIGGER_PIN 15
#define ECHO_PIN 14

float measure_distance();

#endif /* ULTRASONIC_DISTANCE_H */


ultrasonic_distance.h

#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/gpio.h"
#include "pico/time.h" // Added to declare sleep_us()

#define TRIGGER_PIN 15
#define ECHO_PIN 14

void setup_ultrasonic() {
    gpio_init(TRIGGER_PIN);
    gpio_set_dir(TRIGGER_PIN, GPIO_OUT);

    gpio_init(ECHO_PIN);
    gpio_set_dir(ECHO_PIN, GPIO_IN);
}

float measure_distance() {
    gpio_put(TRIGGER_PIN, 1);
    sleep_us(10);
    gpio_put(TRIGGER_PIN, 0);

    uint32_t start_time = time_us_32();
    while (!gpio_get(ECHO_PIN)) {
        if ((time_us_32() - start_time) > 50000) {
            return -1; // Timeout
        }
    }
    start_time = time_us_32();
    while (gpio_get(ECHO_PIN)) {
        if ((time_us_32() - start_time) > 50000) {
            return -1; // Timeout
        }
    }
    uint32_t end_time = time_us_32();
    return ((end_time - start_time) * 0.0343) / 2; // Distance in cm
}
