Test de déplacement en boucle ouverte déplacés

CMakeLists.txt

@@ -40,6 +40,7 @@ Test_gyro.c
 Test_i2c.c
 Test_log.c
 Test_strategie.c
+Tests_deplacement.c
 Tests_unitaires.c
 Trajet.c
 Trajectoire.c

Readme.md

@@ -1,6 +1,6 @@
-Code de déplacement du robot holonome 2023
+Code de déplacement du robot holonome ~~2023~~ 2024
 ==========================================
-Ce dépôt contient de code le déplacement du robot holonome de l'équipe [Poivron Robotique](http://poivron-robotique.fr). Ce robot a pour ambition de participer à la coupe de France de Robotique en 2023.
+Ce dépôt contient de code le déplacement du robot holonome de l'équipe [Poivron Robotique](http://poivron-robotique.fr). Ce robot a pour ambition de participer à la coupe de France de Robotique en 2024.
 
 Le but est de présenter un code assurant toutes les fonctions de déplacement du robot, allant de la commande PWM des moteurs jusqu'au suivi d'une trajectoire (droite, portion de cercle, courbe de Bézier) en passant par le contrôle de l'accélération et de la décélération.
 

Test.c

@@ -35,6 +35,7 @@
 #include "Test_log.h"
 #include "Test_strategie.h"
 #include "Tests_unitaires.h"
+#include "Tests_deplacement.h"
 #include "Test.h"
 
 #define V_INIT -999.0
@@ -42,12 +43,6 @@
 
 int test_APDS9960(void);
 int test_localisation(void);
-int test_avance(void);
-int test_cde_vitesse(void);
-int test_cde_vitesse_rotation(void);
-int test_cde_rotation_ref_robot(float centre_x_mm, float centre_y_mm);
-int test_cde_vitesse_rectangle(void);
-int test_cde_vitesse_cercle(void);
 int test_asser_position_avance(void);
 int test_asser_position_avance_et_tourne(int, int);
 int test_transition_gyro_pas_gyro(void);
