91 lines
2.6 KiB
C
91 lines
2.6 KiB
C
|
#include "hardware/pwm.h"
|
||
|
|
#include "Moteurs.h"
|
||
|
|
|
||
|
|
#define MOTEUR_A_SENS1 7
|
||
|
|
#define MOTEUR_A_SENS2 13
|
||
|
|
#define MOTEUR_A_VITESSE 27 //5B
|
||
|
|
#define MOTEUR_B_SENS1 10
|
||
|
|
#define MOTEUR_B_SENS2 5
|
||
|
|
#define MOTEUR_B_VITESSE 9 //4B
|
||
|
|
|
||
|
|
uint slice_moteur_A, slice_moteur_B, slice_moteur_C;
|
||
|
|
|
||
|
|
/// @brief Initialisation les entrées / sorties requises pour les moteurs
|
||
|
|
void Moteur_init(){
|
||
|
|
gpio_init(MOTEUR_A_SENS1);
|
||
|
|
gpio_init(MOTEUR_A_SENS2);
|
||
|
|
gpio_init(MOTEUR_B_SENS1);
|
||
|
|
gpio_init(MOTEUR_B_SENS2);
|
||
|
|
gpio_set_dir(MOTEUR_A_SENS1, GPIO_OUT);
|
||
|
|
gpio_set_dir(MOTEUR_A_SENS2, GPIO_OUT);
|
||
|
|
gpio_set_dir(MOTEUR_B_SENS1, GPIO_OUT);
|
||
|
|
gpio_set_dir(MOTEUR_B_SENS2, GPIO_OUT);
|
||
|
|
gpio_put(MOTEUR_A_SENS1, 0);
|
||
|
|
gpio_put(MOTEUR_A_SENS2, 0);
|
||
|
|
gpio_put(MOTEUR_B_SENS1, 0);
|
||
|
|
gpio_put(MOTEUR_B_SENS2, 0);
|
||
|
|
|
||
|
|
gpio_set_function(MOTEUR_A_VITESSE, GPIO_FUNC_PWM);
|
||
|
|
gpio_set_function(MOTEUR_B_VITESSE, GPIO_FUNC_PWM);
|
||
|
|
pwm_set_wrap(5, (uint16_t)MOTEUR_COMMANDE_MAX);
|
||
|
|
pwm_set_wrap(4, (uint16_t)MOTEUR_COMMANDE_MAX);
|
||
|
|
pwm_set_chan_level(5, PWM_CHAN_B, 0);
|
||
|
|
pwm_set_chan_level(4, PWM_CHAN_B, 0);
|
||
|
|
pwm_set_enabled(5, true);
|
||
|
|
pwm_set_enabled(4, true);
|
||
|
|
|
||
|
|
Moteur_set_commande(MOTEUR_A, 0);
|
||
|
|
Moteur_set_commande(MOTEUR_B, 0);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/// @brief Configure le PWM et la broche de sens en fonction de la vitesse et du moteur
|
||
|
|
/// @param moteur : Moteur (voir enum t_moteur)
|
||
|
|
/// @param vitesse : Vitesse entre -32767 et 32767
|
||
|
|
void Moteur_set_commande(enum t_moteur moteur, int32_t vitesse ){
|
||
|
|
uint16_t u_vitesse;
|
||
|
|
|
||
|
|
// Le PWM accepte 16 bits de résolution, on se remet sur 16 bits (et non sur 32 bits)
|
||
|
|
if (vitesse < -MOTEUR_COMMANDE_MAX){
|
||
|
|
vitesse = -MOTEUR_COMMANDE_MAX;
|
||
|
|
}else if(vitesse > MOTEUR_COMMANDE_MAX){
|
||
|
|
vitesse = MOTEUR_COMMANDE_MAX;
|
||
|
|
}
|
||
|
|
// On veut une vitesse > 0 pour la consigne PWM
|
||
|
|
if(vitesse > 0){
|
||
|
|
u_vitesse = vitesse;
|
||
|
|
}else{
|
||
|
|
u_vitesse = -vitesse;
|
||
|
|
}
|
||
|
|
|
||
|
|
switch(moteur){
|
||
|
|
case MOTEUR_A:
|
||
|
|
pwm_set_chan_level(5, PWM_CHAN_B, u_vitesse);
|
||
|
|
if(vitesse > 0){
|
||
|
|
gpio_put(MOTEUR_A_SENS1, 0);
|
||
|
|
gpio_put(MOTEUR_A_SENS2, 1);
|
||
|
|
}else{
|
||
|
|
gpio_put(MOTEUR_A_SENS1, 1);
|
||
|
|
gpio_put(MOTEUR_A_SENS2, 0);
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
|
||
|
|
case MOTEUR_B:
|
||
|
|
pwm_set_chan_level(4, PWM_CHAN_B, u_vitesse);
|
||
|
|
if(vitesse < 0){
|
||
|
|
gpio_put(MOTEUR_B_SENS1, 0);
|
||
|
|
gpio_put(MOTEUR_B_SENS2, 1);
|
||
|
|
}else{
|
||
|
|
gpio_put(MOTEUR_B_SENS1, 1);
|
||
|
|
gpio_put(MOTEUR_B_SENS2, 0);
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void Moteur_stop(void){
|
||
|
|
Moteur_set_commande(MOTEUR_A, 0);
|
||
|
|
Moteur_set_commande(MOTEUR_B, 0);
|
||
|
|
}
|