Code de location bien avancé, il y a un soucis sur le gain des QEI !

Asser_Moteurs.c

@@ -1,5 +1,6 @@
 #include "QEI.h"
 #include "Moteurs.h"
+#include "Asser_Moteurs.h"
 
 /*** C'est ici que se fait la conversion en mm 
  * ***/
@@ -43,7 +44,7 @@ double AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms){
     
     default: break;
     }
-    distance = (double) QEI_get(qei) / (double)IMPULSION_PAR_MM;
+    distance = QEI_get_mm(qei);
     temps = step_ms / 1000.0;
 
     return distance / temps;

CMakeLists.txt

@@ -9,17 +9,17 @@ pico_sdk_init()
 
 add_executable(test
 test.c
+APDS_9960.c
 Asser_Moteurs.c
-spi_nb.c
+QEI.c
 gyro.c
-Temps.c
-Servomoteur.c
 gyro_L3GD20H.c
 gyro_ADXRS453.c
-QEI.c
+Localisation.c
-APDS_9960.c
 Moteurs.c
-)
+Temps.c
+Servomoteur.c
+spi_nb.c)
 
 pico_generate_pio_header(test ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio)
 

Localisation.c

@@ -0,0 +1,36 @@
+#include "Localisation.h"
+#include "QEI.h"
+#include "math.h"
+
+#define DISTANCE_ROUES_CENTRE_MM 84.25
+#define RACINE_DE_3 1.73205081
+
+struct position_t position;
+
+void Localisation_init(){
+    position.x_mm = 0;
+    position.y_mm = 0;
+    position.angle_radian = 0;
+}
+
+void Localisation_gestion(){
+    // Voir http://poivron-robotique.fr/Robot-holonome-localisation-partie-2.html
+    double distance_roue_a_mm = QEI_get_mm(QEI_A_NAME);
+    double distance_roue_b_mm = QEI_get_mm(QEI_B_NAME);
+    double  distance_roue_c_mm = QEI_get_mm(QEI_C_NAME);
+    double delta_x_ref_robot, delta_y_ref_robot;
+
+    delta_x_ref_robot = (distance_roue_a_mm + distance_roue_b_mm - 2 * distance_roue_c_mm)  / 3.0;
+    delta_y_ref_robot = (-distance_roue_a_mm + distance_roue_b_mm)  * RACINE_DE_3 / 3.0;
+
+    position.angle_radian += - ( distance_roue_a_mm + distance_roue_b_mm + distance_roue_c_mm) / (3 * DISTANCE_ROUES_CENTRE_MM);
+
+    // Projection dans le référentiel du robot
+    position.x_mm += delta_x_ref_robot * cos(position.angle_radian) - delta_y_ref_robot * sin(position.angle_radian);
+    position.y_mm += delta_x_ref_robot * sin(position.angle_radian) + delta_y_ref_robot * cos(position.angle_radian);
+
+}
+
+struct position_t Localisation_get(void){
+    return position;
+}

Localisation.h

@@ -0,0 +1,8 @@
+struct position_t{
+    double x_mm, y_mm;
+    double angle_radian;
+};
+
+struct position_t Localisation_get(void);
+void Localisation_gestion();
+void Localisation_init();

test.c

@@ -11,6 +11,7 @@
 #include "Moteurs.h"
 #include "QEI.h"
 #include "Asser_Moteurs.h"
+#include "Localisation.h"
 
 const uint LED_PIN = 25;
 const uint LED_PIN_ROUGE = 28;
@@ -23,8 +24,10 @@ const uint LED_PIN_NE_PAS_UTILISER = 22;
 int mode_test();
 int test_moteurs();
 int test_QIE();
+int test_QIE_mm();
 int test_vitesse_moteur(enum t_moteur moteur);
-int test_asser_moteur();
+int test_asser_moteur(void);
+int test_localisation(void);
 int test_avance(void);
 
 int main() {
@@ -60,6 +63,7 @@ int main() {
     Moteur_Init();
     QEI_init();
     AsserMoteur_Init();
+    Localisation_init();
 
     while(mode_test());
 
@@ -123,10 +127,12 @@ int main() {
 int mode_test(){
     static int iteration = 3;
     printf("Appuyez sur une touche pour entrer en mode test :\n");
-    printf("A - pour asser_moteurs\n");
+    printf("A - pour asser_moteurs (rotation)\n");
     printf("B - pour avance (asser_moteur)\n");
     printf("C - pour les codeurs\n");
+    printf("D - pour les codeurs (somme en mm)\n");
     printf("M - pour les moteurs\n");
+    printf("L - pour la localisation\n");
     stdio_flush();
     int rep = getchar_timeout_us(TEST_TIMEOUT_US);
     stdio_flush();
@@ -145,11 +151,22 @@ int mode_test(){
     case 'c':
         while(test_QIE());
         break;
+
+    case 'D':
+    case 'd':
+        while(test_QIE_mm());
+        break;
+        
     case 'M':
     case 'm':
         /* code */
         while(test_moteurs());
         break;
+    case 'L':
+    case 'l':
+        /* code */
+        while(test_localisation());
+        break;
     case PICO_ERROR_TIMEOUT:
         iteration--;        
         if(iteration == 0){
@@ -224,7 +241,10 @@ int test_QIE(){
     printf("Affichage des QEI :\nAppuyez sur une touche pour quitter\n");
     do{
         QEI_update();
-        printf("Codeur A : %d, codeur B : %d, codeur C : %d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME), QEI_get(QEI_C_NAME));
+        printf("Codeur A : %d (%3.2f mm), codeur B : %d (%3.2f mm), codeur C : %d (%3.2f mm)\n", 
+            QEI_get(QEI_A_NAME), QEI_get_mm(QEI_A_NAME),
+            QEI_get(QEI_B_NAME), QEI_get_mm(QEI_B_NAME),
+            QEI_get(QEI_C_NAME), QEI_get_mm(QEI_C_NAME));
         sleep_ms(100);
 
         lettre = getchar_timeout_us(0);
@@ -233,6 +253,43 @@ int test_QIE(){
 
 }
 
+int test_QIE_mm(){
+    int lettre;
+    printf("Affichage des QEI :\nAppuyez sur une touche pour quitter\n");
+    double a_mm=0, b_mm=0, c_mm=0;
+    do{
+        QEI_update();
+        a_mm += QEI_get_mm(QEI_A_NAME);
+        b_mm += QEI_get_mm(QEI_B_NAME);
+        c_mm += QEI_get_mm(QEI_C_NAME);
+        printf("Codeur A : %3.2f mm, codeur B : %3.2f mm, codeur C : %3.2f mm\n", a_mm, b_mm, c_mm);
+        sleep_ms(100);
+
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT);
+    return 0;
+
+}
+
+int test_localisation(){
+    int lettre;
+    struct position_t position;
+    
+    printf("Affichage de la position du robot.\nAppuyez sur une touche pour quitter\n");
+    do{
+        QEI_update();
+        Localisation_gestion();
+        position = Localisation_get();
+        printf("X: %f, Y: %f, angle: %f\n", position.x_mm, position.y_mm, position.angle_radian *180. / 3.141592654);
+        sleep_ms(100);
+
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT);
+
+    return 0;
+    
+}
+
 int test_moteurs(){
     int lettre_moteur;