98 lines
2.4 KiB
C
98 lines
2.4 KiB
C
|
#include "hardware/pwm.h"
|
||
|
|
#include "Moteurs.h"
|
||
|
|
|
||
|
|
#define MOTEUR_A 0
|
||
|
|
#define MOTEUR_B 1
|
||
|
|
#define MOTEUR_C 2
|
||
|
|
|
||
|
|
#define MOTEUR_A_VITESSE 6
|
||
|
|
#define MOTEUR_B_VITESSE 8
|
||
|
|
#define MOTEUR_C_VITESSE 10
|
||
|
|
|
||
|
|
#define MOTEUR_A_SENS 5
|
||
|
|
#define MOTEUR_B_SENS 7
|
||
|
|
#define MOTEUR_C_SENS 9
|
||
|
|
|
||
|
|
#define M1_SENS1 7
|
||
|
|
#define M1_SENS2 13
|
||
|
|
#define M1_VITESSE 27 //5B
|
||
|
|
#define M2_SENS1 10
|
||
|
|
#define M2_SENS2 5
|
||
|
|
#define M2_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(M1_SENS1);
|
||
|
|
gpio_init(M1_SENS2);
|
||
|
|
gpio_init(M2_SENS1);
|
||
|
|
gpio_init(M2_SENS2);
|
||
|
|
gpio_set_dir(M1_SENS1, GPIO_OUT);
|
||
|
|
gpio_set_dir(M1_SENS2, GPIO_OUT);
|
||
|
|
gpio_set_dir(M2_SENS1, GPIO_OUT);
|
||
|
|
gpio_set_dir(M2_SENS2, GPIO_OUT);
|
||
|
|
gpio_put(M1_SENS1, 0);
|
||
|
|
gpio_put(M1_SENS2, 0);
|
||
|
|
gpio_put(M2_SENS1, 0);
|
||
|
|
gpio_put(M2_SENS2, 0);
|
||
|
|
|
||
|
|
gpio_set_function(M1_VITESSE, GPIO_FUNC_PWM);
|
||
|
|
gpio_set_function(M2_VITESSE, GPIO_FUNC_PWM);
|
||
|
|
pwm_set_wrap(5, (uint16_t)65535);
|
||
|
|
pwm_set_wrap(4, (uint16_t)65535);
|
||
|
|
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_SetVitesse(MOTEUR_A, 0);
|
||
|
|
Moteur_SetVitesse(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_SetVitesse(enum t_moteur moteur, int16_t vitesse ){
|
||
|
|
uint16_t u_vitesse;
|
||
|
|
|
||
|
|
// Le PWM accepte 16 bits de résolution, on se remet sur 16 bits (et non sur 15 + signe)
|
||
|
|
if (vitesse < 0){
|
||
|
|
u_vitesse = -vitesse;
|
||
|
|
}else{
|
||
|
|
u_vitesse = vitesse;
|
||
|
|
}
|
||
|
|
u_vitesse = u_vitesse * 2;
|
||
|
|
|
||
|
|
switch(moteur){
|
||
|
|
case MOTEUR_A:
|
||
|
|
pwm_set_chan_level(5, PWM_CHAN_B, u_vitesse);
|
||
|
|
if(vitesse > 0){
|
||
|
|
gpio_put(M1_SENS1, 0);
|
||
|
|
gpio_put(M1_SENS2, 1);
|
||
|
|
}else{
|
||
|
|
gpio_put(M1_SENS1, 1);
|
||
|
|
gpio_put(M1_SENS2, 0);
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
|
||
|
|
case MOTEUR_B:
|
||
|
|
pwm_set_chan_level(4, PWM_CHAN_B, u_vitesse);
|
||
|
|
if(vitesse < 0){
|
||
|
|
gpio_put(M2_SENS1, 0);
|
||
|
|
gpio_put(M2_SENS2, 1);
|
||
|
|
}else{
|
||
|
|
gpio_put(M2_SENS1, 1);
|
||
|
|
gpio_put(M2_SENS2, 0);
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void Moteur_Stop(void){
|
||
|
|
Moteur_SetVitesse(MOTEUR_A, 0);
|
||
|
|
Moteur_SetVitesse(MOTEUR_B, 0);
|
||
|
|
}
|