Initialisation du test de l'homologation + fonction pour définir la position

Localisation.c

@@ -13,6 +13,24 @@ void Localisation_init(){
     position.angle_radian = 0;
 }
 
+void Localisation_set(double x_mm, double y_mm, double angle_radian){
+    position.x_mm = x_mm;
+    position.y_mm = y_mm;
+    position.angle_radian = angle_radian;
+}
+
+void Localisation_set_x(double x_mm){
+    position.x_mm = x_mm;
+}
+
+void Localisation_set_y(double y_mm){
+    position.y_mm = y_mm;
+}
+
+void Localisation_set_angle(double angle_radian){
+    position.angle_radian = angle_radian;
+}
+
 void Localisation_gestion(){
     struct t_angle_gyro_double angle_gyro;
     // Voir http://poivron-robotique.fr/Robot-holonome-localisation-partie-2.html

Localisation.h

@@ -3,3 +3,8 @@
 struct position_t Localisation_get(void);
 void Localisation_gestion();
 void Localisation_init();
+
+void Localisation_set(double x_mm, double y_mm, double angle_radian);
+void Localisation_set_x(double x_mm);
+void Localisation_set_y(double y_mm);
+void Localisation_set_angle(double angle_radian);

Strategie.c

@@ -0,0 +1,12 @@
+
+
+void Homologation(void){
+    static enum etat_strategie_t{
+        STRATEGIE_INIT
+    }etat_strategie;
+
+    switch(etat_strategie){
+        case STRATEGIE_INIT:
+            
+    }
+}

Strategie.h

@@ -9,4 +9,5 @@ enum longer_direction_t{
 };
 
 enum etat_action_t cerise_accostage(void);
-enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction);
+enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction);
+void Homologation(void);

Test_strategie.c

@@ -15,8 +15,9 @@
 #include "Trajectoire.h"
 #include "Test.h"
 
-int test_accostage();
-int test_longe();
+int test_accostage(void);
+int test_longe(void);
+int test_homologation(void);
 
 void affichage_test_strategie(){
     uint32_t temps;
@@ -34,20 +35,28 @@ void affichage_test_strategie(){
 int test_strategie(){
     printf("L - longer.\n");
     printf("A - Accoster.\n");
+    printf("H - Homologation.\n");
     int lettre;
     do{
         lettre = getchar_timeout_us(0);
     }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
     switch(lettre){
+        case 'a':
+        case 'A':
+            while(test_accostage());
+            break;
+
+        case 'h':
+        case 'H':
+            while(test_homologation());
+            break;
+
         case 'l':
         case 'L':
             while(test_longe());
             break;
 
-        case 'a':
-        case 'A':
-            while(test_accostage());
-            break;
+        
 
         case 'q':
         case 'Q':
@@ -57,6 +66,53 @@ int test_strategie(){
     }
 }
 
+int test_homologation(){
+    int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
+
+    i2c_maitre_init();
+    Trajet_init();
+    //printf("Init gyroscope\n");
+    Gyro_Init();
+
+    stdio_flush();
+
+    set_position_avec_gyroscope(1);
+
+    temps_ms = Temps_get_temps_ms();
+    temps_ms_init = temps_ms;
+    do{
+        i2c_gestion(i2c0);
+        i2c_annexe_gestion();
+        // Routines à 1 ms
+        if(temps_ms != Temps_get_temps_ms()){
+            temps_ms = Temps_get_temps_ms();
+            QEI_update();
+            Localisation_gestion();
+            AsserMoteur_Gestion(_step_ms);
+
+            // Routine à 2 ms
+            if(temps_ms % _step_ms_gyro == 0){
+            //    Gyro_Read(_step_ms_gyro);
+            }
+
+            if(temps_ms > temps_ms_init + 200){
+                if(cerise_longer_bord(LONGER_VERS_A) == ACTION_TERMINEE){
+                    printf("Accostage_terminee\n");
+                }
+            }
+
+        }
+        lettre = getchar_timeout_us(0);
+    }while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
+    printf("Lettre : %d; %c\n", lettre, lettre);
+    
+    if(lettre == 'q' && lettre == 'Q'){
+        return 0;
+    }
+    return 1;
+
+}
+
 
 int test_longe(){
     int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;