@@ -67,9 +62,8 @@ int continuous_printf = 1;
 int mode_test(){
     static int iteration = 2;
     printf("Appuyez sur une touche pour entrer en mode test :\n");
-    printf("A - Tests unitaires\n");
-    printf("B - pour avance (asser_moteur)\n");
-    printf("E - Commande en vitesse...\n");
+    printf("A - Tests unitaires...\n");
+    printf("B - Tests deplacement...\n");
     printf("F - Strategie...\n");
     printf("G - Lecture des capteurs\n");
     printf("H - Asser Position - avance\n");
@@ -97,14 +91,10 @@ int mode_test(){
     case 'A':
         while(mode_test_unitaire());
         break;
+    
     case 'b':
     case 'B':
-        while(test_avance());
-        break;
-
-    case 'E':
-    case 'e':
-        while(test_cde_vitesse());
+        while(mode_test_deplacement());
         break;
 
     case 'F':
@@ -800,150 +790,6 @@ int test_asser_position_avance(){
     return 0;
 }
 
-int test_cde_vitesse(){
-    printf("A - Commande en vitesse - rectangle\n");
-    printf("B - Commande en vitesse - cercle\n");
-    printf("C - Commande en vitesse - rotation pure\n");
-    printf("D - Commande en vitesse - rotation par rapport à un point (contacteur longer A)\n");
-    printf("E - Commande en vitesse - rotation par rapport à un point (contacteur longer C)\n");
-
-    int lettre;
-    do{ 
-        lettre = getchar_timeout_us(0);
-        stdio_flush();
-
-    }while(lettre == PICO_ERROR_TIMEOUT || lettre == '\0');
-
-    switch(lettre){
-        case 'a':
-        case 'A':
-            while(test_cde_vitesse_rectangle());
-            break;
-        
-        case 'b':
-        case 'B':
-            while(test_cde_vitesse_cercle());
-            break;
-        
-        case 'c':
-        case 'C':
-            while(test_cde_vitesse_rotation());
-            break;
-        
-        case 'd':
-        case 'D':
-            while(test_cde_rotation_ref_robot(RAYON_ROBOT, 0));
-            break;
-
-        case 'e':
-        case 'E':
-            while(test_cde_rotation_ref_robot(RAYON_ROBOT/2, -RAYON_ROBOT* RACINE_DE_3/2));
-            break;
-    }
-}
-
-int test_cde_vitesse_rotation(){
-    int lettre, _step_ms = 1;
-    float vitesse =90.0/2 * 3.14159 /180.0;
-    printf("Rotation du robot sur lui-même en 8 secondes\nVitesse : %f rad/s\n", vitesse);
-
-    commande_vitesse(0, 0, vitesse);
-    do{
-        QEI_update();
-        AsserMoteur_Gestion(_step_ms);
-        sleep_ms(_step_ms);
-        lettre = getchar_timeout_us(0);
-    }while(lettre == PICO_ERROR_TIMEOUT);
-    Moteur_Stop();
-
-    return 0;
-}
-
-
-int test_cde_rotation_ref_robot(float centre_x_mm, float centre_y_mm){
-    int lettre, _step_ms = 1;
-    float vitesse =90.0/4 * 3.14159 /180.0;
-    printf("Rotation du robot par rapport au point (Rayon, O)\nVitesse : %f rad/s\n", vitesse);
-
-    commande_rotation(vitesse, centre_x_mm, centre_y_mm);
-    do{
-        QEI_update();
-        AsserMoteur_Gestion(_step_ms);
-        sleep_ms(_step_ms);
-        lettre = getchar_timeout_us(0);
-    }while(lettre == PICO_ERROR_TIMEOUT);
-    Moteur_Stop();
-
-    return 0;
-}
-
-int test_cde_vitesse_rectangle(){
-    int lettre, _step_ms = 1, temps_ms=0;
-
-    printf("déplacement en rectangle du robot : 500x200 mm, 100 mm/s\n");
-    do{
-        QEI_update();
-        AsserMoteur_Gestion(_step_ms);
-
-        if(temps_ms < 5000){
-            commande_vitesse(0, 100, 0);
-        }else if(temps_ms < 7000){
-            commande_vitesse(-100, 0, 0);
-        }else if(temps_ms < 12000){
-            commande_vitesse(0, -100, 0);
-        }else if(temps_ms < 14000){
-            commande_vitesse(100, 0, 0);
-        }else{
-            temps_ms = 0;
-        }
-        
-        sleep_ms(_step_ms);
-        temps_ms += _step_ms;
-        lettre = getchar_timeout_us(0);
-    }while(lettre == PICO_ERROR_TIMEOUT);
-    Moteur_Stop();
-
-    return 0;
-}
-
-int test_cde_vitesse_cercle(){
-    int lettre, _step_ms = 1, temps_ms=0;
-
-    printf("déplacement en cercle du robot : 100 mm/s\n");
-    do{
-        QEI_update();
-        AsserMoteur_Gestion(_step_ms);
-        commande_vitesse(cos((float)temps_ms / 1000.) * 200.0, sin((float)temps_ms /1000.) * 200.0, 0);
-        temps_ms += _step_ms;
-        sleep_ms(_step_ms);
-        
-
-        lettre = getchar_timeout_us(0);
-    }while(lettre == PICO_ERROR_TIMEOUT);
-    Moteur_Stop();
-
-    return 0;
-}
-
-int test_avance(void){
-    int lettre;
-    int _step_ms = 1;
-    AsserMoteur_setConsigne_mm_s(MOTEUR_A, -100);
-    AsserMoteur_setConsigne_mm_s(MOTEUR_B, 100);
-    AsserMoteur_setConsigne_mm_s(MOTEUR_C, 0);
-    
-    do{
-        QEI_update();
-        AsserMoteur_Gestion(_step_ms);
-        sleep_ms(_step_ms);
-        lettre = getchar_timeout_us(0);
-
-    }while(lettre == PICO_ERROR_TIMEOUT);
-    Moteur_SetVitesse(MOTEUR_A, 0);
-    Moteur_SetVitesse(MOTEUR_B, 0);
-    Moteur_SetVitesse(MOTEUR_C, 0);
-    return 0;
-}
 
 void affiche_localisation(){
     struct position_t position;

Tests_deplacement.c

@@ -0,0 +1,207 @@
+#include <stdio.h>
+#include <math.h>
+#include "pico/multicore.h"
+
+#include "QEI.h"
+#include "Moteurs.h"
+#include "Asser_Moteurs.h"
+#include "Geometrie_robot.h"
+#include "Commande_vitesse.h"
+
+int test_avance(void);
+int test_cde_vitesse(void);
+int test_cde_vitesse_rotation(void);
+int test_cde_rotation_ref_robot(float centre_x_mm, float centre_y_mm);
+int test_cde_vitesse_rectangle(void);
+int test_cde_vitesse_cercle(void);
+
+#define TEST_TIMEOUT_US 10000000
+
+// Mode test : renvoie 0 pour quitter le mode test
+int mode_test_deplacement(){
+    static int iteration = 2;
+    printf("Appuyez sur une touche pour entrer en mode test :\n");
+    printf("A - AsserMoteur - pour avancer selon Y\n");
+    printf("B - AsserMoteur - Commande en vitesse...\n");
+    
+    
+    stdio_flush();
+    int rep = getchar_timeout_us(TEST_TIMEOUT_US);
+    stdio_flush();
+    switch (rep)
+    {
+    case 'a':
+    case 'A':
+        while(test_avance());
+        break;
+    case 'b':
+    case 'B':
+        while(test_cde_vitesse());
+        break;
+
+    case PICO_ERROR_TIMEOUT:
+        iteration--;        
+        if(iteration == 0){
+            //printf("Sortie du mode test\n");
+            //return 0;
+        }
+
+    default:
+        printf("Commande inconnue\n");
+        break;
+    }
+    return 1;
+    
+}
+
+/// @brief Avance doucement (<10cm/s) selon l'axe X
+/// @param  
+/// @return 
+int test_avance(void){
+    int lettre;
+    int _step_ms = 1;
+    AsserMoteur_Init();
+    AsserMoteur_setConsigne_mm_s(MOTEUR_A, -100);
+    AsserMoteur_setConsigne_mm_s(MOTEUR_B, 100);
+    AsserMoteur_setConsigne_mm_s(MOTEUR_C, 0);
+    
+    do{
+        QEI_update();
+        AsserMoteur_Gestion(_step_ms);
+        sleep_ms(_step_ms);
+        lettre = getchar_timeout_us(0);
+
+    }while(lettre == PICO_ERROR_TIMEOUT);
+    Moteur_SetVitesse(MOTEUR_A, 0);
+    Moteur_SetVitesse(MOTEUR_B, 0);
+    Moteur_SetVitesse(MOTEUR_C, 0);
+    return 0;
+}
+
+int test_cde_vitesse(){
+    printf("A - Commande en vitesse - rectangle\n");
+    printf("B - Commande en vitesse - cercle\n");
+    printf("C - Commande en vitesse - rotation pure\n");
+    printf("D - Commande en vitesse - rotation par rapport à un point (contacteur longer A)\n");
+    printf("E - Commande en vitesse - rotation par rapport à un point (contacteur longer C)\n");
+
+    int lettre;
+    do{ 
+        lettre = getchar_timeout_us(0);
+        stdio_flush();
+
+    }while(lettre == PICO_ERROR_TIMEOUT || lettre == '\0');
+
+    switch(lettre){
+        case 'a':
+        case 'A':
+            while(test_cde_vitesse_rectangle());
+            break;
+        
+        case 'b':
+        case 'B':
+            while(test_cde_vitesse_cercle());
+            break;
+        
+        case 'c':
+        case 'C':
+            while(test_cde_vitesse_rotation());
+            break;
+        
+        case 'd':
+        case 'D':
+            while(test_cde_rotation_ref_robot(RAYON_ROBOT, 0));
+            break;
+
+        case 'e':
+        case 'E':
+            while(test_cde_rotation_ref_robot(RAYON_ROBOT/2, -RAYON_ROBOT* RACINE_DE_3/2));
+            break;
+    }
+}
+
+int test_cde_vitesse_rotation(){
+    int lettre, _step_ms = 1;
+    float vitesse =90.0/2 * 3.14159 /180.0;
+    AsserMoteur_Init();
+    printf("Rotation du robot sur lui-même en 8 secondes\nVitesse : %f rad/s\n", vitesse);
+
+    commande_vitesse(0, 0, vitesse);
+    do{
+        QEI_update();
+        AsserMoteur_Gestion(_step_ms);
+        sleep_ms(_step_ms);
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT);
+    Moteur_Stop();
+
+    return 0;
+}
+
+
+int test_cde_rotation_ref_robot(float centre_x_mm, float centre_y_mm){
+    int lettre, _step_ms = 1;
+    float vitesse =90.0/4 * 3.14159 /180.0;
+    AsserMoteur_Init();
+    printf("Rotation du robot par rapport au point (Rayon, O)\nVitesse : %f rad/s\n", vitesse);
+
+    commande_rotation(vitesse, centre_x_mm, centre_y_mm);
+    do{
+        QEI_update();
+        AsserMoteur_Gestion(_step_ms);
+        sleep_ms(_step_ms);
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT);
+    Moteur_Stop();
+
+    return 0;
+}
+
+int test_cde_vitesse_rectangle(){
+    int lettre, _step_ms = 1, temps_ms=0;
+    AsserMoteur_Init();
+
+    printf("déplacement en rectangle du robot : 500x200 mm, 100 mm/s\n");
+    do{
+        QEI_update();
+        AsserMoteur_Gestion(_step_ms);
+
+        if(temps_ms < 5000){
+            commande_vitesse(0, 100, 0);
+        }else if(temps_ms < 7000){
+            commande_vitesse(-100, 0, 0);
+        }else if(temps_ms < 12000){
+            commande_vitesse(0, -100, 0);
+        }else if(temps_ms < 14000){
+            commande_vitesse(100, 0, 0);
+        }else{
+            temps_ms = 0;
+        }
+        
+        sleep_ms(_step_ms);
+        temps_ms += _step_ms;
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT);
+    Moteur_Stop();
+
+    return 0;
+}
+
+int test_cde_vitesse_cercle(){
+    int lettre, _step_ms = 1, temps_ms=0;
+    AsserMoteur_Init();
+    printf("déplacement en cercle du robot : 100 mm/s\n");
+    do{
+        QEI_update();
+        AsserMoteur_Gestion(_step_ms);
+        commande_vitesse(cos((float)temps_ms / 1000.) * 200.0, sin((float)temps_ms /1000.) * 200.0, 0);
+        temps_ms += _step_ms;
+        sleep_ms(_step_ms);
+        
+
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT);
+    Moteur_Stop();
+
+    return 0;
+}

Tests_deplacement.h

@@ -0,0 +1 @@
+int mode_test_deplacement();