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



void Servomoteur_Init(void){
    uint slice_num;
    gpio_set_function(SERVO0, GPIO_FUNC_PWM);
    
    pwm_config pwm_servo = pwm_get_default_config();
    
    // On veut un rebouclage au bout de 20 ms (50 Hz).
    // Division de l'horloge système (133 MHz) par 254 : 523 kHz
    pwm_config_set_clkdiv_int(&pwm_servo, 254);
    // Valeur max du PXM pour avoir 50 Hz :  523 kHz / 50 Hz : 10460
    pwm_config_set_wrap(&pwm_servo, 10460);
    // À la valeur finale, rebouclage à 0 (et non un compte à rebours)
    pwm_config_set_phase_correct(&pwm_servo, false);

    slice_num = pwm_gpio_to_slice_num(SERVO0);
    pwm_init(slice_num, &pwm_servo, true);

    slice_num = pwm_gpio_to_slice_num(SERVO1);
    pwm_init(slice_num, &pwm_servo, true);

    Servomoteur_set(SERVO0, SERVO_VALEUR_1_5MS);

    Servomoteur_set(SERVO1, SERVO_VALEUR_1_5MS);
}

void Servomoteur_set(uint8_t servomoteur, uint16_t valeur){
    uint slice_num = pwm_gpio_to_slice_num(servomoteur);
    // Chan : A ou 0 si la GPIO est paire
    //        B ou 1 si la GPIO est impaire
    pwm_set_chan_level(slice_num, servomoteur & 0x01, valeur);
}