From 0e66ae9e4b65d827af652362b8f32691e3d2d0a7 Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Sun, 26 Mar 2023 16:56:34 +0200
Subject: [PATCH] =?UTF-8?q?Initialisation=20du=20test=20de=20l'homologatio?=
 =?UTF-8?q?n=20+=20fonction=20pour=20d=C3=A9finir=20la=20position?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Localisation.c   | 18 +++++++++++++
 Localisation.h   |  5 ++++
 Strategie.c      | 12 +++++++++
 Strategie.h      |  3 ++-
 Test_strategie.c | 68 +++++++++++++++++++++++++++++++++++++++++++-----
 5 files changed, 99 insertions(+), 7 deletions(-)
 create mode 100644 Strategie.c

diff --git a/Localisation.c b/Localisation.c
index b83929f..446fe1c 100644
--- a/Localisation.c
+++ b/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
diff --git a/Localisation.h b/Localisation.h
index cab2c97..2790e27 100644
--- a/Localisation.h
+++ b/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);
\ No newline at end of file
diff --git a/Strategie.c b/Strategie.c
new file mode 100644
index 0000000..15c4188
--- /dev/null
+++ b/Strategie.c
@@ -0,0 +1,12 @@
+
+
+void Homologation(void){
+    static enum etat_strategie_t{
+        STRATEGIE_INIT
+    }etat_strategie;
+
+    switch(etat_strategie){
+        case STRATEGIE_INIT:
+            
+    }
+}
\ No newline at end of file
diff --git a/Strategie.h b/Strategie.h
index 52cead8..eba172a 100644
--- a/Strategie.h
+++ b/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);
\ No newline at end of file
+enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction);
+void Homologation(void);
\ No newline at end of file
diff --git a/Test_strategie.c b/Test_strategie.c
index 0490589..d4c675f 100644
--- a/Test_strategie.c
+++ b/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;