74 lines
2.5 KiB
C
74 lines
2.5 KiB
C
#include "pico/stdlib.h"
|
|
#include "hardware/pwm.h"
|
|
#include "Servomoteur.h"
|
|
|
|
|
|
|
|
void Servomoteur_Init(void){
|
|
uint slice_num;
|
|
gpio_set_function(SERVO5, GPIO_FUNC_PWM);
|
|
gpio_set_function(SERVO6, GPIO_FUNC_PWM);
|
|
gpio_set_function(SERVO7, GPIO_FUNC_PWM);
|
|
gpio_set_function(SERVO_TURBINE, 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(SERVO5);
|
|
pwm_init(slice_num, &pwm_servo, true);
|
|
|
|
slice_num = pwm_gpio_to_slice_num(SERVO_TURBINE);
|
|
pwm_init(slice_num, &pwm_servo, true);
|
|
|
|
slice_num = pwm_gpio_to_slice_num(SERVO6);
|
|
pwm_init(slice_num, &pwm_servo, true);
|
|
|
|
slice_num = pwm_gpio_to_slice_num(SERVO7);
|
|
pwm_init(slice_num, &pwm_servo, true);
|
|
|
|
Servomoteur_set(SERVO5, SERVO_VALEUR_1_5MS);
|
|
Servomoteur_set(SERVO6, SERVO_VALEUR_1_5MS);
|
|
Servomoteur_set(SERVO7, SERVO_VALEUR_1_5MS);
|
|
Servomoteur_set(SERVO_TURBINE, SERVO_VALEUR_1MS);
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
void Moteur_Init(){
|
|
uint slice_num;
|
|
gpio_set_function(SERVO8_MOT_PROPULSEUR, 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 25 : 5230 kHz
|
|
pwm_config_set_clkdiv_int(&pwm_servo, 25);
|
|
// Valeur max du PXM pour avoir 5 kHz : 5230 kHz / 5 kHz : 1064
|
|
pwm_config_set_wrap(&pwm_servo, 1064);
|
|
// À 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(SERVO8_MOT_PROPULSEUR);
|
|
pwm_init(slice_num, &pwm_servo, true);
|
|
|
|
Moteur_Set(0);
|
|
|
|
}
|
|
|
|
/// @brief Envoie la consigne PWM au moteur.
|
|
/// @param consigne : consigne entre 0 et 1.
|
|
void Moteur_Set(double consigne){
|
|
Servomoteur_set(SERVO8_MOT_PROPULSEUR, (uint16_t) (1064 * consigne));
|
|
} |