Ajout des test avec perturbations crées par les moteurs des propulseurs

Test.c

@@ -48,7 +48,8 @@ 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 test_asser_position_avance_et_tourne(int, int);
+int test_transition_gyro_pas_gyro(void);
 int test_trajectoire(void);
 int test_i2c_bus(void);
 void affiche_localisation(void);
@@ -83,6 +84,8 @@ int mode_test(){
     printf("M - pour les moteurs\n");
     printf("N - Fonctions geometrique\n");
     printf("O - Analyse obstacle\n");
+    printf("P - Asser Position - perturbation\n");
+    printf("Q - Asser Position - transition Gyro -> Pas gyro\n");
     printf("T - Trajectoire\n");
     printf("U - Scan du bus i2c\n");
     printf("V - APDS_9960\n");
@@ -136,12 +139,12 @@ int mode_test(){
 
     case 'I':
     case 'i':
-        while(test_asser_position_avance_et_tourne(1));
+        while(test_asser_position_avance_et_tourne(1, 0));
         break;
     
     case 'J':
     case 'j':
-        while(test_asser_position_avance_et_tourne(0));
+        while(test_asser_position_avance_et_tourne(0, 0));
         break;
         
     case 'K':
@@ -169,6 +172,16 @@ int mode_test(){
         while(test_angle_balise());
         break;
 
+    case 'P':
+    case 'p':
+        while(test_asser_position_avance_et_tourne(1, 1));
+        break;
+
+    case 'Q':
+    case 'q':
+        while(test_transition_gyro_pas_gyro());
+        break;
+
     case 'T':
     case 't':
         while(test_trajectoire());
@@ -915,12 +928,73 @@ int test_trajectoire(){
     
 }
 
+
 /// @brief Avance droit 100 mm/s en tournant sur lui-même (1rad/s)
 /// @param m_gyro : 1 pour utiliser le gyroscope, 0 sans
+/// @param propulseur : 1 pour activer le propulseur toutes les secondes
 /// @return 
-int test_asser_position_avance_et_tourne(int m_gyro){
+int test_transition_gyro_pas_gyro(void){
-    int lettre, _step_ms = 1, _step_ms_gyro = 2, step_gyro=2;
+    int lettre, _step_ms = 1, _step_ms_gyro = 2, step_gyro=2, propulseur_on=0;
-    uint32_t temps_ms = 0, temps_ms_init = 0, temps_ms_old;
+    uint32_t temps_ms = 0, temps_ms_init = 0, temps_ms_old, tempo_prop=0;
+    struct position_t position_consigne;
+
+    position_consigne.angle_radian = 0;
+    position_consigne.x_mm = 0;
+    position_consigne.y_mm = 0;
+    
+    printf("Le robot tourne sur lui-même, transition sans gyro @ t=5s\n");
+
+    printf("Init gyroscope\n");
+    Gyro_Init();
+    printf("C'est parti !\n");
+    stdio_flush();
+
+    set_position_avec_gyroscope(1);
+    temps_ms = Temps_get_temps_ms();
+    temps_ms_old = temps_ms;
+    temps_ms_init = temps_ms;
+
+    multicore_launch_core1(affiche_localisation);
+    do{
+        
+        while(temps_ms == Temps_get_temps_ms());
+        
+        temps_ms = Temps_get_temps_ms();
+        temps_ms_old = temps_ms;
+
+        QEI_update();
+        if(get_position_avec_gyroscope()){
+            if(temps_ms % _step_ms_gyro == 0){
+                Gyro_Read(_step_ms_gyro);
+            }
+        }
+
+        if(temps_ms - temps_ms_init > 5000){
+            set_position_avec_gyroscope(0);
+        }
+        Localisation_gestion();
+        AsserMoteur_Gestion(_step_ms);
+        
+        position_consigne.angle_radian = (float) (temps_ms - temps_ms_init) /1000.;
+
+        Asser_Position(position_consigne);
+
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
+
+    printf("lettre : %c, %d\n", lettre, lettre);
+
+    return 0;
+}
+
+
+/// @brief Avance droit 100 mm/s en tournant sur lui-même (1rad/s)
+/// @param m_gyro : 1 pour utiliser le gyroscope, 0 sans
+/// @param propulseur : 1 pour activer le propulseur toutes les secondes
+/// @return 
+int test_asser_position_avance_et_tourne(int m_gyro, int propulseur){
+    int lettre, _step_ms = 1, _step_ms_gyro = 2, step_gyro=2, propulseur_on=0;
+    uint32_t temps_ms = 0, temps_ms_init = 0, temps_ms_old, tempo_prop=0;
     struct position_t position_consigne;
 
     position_consigne.angle_radian = 0;
@@ -933,6 +1007,9 @@ int test_asser_position_avance_et_tourne(int m_gyro){
         Gyro_Init();
         printf("C'est parti !\n");
     }
+    if(propulseur){
+        i2c_maitre_init();
+    }
     stdio_flush();
 
     set_position_avec_gyroscope(m_gyro);
@@ -942,7 +1019,26 @@ int test_asser_position_avance_et_tourne(int m_gyro){
 
     multicore_launch_core1(affiche_localisation);
     do{
-        while(temps_ms == Temps_get_temps_ms());
+        
+        while(temps_ms == Temps_get_temps_ms()){
+            if(propulseur){
+                i2c_gestion(i2c0);
+                i2c_annexe_gestion();
+            }
+        }
+        if(propulseur){
+            tempo_prop++;
+            if(tempo_prop>1000){
+                tempo_prop=0;
+                if(propulseur_on){
+                    i2c_annexe_active_propulseur();
+                }else{
+                    i2c_annexe_desactive_propulseur();
+                }
+                propulseur_on = !propulseur_on;
+            }
+        }
+
         temps_ms = Temps_get_temps_ms();
         temps_ms_old = temps_ms;
 
@@ -954,7 +1050,9 @@ int test_asser_position_avance_et_tourne(int m_gyro){
         AsserMoteur_Gestion(_step_ms);
         
         position_consigne.angle_radian = (float) (temps_ms - temps_ms_init) /1000.;
+        if(!propulseur){
             position_consigne.y_mm = (float) (temps_ms - temps_ms_init) * 100. / 1000.;
+        }
 
         Asser_Position(position_consigne);
 
@@ -1147,8 +1245,8 @@ void affiche_localisation(){
     struct position_t position;
     while(1){
         position = Localisation_get();
-        printf("X: %f, Y: %f, angle: %f\n", position.x_mm, position.y_mm, position.angle_radian *180. / 3.141592654);
+        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());
     }
 }
 
@@ -1222,13 +1320,17 @@ int test_QIE_mm(){
 
 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;    
+    uint32_t _step_ms_gyro = 2, _step_ms=1, tempo_moteur=0;
     uint32_t m_gyro = 0;
+    int16_t vitesse;
+    int propulseur=0;
 
     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();
@@ -1240,6 +1342,10 @@ int test_localisation(){
             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);
@@ -1255,10 +1361,12 @@ int test_localisation(){
 
     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){
@@ -1268,6 +1376,23 @@ int test_localisation(){
         }
         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);