#include "pico/stdlib.h"
#include "hardware/pwm.h"

#define SERVO_PIN 22 // GPIO where the servo signal is connected

// Function to set the servo angle (0° to 180°)
void set_servo_angle(uint slice_num, uint angle) {
    // Map angle (0° to 180°) to PWM duty cycle (500-2500 microseconds)
    uint duty = 500 + (angle * 2000 / 180);
    pwm_set_chan_level(slice_num, PWM_CHAN_A, duty);
    printf("Setting angle: %d°, Duty cycle: %d\n", angle, duty);
}

int main() {
    stdio_init_all(); // Initialize standard I/O for debugging
    printf("Servo Motor Control Initialized\n");

    // Set up GPIO for PWM
    gpio_set_function(SERVO_PIN, GPIO_FUNC_PWM); // Set GPIO pin as PWM
    uint slice_num = pwm_gpio_to_slice_num(SERVO_PIN); // Get PWM slice

    // Set PWM frequency to 50Hz (20ms period)
    pwm_set_wrap(slice_num, 20000); // 20,000 ticks for 50Hz frequency
    pwm_set_clkdiv(slice_num, 64.0f); // Set clock divider
    pwm_set_enabled(slice_num, true); // Enable PWM on the slice

    while (true) {
        // Gradually increase angle from 0° to 180°
        for (int angle = 0; angle <= 180; angle += 5) {
            set_servo_angle(slice_num, angle);
            sleep_ms(50); // Wait for the servo to reach the position
        }

        // Gradually decrease angle from 180° to 0°
        for (int angle = 180; angle >= 0; angle -= 5) {
            set_servo_angle(slice_num, angle);
            sleep_ms(50); // Wait for the servo to reach the position
        }
    }

    return 0;
}