Asservissement des moteurs fonctionnel
This commit is contained in:
parent
6162c6f412
commit
bde7f64555
@ -2,20 +2,8 @@
|
||||
#include "Moteurs.h"
|
||||
#include "Asser_Moteurs.h"
|
||||
|
||||
/*** C'est ici que se fait la conversion en mm
|
||||
* ***/
|
||||
|
||||
// Roues 60 mm de diamètre, 188,5 mm de circonférence
|
||||
// Réduction Moteur 30:1
|
||||
// Réduction poulie 16:12
|
||||
// Nombre d'impulsions par tour moteur : 200
|
||||
// Nombre d'impulsions par tour réducteur : 6000
|
||||
// Nombre d'impulsions par tour de roue : 8000
|
||||
// Impulsion / mm : 42,44
|
||||
|
||||
#define IMPULSION_PAR_MM (1.f)
|
||||
#define ASSERMOTEUR_GAIN_P 2.f
|
||||
#define ASSERMOTEUR_GAIN_I 0.f
|
||||
#define ASSERMOTEUR_GAIN_P 300000.f
|
||||
#define ASSERMOTEUR_GAIN_I 30000.f
|
||||
|
||||
float consigne_mm_s[3]; // Consigne de vitesse (en mm/s)
|
||||
float commande_I[3]; // Terme integral
|
||||
|
||||
@ -23,6 +23,7 @@ pico_generate_pio_header(Mon_Projet ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder
|
||||
target_include_directories(Mon_Projet PRIVATE Mon_Projet_ULD_API/inc/)
|
||||
|
||||
target_link_libraries(Mon_Projet
|
||||
hardware_adc
|
||||
hardware_i2c
|
||||
hardware_pwm
|
||||
hardware_pio
|
||||
|
||||
49
QEI.c
49
QEI.c
@ -17,9 +17,8 @@
|
||||
// Nombre d'impulsions par tour de roue : 8000
|
||||
// Impulsion / mm : 42,44
|
||||
|
||||
#define IMPULSION_PAR_MM (1.f)
|
||||
#define ASSERMOTEUR_GAIN_P 2
|
||||
#define ASSERMOTEUR_GAIN_I .0f
|
||||
#define IMPULSION_PAR_MM (12.45f)
|
||||
|
||||
|
||||
struct QEI_t QEI_A, QEI_B;
|
||||
|
||||
@ -27,38 +26,26 @@ bool QEI_est_init = false;
|
||||
|
||||
PIO pio_QEI = pio0;
|
||||
|
||||
const uint CODEUR_1_A = 26;
|
||||
const uint CODEUR_1_B = 27;
|
||||
|
||||
void QEI_init(){
|
||||
// Initialisation des 3 modules QEI
|
||||
// Chaque module QEI sera dans une machine à état du PIO 0
|
||||
if(!QEI_est_init){
|
||||
// Offset le début du programme
|
||||
// Si ce n'est pas 0, le programme ne marchera pas
|
||||
uint offset = pio_add_program(pio_QEI, &quadrature_encoder_program);
|
||||
if(offset != 0){
|
||||
printf("PIO init error: offset != 0");
|
||||
}
|
||||
// Initialisation des "machines à états" :
|
||||
// QEI1 : broche 11 et 12 - pio : pio0, sm : 0, Offset : 0, GPIO 11 et 12, clock div : 0 pour commencer
|
||||
quadrature_encoder_program_init(pio_QEI, 0, offset, 11, 0);
|
||||
// QEI2 : broche 2 et 3 - pio : pio0, sm : 1, Offset : 0, GPIO 2 et 3, clock div : 0 pour commencer
|
||||
quadrature_encoder_program_init(pio_QEI, 1, offset, 2, 0);
|
||||
|
||||
// Offset le début du programme
|
||||
// Si ce n'est pas 0, le programme ne marchera pas
|
||||
uint offset = pio_add_program(pio_QEI, &quadrature_encoder_program);
|
||||
if(offset != 0){
|
||||
printf("PIO init error: offset != 0");
|
||||
}
|
||||
// bizarrement, il faut initialiser les boches en entrée pour les GPIO 26 et 27.
|
||||
// Probablement car elle sont en analogique par défaut...
|
||||
/*gpio_init(CODEUR_1_A);
|
||||
gpio_set_dir(CODEUR_1_A, GPIO_IN);
|
||||
|
||||
gpio_init(CODEUR_1_B);
|
||||
gpio_set_dir(CODEUR_1_B, GPIO_IN);*/
|
||||
|
||||
// Initialisation des "machines à états" :
|
||||
// QEI1 : broche 31 et 32 - pio : pio0, sm : 0, Offset : 0, GPIO 2 et 3, clock div : 0 pour commencer
|
||||
// QEI1 : !!! Attention, il faudra modifier la carte élec !!! => Fait.
|
||||
quadrature_encoder_program_init(pio_QEI, 0, offset, 2, 0);
|
||||
// QEI2 : broche 26 et 27 - pio : pio0, sm : 1, Offset : 0, GPIO 11 et 12, clock div : 0 pour commencer
|
||||
quadrature_encoder_program_init(pio_QEI, 1, offset, 11, 0);
|
||||
|
||||
QEI_A.value=0;
|
||||
QEI_B.value=0;
|
||||
QEI_est_init=true;
|
||||
QEI_A.value=0;
|
||||
QEI_B.value=0;
|
||||
QEI_est_init=true;
|
||||
}
|
||||
|
||||
}
|
||||
@ -87,11 +74,11 @@ int QEI_get(enum QEI_name_t qei){
|
||||
switch (qei)
|
||||
{
|
||||
case QEI_A_NAME:
|
||||
return -QEI_A.delta;
|
||||
return QEI_A.delta;
|
||||
break;
|
||||
|
||||
case QEI_B_NAME:
|
||||
return QEI_B.delta;
|
||||
return -QEI_B.delta;
|
||||
break;
|
||||
|
||||
default:
|
||||
|
||||
80
main.c
80
main.c
@ -5,6 +5,7 @@
|
||||
*/
|
||||
#include "pico/stdlib.h"
|
||||
#include "pico/multicore.h"
|
||||
#include "hardware/adc.h"
|
||||
#include "hardware/pwm.h"
|
||||
#include "Asser_Moteurs.h"
|
||||
#include "Moteurs.h"
|
||||
@ -13,43 +14,53 @@
|
||||
#include "QEI.h"
|
||||
|
||||
#define LED1PIN 20
|
||||
#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
|
||||
|
||||
void affichage(void);
|
||||
void tension_batterie_init(void);
|
||||
uint16_t tension_batterie_lire(void);
|
||||
|
||||
void identifiant_init(void);
|
||||
uint identifiant_lire(void);
|
||||
|
||||
uint32_t step_ms=1;
|
||||
float distance1_mm=0, distance2_mm=0;
|
||||
|
||||
void main(void)
|
||||
{
|
||||
int ledpower = 500;
|
||||
|
||||
|
||||
|
||||
stdio_init_all();
|
||||
AsserMoteur_Init();
|
||||
Temps_init();
|
||||
tension_batterie_init();
|
||||
identifiant_init();
|
||||
uint32_t temps_ms = Temps_get_temps_ms();
|
||||
uint32_t step_ms=1;
|
||||
|
||||
|
||||
gpio_init(LED1PIN);
|
||||
gpio_set_dir(LED1PIN, GPIO_OUT );
|
||||
gpio_set_dir(LED1PIN, GPIO_OUT );
|
||||
gpio_put(LED1PIN, 1);
|
||||
AsserMoteur_setConsigne_mm_s(MOTEUR_A, 6000);
|
||||
AsserMoteur_setConsigne_mm_s(MOTEUR_B, 6000);
|
||||
AsserMoteur_setConsigne_mm_s(MOTEUR_A, 100.);
|
||||
AsserMoteur_setConsigne_mm_s(MOTEUR_B, 100.);
|
||||
|
||||
|
||||
multicore_launch_core1(affichage);
|
||||
Moteur_SetVitesse(MOTEUR_A, 16000);
|
||||
Moteur_SetVitesse(MOTEUR_B, 16000);
|
||||
Moteur_SetVitesse(MOTEUR_A, 000);
|
||||
Moteur_SetVitesse(MOTEUR_B, 000);
|
||||
|
||||
|
||||
while(1){
|
||||
|
||||
if(temps_ms != Temps_get_temps_ms()){
|
||||
temps_ms = Temps_get_temps_ms();
|
||||
QEI_update();
|
||||
AsserMoteur_Gestion(step_ms);
|
||||
if(temps_ms % step_ms == 0){
|
||||
QEI_update();
|
||||
AsserMoteur_Gestion(step_ms);
|
||||
}
|
||||
if(temps_ms % 100 == 0){
|
||||
identifiant_lire();
|
||||
//printf(">c1:%d\n>c2:%d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME) );
|
||||
}
|
||||
}
|
||||
@ -61,11 +72,44 @@ void main(void)
|
||||
|
||||
void affichage(void){
|
||||
while(1){
|
||||
printf(">c1_mm:%f\n>c2_mm:%f\n", QEI_get_mm(QEI_A_NAME), QEI_get_mm(QEI_B_NAME) );
|
||||
|
||||
printf(">c1:%d\n>c1_mm:%f\n", QEI_get(QEI_A_NAME), QEI_get_mm(QEI_A_NAME) );
|
||||
//printf(">c2:%d\n>c2_mm:%f\n", QEI_get(QEI_B_NAME), QEI_get_mm(QEI_B_NAME) );
|
||||
printf(">m1:%f\n>m2:%f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, 1), AsserMoteur_getVitesse_mm_s(MOTEUR_B, 1) );
|
||||
printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) );
|
||||
/*printf(">m1:%f\n>m2:%f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms) );
|
||||
printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) );*/
|
||||
sleep_ms(100);
|
||||
}
|
||||
}
|
||||
|
||||
void tension_batterie_init(void){
|
||||
adc_init();
|
||||
adc_gpio_init(28); // Analog_2
|
||||
adc_select_input(2);
|
||||
adc_run(1); // Free running mode
|
||||
}
|
||||
|
||||
uint16_t tension_batterie_lire(){
|
||||
uint16_t result = (uint16_t) adc_hw->result;
|
||||
const float conversion_factor = 3.3f / (1 << 12);
|
||||
float v_bat = result * conversion_factor * 11.;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
void identifiant_init(){
|
||||
gpio_init(21);
|
||||
gpio_init(22);
|
||||
gpio_init(26);
|
||||
gpio_pull_up(21);
|
||||
gpio_pull_up(22);
|
||||
gpio_pull_up(26);
|
||||
gpio_set_dir(21, GPIO_IN);
|
||||
gpio_set_dir(22, GPIO_IN);
|
||||
gpio_set_dir(26, GPIO_IN);
|
||||
}
|
||||
|
||||
|
||||
/// @brief !! Arg la GPIO 26 ne répond pas !
|
||||
/// @return
|
||||
uint identifiant_lire(){
|
||||
return (gpio_get(21) << 2)+ (gpio_get(22) << 1) + gpio_get(26);
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user