diff --git a/Asser_Moteurs.c b/Asser_Moteurs.c
index 300a9e8..808989d 100644
--- a/Asser_Moteurs.c
+++ b/Asser_Moteurs.c
@@ -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
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4030050..7707fc3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -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
diff --git a/QEI.c b/QEI.c
index af9d384..149d115 100644
--- a/QEI.c
+++ b/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:
diff --git a/main.c b/main.c
index 653ae26..cd50c96 100644
--- a/main.c
+++ b/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);
}
\ No newline at end of file