Test unitaires : ajout du gyroscope et de la localisation

Localisation.c

@@ -8,6 +8,8 @@
 struct position_t position;
 
 void Localisation_init(){
+    Temps_init();
+    QEI_init();
     position.x_mm = 0;
     position.y_mm = 0;
     position.angle_radian = 0;

Temps.c

@@ -3,6 +3,7 @@
 #include "Temps.h"
 
 uint32_t temps_ms=0;
+bool temps_est_init=false;
 struct repeating_timer timer;
 
 bool Temps_increment(struct repeating_timer *t){
@@ -11,8 +12,11 @@ bool Temps_increment(struct repeating_timer *t){
 }
 
 void Temps_init(void){
-    temps_ms=0;
+    if(!temps_est_init){
-    add_repeating_timer_ms(-1, Temps_increment, NULL, &timer);
+        temps_ms=0;
+        add_repeating_timer_ms(-1, Temps_increment, NULL, &timer);
+        temps_est_init = true;
+    }
 }
 
 uint32_t Temps_get_temps_ms(void){

Test.c

@@ -76,7 +76,6 @@ int mode_test(){
     printf("P - Asser Position - perturbation\n");
     printf("Q - Asser Position - transition Gyro -> Pas gyro\n");
     printf("R - Test des logs...\n");
-    printf("S - Test du gyroscope...\n");
     printf("T - Trajectoire\n");
     printf("U - Tests i2c...\n");
     printf("V - APDS_9960\n");
@@ -156,11 +155,6 @@ int mode_test(){
     case 'r':
         while(test_log());
         break;
-    
-    case 'S':
-    case 's':
-        while(test_gyro());
-        break;
 
     case 'T':
     case 't':
@@ -261,16 +255,6 @@ int test_APDS9960(){
     return 1;
 }
 
-
-void test_trajectoire_printf(){
-    struct position_t _position;
-    while(1){
-        _position  = Trajet_get_consigne();
-        printf("T: %ld, X: %f, Y: %f, orientation: %2.1f\n", time_us_32()/1000, _position.x_mm, _position.y_mm, _position.angle_radian/M_PI*180);
-    }
-
-}
-
 void test_trajectoire_teleplot(){
     struct position_t _position, _consigne;
     _consigne = Trajet_get_consigne();
@@ -791,107 +775,8 @@ int test_asser_position_avance(){
 }
 
 
-void affiche_localisation(){
-    struct position_t position;
-    while(1){
-        position = Localisation_get();
-        printf(">X:%f\n>Y:%f\n>angle:%f\n", position.x_mm, position.y_mm, position.angle_radian *180. / 3.141592654);
-        printf(">v_bat:%2.2f\n", i2c_annexe_get_tension_batterie());
-    }
-}
 
 
-int test_localisation(){
-    int lettre;
-    int moteurs = 0;
-    struct position_t position;
-    uint32_t temps_ms;
-    uint32_t _step_ms_gyro = 2, _step_ms=1, tempo_moteur=0;
-    uint32_t m_gyro = 0;
-    int16_t vitesse;
-    int propulseur=0;
-
-    Temps_init();
-    QEI_init();
-    Localisation_init();
-
-    printf("A - Sans gyroscope\n");
-    printf("B - Avec Gyroscope\n");
-    printf("C - Avec Gyroscope + moteurs\n");
-    do{
-         lettre = getchar_timeout_us(TEST_TIMEOUT_US);
-         stdio_flush();
-    }while(lettre == PICO_ERROR_TIMEOUT);
-
-    switch(lettre){
-        case 'A':
-        case 'a':
-            set_position_avec_gyroscope(0);
-            printf("Sans gyroscope\n");
-            break;
-        case 'c':
-        case 'C':
-            moteurs = 1;
-            i2c_maitre_init();
-        case 'B':
-        case 'b':
-            set_position_avec_gyroscope(1);
-            printf("Avec gyroscope, initialisation...\n");
-            m_gyro=1;
-            Gyro_Init();
-            break;
-        default:
-            return 0;
-    }
-
-    temps_ms = Temps_get_temps_ms();
-
-    multicore_launch_core1(affiche_localisation);
-
-    vitesse = (int16_t) 32766.0;
-    
-    printf("Affichage de la position du robot.\nAppuyez sur une touche pour quitter\n");
-    do{
-        i2c_gestion(i2c0);
-        i2c_annexe_gestion();
-        while(temps_ms == Temps_get_temps_ms());
-        QEI_update();
-        if(m_gyro){
-            if(temps_ms % _step_ms_gyro == 0){
-                Gyro_Read(_step_ms_gyro);
-            }
-        }
-        Localisation_gestion();
-        position = Localisation_get();
-        if(moteurs){
-            tempo_moteur++;
-            if(tempo_moteur > 1000){
-                tempo_moteur = 0;
-                vitesse = -vitesse;
-                Moteur_SetVitesse(MOTEUR_A ,vitesse);
-                Moteur_SetVitesse(MOTEUR_B ,vitesse);
-                Moteur_SetVitesse(MOTEUR_C ,vitesse);
-                if(propulseur){
-                    i2c_annexe_active_propulseur();
-                }else{
-                    i2c_annexe_desactive_propulseur();
-                }
-                propulseur = !propulseur;
-            }
-        }
-        
-
-        lettre = getchar_timeout_us(0);
-
-        temps_ms += _step_ms;
-    }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
-
-    multicore_reset_core1();
-
-    return 0;
-    
-}
-
 int test_geometrie(){
     float angle = 270, angle_min, angle_max;
     printf("Normalise %f° : %f°\n", angle, Geometrie_get_angle_normalisee(angle*DEGRE_EN_RADIAN)/DEGRE_EN_RADIAN);

Test_gyro.c

@@ -50,6 +50,7 @@ int test_gyro_vitesse_brute(void){
     printf("Lecture vitesse brute\n");
     uint16_t tampon_envoi[4];
     uint8_t tampon_reception[4];
+    char lettre;
     char message[LOG_MAX_MESSAGE_SIZE];
     Log_init();
     Gyro_init_spi();
@@ -62,7 +63,12 @@ int test_gyro_vitesse_brute(void){
         gyro_get_vitesse_brute(&angle_gyro, NULL);
         if(time_us_32() % 100000){
             sprintf(message,">angle:%d\n", angle_gyro.rot_z);
-            Log_message(message, TELEPLOT);
+            //Log_message(message, TELEPLOT);
+            printf("%s", message);
+            lettre = getchar_timeout_us(0);
+            if(lettre != PICO_ERROR_TIMEOUT){
+                return 0;
+            }
         }
     }
 

Tests_unitaires.c

@@ -4,6 +4,15 @@
 #include "QEI.h"
 #include "Moteurs.h"
 #include "Asser_Moteurs.h"
+#include "Localisation.h"
+#include "i2c_maitre.h"
+#include "i2c_annexe.h"
+#include "Robot_config.h"
+#include "gyro.h"
+#include "Temps.h"
+
+#include "Test_gyro.h"
+
 
 #define TEST_TIMEOUT_US 10000000
 
@@ -12,6 +21,8 @@ int test_QIE_mm(void);
 int test_asser_moteur(void);
 int test_vitesse_moteur(enum t_moteur moteur);
 int test_moteurs(void);
+void affiche_localisation();
+int test_localisation();
 
 
 // Mode test : renvoie 0 pour quitter le mode test
@@ -22,6 +33,8 @@ int mode_test_unitaire(){
     printf("B - pour les codeurs (somme en mm)\n");
     printf("C - pour les moteurs\n");
     printf("D - pour asser_moteurs (rotation)\n");
+    printf("E - pour le gyroscope\n");
+    printf("F - pour la localisation\n");
     
     
     stdio_flush();
@@ -48,6 +61,17 @@ int mode_test_unitaire(){
         while(test_asser_moteur());
         break;
 
+    case 'E':
+    case 'e':
+        while(test_gyro());
+        break;
+        
+    
+    case 'F':
+    case 'f':
+        while(test_localisation());
+        break;
+
     case PICO_ERROR_TIMEOUT:
         iteration--;        
         if(iteration == 0){
@@ -226,3 +250,102 @@ int test_asser_moteur(){
     return 0;
 }
 
+void affiche_localisation(){
+    struct position_t position;
+    while(1){
+        position = Localisation_get();
+        printf(">X:%f\n>Y:%f\n>angle:%f\n", position.x_mm, position.y_mm, position.angle_radian *180. / 3.141592654);
+        printf(">v_bat:%2.2f\n", i2c_annexe_get_tension_batterie());
+    }
+}
+
+
+int test_localisation(){
+    int lettre;
+    int moteurs = 0;
+    struct position_t position;
+    uint32_t temps_ms;
+    uint32_t _step_ms_gyro = 2, _step_ms=1, tempo_moteur=0;
+    uint32_t m_gyro = 0;
+    int16_t vitesse;
+    int propulseur=0;
+
+    
+    Localisation_init();
+
+    printf("A - Sans gyroscope\n");
+    printf("B - Avec Gyroscope\n");
+    printf("C - Avec Gyroscope + moteurs\n");
+    do{
+         lettre = getchar_timeout_us(TEST_TIMEOUT_US);
+         stdio_flush();
+    }while(lettre == PICO_ERROR_TIMEOUT);
+
+    switch(lettre){
+        case 'A':
+        case 'a':
+            set_position_avec_gyroscope(0);
+            printf("Sans gyroscope\n");
+            break;
+        case 'c':
+        case 'C':
+            moteurs = 1;
+            i2c_maitre_init();
+        case 'B':
+        case 'b':
+            set_position_avec_gyroscope(1);
+            printf("Avec gyroscope, initialisation...\n");
+            m_gyro=1;
+            Gyro_Init();
+            break;
+        default:
+            return 0;
+    }
+
+    temps_ms = Temps_get_temps_ms();
+
+    multicore_launch_core1(affiche_localisation);
+
+    vitesse = (int16_t) 32766.0;
+    
+    printf("Affichage de la position du robot.\nAppuyez sur une touche pour quitter\n");
+    do{
+        i2c_gestion(i2c0);
+        i2c_annexe_gestion();
+        while(temps_ms == Temps_get_temps_ms());
+        QEI_update();
+        if(m_gyro){
+            if(temps_ms % _step_ms_gyro == 0){
+                Gyro_Read(_step_ms_gyro);
+            }
+        }
+        Localisation_gestion();
+        position = Localisation_get();
+        if(moteurs){
+            tempo_moteur++;
+            if(tempo_moteur > 1000){
+                tempo_moteur = 0;
+                vitesse = -vitesse;
+                Moteur_SetVitesse(MOTEUR_A ,vitesse);
+                Moteur_SetVitesse(MOTEUR_B ,vitesse);
+                Moteur_SetVitesse(MOTEUR_C ,vitesse);
+                if(propulseur){
+                    i2c_annexe_active_propulseur();
+                }else{
+                    i2c_annexe_desactive_propulseur();
+                }
+                propulseur = !propulseur;
+            }
+        }
+        
+
+        lettre = getchar_timeout_us(0);
+
+        temps_ms += _step_ms;
+    }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
+
+    multicore_reset_core1();
+
+    return 0;
+    
+}