From 189101b849e4395954a315382d1f9599c5d49558 Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Wed, 1 May 2024 09:23:28 +0200
Subject: [PATCH] =?UTF-8?q?Tentative=20de=20pr=C3=A9-homologation=20:=20ma?=
 =?UTF-8?q?rquer=20des=20points?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Strategie_2024_plante.c |  79 ++++++++++++++--
 Strategie_2024_plante.h |   5 +-
 Strategie_2024_pots.c   |  21 +++--
 Strategie_2024_pots.h   |   3 +
 Test_i2c.c              |   6 +-
 Test_strategie_2024.c   | 194 ++++++++++++++++++++++++++++++++++++++--
 6 files changed, 281 insertions(+), 27 deletions(-)

diff --git a/Strategie_2024_plante.c b/Strategie_2024_plante.c
index 2a35382..08610bb 100644
--- a/Strategie_2024_plante.c
+++ b/Strategie_2024_plante.c
@@ -3,12 +3,14 @@
 #include "Geometrie_robot.h"
 #include "Localisation.h"
 #include "i2c_annexe.h"
-#include "math.h"
+#include <math.h>
 #include <stdio.h>
 
 #define ASSER_ANGLE_GAIN_PLANTE_P 1.5
 #define ASSER_DISTANCE_GAIN_PLANTE_P 10
 
+#define RAYON_ZONE_PLANTE_MM 180
+
 struct position_t liste_zone_plante[]=
 {
   {.x_mm = 1500, .y_mm = 1500 },
@@ -30,26 +32,75 @@ enum etat_action_t Strat_2024_aller_zone_plante(enum zone_plante_t zone_plante,
     angle = atan2f(position_zone_plante.y_mm - position_robot.y_mm, position_zone_plante.x_mm - position_robot.x_mm);
     return Strategie_tourner_a(angle - ANGLE_PINCE, step_ms);
 }
+/// @brief Indique si la plante se trouve dans la zone de récole (en comptant une marge)
+/// @param position_plante : position de la plante
+/// @param zone_plante : ID de la zone dans laquelle on pense trouver la plante
+/// @return 1 si la plante est dans la zone, 0 si le robot ne trouve pas la plante ou qu'elle est hors zone.
+bool est_dans_zone(struct position_t position_plante, enum zone_plante_t zone_plante){
+    struct position_t position_zone_plante;
+    float distance_plante_zone;
 
-enum etat_action_t Strat_2024_aller_a_plante(void){
+    position_zone_plante = liste_zone_plante[zone_plante];
+    
+    distance_plante_zone = sqrtf( powf(position_zone_plante.x_mm - position_zone_plante.x_mm, 2) + 
+                                powf(position_zone_plante.y_mm - position_zone_plante.y_mm, 2));
+    
+    if(distance_plante_zone < RAYON_ZONE_PLANTE_MM){
+        return 1;
+    }
+    return 0;   
+}
+
+/// @brief Déplace le robot vers une plante, vérifie que la plante est bien dans la zone plante qu'on vise
+/// @param zone_plante : ID de la zone dans laquelle on pense trouver la plante
+/// @return ACTION_SUCCESS si le robot est prêt à attraper la plante, ACTION_ECHEC si la plante 
+enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){
     static enum {
         SAAP_INIT_DETECTION,
         SAAP_ASSERV
     } etat_aller_a_plante = SAAP_INIT_DETECTION;
     enum validite_vl53l8_t validite;
-    float angle, distance, commande_vitesse_plante;
-    static int tempo;
+    float angle_rad, distance_mm, commande_vitesse_plante;
+    static int tempo_ms;
+    static bool entree_dans_zone;
 
     switch(etat_aller_a_plante){
         case SAAP_INIT_DETECTION:
             i2c_annexe_set_mode_VL53L8(VL53L8_PLANTE);
+            tempo_ms = 2000; 
+            entree_dans_zone=false;
             etat_aller_a_plante = SAAP_ASSERV;
             break;
 
         case SAAP_ASSERV:
-            i2c_annexe_get_VL53L8(&validite, &angle, &distance);
+            i2c_annexe_get_VL53L8(&validite, &angle_rad, &distance_mm);
             if(validite == VL53L8_PLANTE){
-                commande_vitesse_plante = (distance - 70) * ASSER_DISTANCE_GAIN_PLANTE_P;
+                tempo_ms = 2000;
+                // 1 on s'assure que la plante est dans la zone qu'on recherche !
+                if(zone_plante != ZONE_PLANTE_AUCUNE){
+                    // Cas où on checher une plante dans une zone
+                    struct position_t position_robot, position_plante;
+                    bool robot_dans_zone;
+                    position_robot = Localisation_get();
+                    position_robot.angle_radian += ANGLE_PINCE + angle_rad;
+                    position_plante = Geometrie_deplace(position_robot, distance_mm);
+                    if( !est_dans_zone(position_plante, zone_plante)){
+                        return ACTION_ECHEC;
+                    }
+                    robot_dans_zone = est_dans_zone(Localisation_get(), zone_plante);
+                    if(entree_dans_zone == true){
+                        if(robot_dans_zone == false){
+                            // Le robot est sorti de la zone
+                            return ACTION_ECHEC;
+                        }
+                    }
+                    if(robot_dans_zone == true){
+                        entree_dans_zone = true;
+                    }
+                }
+
+                // 2 on s'assure qu'il n'y a pas de robot en face (TODO)
+                commande_vitesse_plante = (distance_mm - 70) * ASSER_DISTANCE_GAIN_PLANTE_P;
                 if(commande_vitesse_plante > 300){
                     commande_vitesse_plante = 300;
                 }
@@ -61,8 +112,12 @@ enum etat_action_t Strat_2024_aller_a_plante(void){
                 }
                 
                 commande_vitesse(cosf(ANGLE_PINCE) * commande_vitesse_plante ,
-                    sinf(ANGLE_PINCE) * commande_vitesse_plante , angle * ASSER_ANGLE_GAIN_PLANTE_P);
+                    sinf(ANGLE_PINCE) * commande_vitesse_plante , angle_rad * ASSER_ANGLE_GAIN_PLANTE_P);
             }else{
+                tempo_ms--;
+                if(tempo_ms <= 0){
+                    return ACTION_ECHEC;
+                }
             }
             break;
     }
@@ -74,7 +129,7 @@ enum etat_action_t Strat_2024_aller_a_plante(void){
 /// @param step_ms 
 /// @param bras_depose_pot : PLANTE_BRAS_1 ou PLANTE_BRAS_6
 /// @return ACTION_EN_COURS ou ACTION_TERMINEE
-enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot){
+enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot, enum zone_plante_t zone_plante){
     static enum{
         PDP_ALLER_PLANTE,
         PDP_PRE_PRISE_PLANTE,
@@ -90,7 +145,7 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot
     switch (etat_plante_dans_pot)
     {
         case PDP_ALLER_PLANTE:
-            if (Strat_2024_aller_a_plante() == ACTION_TERMINEE){
+            if (Strat_2024_aller_a_plante(zone_plante) == ACTION_TERMINEE){
                 etat_plante_dans_pot=PDP_PRE_PRISE_PLANTE;
             }
             break;
@@ -152,6 +207,12 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot
             tempo_ms--;
             if(tempo_ms <= 0){
                 etat_plante_dans_pot=PDP_ALLER_PLANTE;
+                if(bras_depose_pot == PLANTE_BRAS_1){
+                    i2c_annexe_actionneur_pot(0, BRAS_HAUT, DOIGT_TIENT);
+                }else{
+                    i2c_annexe_actionneur_pot(5, BRAS_HAUT, DOIGT_TIENT);
+                }
+                
                 return ACTION_TERMINEE;
             }            
         default:
diff --git a/Strategie_2024_plante.h b/Strategie_2024_plante.h
index 15a8e16..fd3faa2 100644
--- a/Strategie_2024_plante.h
+++ b/Strategie_2024_plante.h
@@ -7,8 +7,9 @@ enum zone_plante_t{
     ZONE_PLANTE_4=3,
     ZONE_PLANTE_5=4,
     ZONE_PLANTE_6=5,
+    ZONE_PLANTE_AUCUNE=99
 };
 
 enum etat_action_t Strat_2024_aller_zone_plante(enum zone_plante_t zone_plante, uint32_t step_ms);
-enum etat_action_t Strat_2024_aller_a_plante();
-enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot);
+enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante);
+enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot, enum zone_plante_t zone_plante);
diff --git a/Strategie_2024_pots.c b/Strategie_2024_pots.c
index 74ff47a..b7f6407 100644
--- a/Strategie_2024_pots.c
+++ b/Strategie_2024_pots.c
@@ -6,8 +6,7 @@
 #include "i2c_annexe.h"
 #include "Localisation.h"
 
-#define DISTANCE_APPROCHE_POT_MM 300. 
-#define DISTANCE_ATTRAPE_POT_MM 185.
+
 
 float angle_bras[6] =
 {
@@ -19,6 +18,16 @@ float angle_bras[6] =
     -120 * DEGRE_EN_RADIAN
 };
 
+float angle_bras_correction[6] =
+{
+    0 * DEGRE_EN_RADIAN,
+    0 * DEGRE_EN_RADIAN,
+    0 * DEGRE_EN_RADIAN,
+    0,
+    0 * DEGRE_EN_RADIAN,
+    7 * DEGRE_EN_RADIAN
+};
+
 enum etat_bras_t{
     BRAS_LIBRE,
     BRAS_OCCUPE,
@@ -68,12 +77,12 @@ struct position_t groupe_pot_get_pot(unsigned int groupe_pot, unsigned int num_p
 }
 
 /// @brief A Affiner
-/// @param  
-/// @return 
+/// @return numéro du bras libre (entre 0 et 5)
+int ordre_bras[NB_BRAS]={5, 0, 1, 2, 3, 4};
 int get_bras_libre(void){
     for (int i=0; i<NB_BRAS; i++){
-        if(etat_bras[i] == BRAS_LIBRE){
-            return i;
+        if(etat_bras[ordre_bras[i]] == BRAS_LIBRE){
+            return ordre_bras[i];
         }
     }
     return 9;
diff --git a/Strategie_2024_pots.h b/Strategie_2024_pots.h
index 718bd25..3e78b2a 100644
--- a/Strategie_2024_pots.h
+++ b/Strategie_2024_pots.h
@@ -21,5 +21,8 @@
 #define BRAS_5 4
 #define BRAS_6 5
 
+#define DISTANCE_APPROCHE_POT_MM 300. 
+#define DISTANCE_ATTRAPE_POT_MM 185.
+
 struct position_t groupe_pot_get_pot(unsigned int groupe_pot, unsigned int num_pot);
 enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step_ms);
diff --git a/Test_i2c.c b/Test_i2c.c
index 732cff2..314af75 100644
--- a/Test_i2c.c
+++ b/Test_i2c.c
@@ -292,9 +292,9 @@ int test_i2c_ecriture_pico_annex_nb_2(){
     enum i2c_resultat_t retour_i2c = I2C_EN_COURS;
 
 
-    printf("A - Aimant tient pot\n");
+    printf("A - Aimant lache pot\n");
     printf("B - Bras attrape pot\n");
-    printf("C - Aimant lache pot\n");
+    printf("C - Aimant tient pot\n");
     printf("D - Bras lève pot\n");
     printf("E - Attrape plante - bras 1\n");
     printf("F - Attrape plante - bras 6\n");
@@ -432,7 +432,7 @@ int test_i2c_ecriture_pico_annex_nb_2(){
                     printf("lettre non reconnue: %d %c\n", lettre, lettre);
             }
         }
-        //sleep_ms(1);
+        sleep_ms(1);
 
         i2c_gestion(i2c0);        
         i2c_annexe_gestion();
diff --git a/Test_strategie_2024.c b/Test_strategie_2024.c
index 4c1ca2f..275045a 100644
--- a/Test_strategie_2024.c
+++ b/Test_strategie_2024.c
@@ -27,6 +27,7 @@ int test_aller_a_plante(void);
 int test_attrape_plante(void);
 int test_aller_zone_plante();
 int test_pseudo_homologation(void);
+int test_attrape_1_pot(void);
 void affichage_test_strategie_2024(void);
 
 int test_strategie_2024(){
@@ -37,6 +38,7 @@ int test_strategie_2024(){
     printf("E - Attrape plante.\n");
     printf("F - Aller zone plante.\n");
     printf("G - Pseudo homologation.\n");
+    printf("H - réglage pots.\n");
     
     int lettre;
     do{
@@ -78,6 +80,11 @@ int test_strategie_2024(){
             while(test_pseudo_homologation());
             break;
 
+        case 'h':
+        case 'H':
+            while(test_attrape_1_pot());
+            break;
+
         case 'q':
         case 'Q':
             return 0;
@@ -235,7 +242,7 @@ int test_attrape_plante(){
                 }
             }
 
-            etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_6);
+            etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_6, ZONE_PLANTE_3);
 
 
         }
@@ -294,7 +301,7 @@ int test_aller_a_plante(){
                 }
             }
 
-            etat_action = Strat_2024_aller_a_plante();
+            etat_action = Strat_2024_aller_a_plante(ZONE_PLANTE_AUCUNE);
 
 
         }
@@ -461,7 +468,10 @@ int test_pseudo_homologation(){
         TAP_CALAGE,
         TAP_POT,
         TAP_PLANTE_ORIENTATION,
-        TAP_PLANTE_ATTRAPE
+        TAP_PLANTE_ATTRAPE_1,
+        TAP_PLANTE_ATTRAPE_2,
+        TAP_RENTRE,
+        TAP_DEPOSE
     } etat_test = TAP_CALAGE;
 
     printf("test_attrape_pot\n");
@@ -520,15 +530,43 @@ int test_pseudo_homologation(){
 
                 case TAP_PLANTE_ORIENTATION:
                     if(Strat_2024_aller_zone_plante(ZONE_PLANTE_3, _step_ms) == ACTION_TERMINEE){
-                        etat_test=TAP_PLANTE_ATTRAPE;
+                        etat_test=TAP_PLANTE_ATTRAPE_1;
                     }
                     break;
 
-                case TAP_PLANTE_ATTRAPE:
-                    if(Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1) == ACTION_TERMINEE){
-                        etat_action= ACTION_TERMINEE;
+                case TAP_PLANTE_ATTRAPE_1:
+                    etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, ZONE_PLANTE_3);
+                    if( etat_action == ACTION_TERMINEE){
+                        etat_test=TAP_PLANTE_ATTRAPE_2;
+                        etat_action = ACTION_EN_COURS;
+                    }else if( etat_action == ACTION_ECHEC){
+                        etat_test=TAP_RENTRE;
+                        etat_action = ACTION_EN_COURS;
                     }
                     break;
+                
+                case TAP_PLANTE_ATTRAPE_2:
+                    etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_6, ZONE_PLANTE_3);
+                    if( etat_action == ACTION_TERMINEE){
+                        etat_test=TAP_RENTRE;
+                        etat_action = ACTION_EN_COURS;
+                    }else if( etat_action == ACTION_ECHEC){
+                        etat_test=TAP_RENTRE;
+                        etat_action = ACTION_EN_COURS;
+                    }
+                    break;
+
+                case TAP_RENTRE:
+                    if(Strategie_aller_a(250, 250, _step_ms) == ACTION_TERMINEE){
+                        etat_test=TAP_DEPOSE;
+                    }
+                    break;
+                
+                case TAP_DEPOSE:
+                    i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_LACHE);
+                    i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_LACHE);
+                    break;
+
             }
 
         }
@@ -587,3 +625,145 @@ void affichage_test_strategie_2024(){
         sleep_ms(100);
     }
 }
+
+/// @brief Fonction abandonnée - pour le réglage des bras un par un 
+/// @param  
+/// @return 
+int test_attrape_1_pot(void){
+    int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
+    struct trajectoire_t trajectoire;
+    enum evitement_t evitement;
+    enum etat_action_t etat_action=ACTION_EN_COURS;
+    int32_t tempo_ms;
+
+    enum {
+        TAP_CALAGE,
+        TAP_APPROCHE_POT,
+        TAP_TOURNE_POT,
+        TAP_ATTRAPE_POT,
+        TAP_ATTRAPE_TEMPO
+    } etat_test = TAP_CALAGE;
+
+    printf("test_attrape_1_pot\n");
+
+
+    i2c_maitre_init();
+    Trajet_init();
+    Balise_VL53L1X_init();
+    Localisation_set(250, 250, (45. * DEGRE_EN_RADIAN) - ANGLE_PINCE);
+
+    
+    set_position_avec_gyroscope(1);
+    if(get_position_avec_gyroscope()){
+        printf("Init gyroscope\n");
+        Gyro_Init();
+    }
+
+    stdio_flush();
+    Trajet_config(TRAJECT_CONFIG_STD);
+
+    multicore_launch_core1(affichage_test_strategie_2024);
+
+    float angle_bras[6] =
+    {
+        180 * DEGRE_EN_RADIAN,
+        120 * DEGRE_EN_RADIAN,
+        60 * DEGRE_EN_RADIAN,
+        0,
+        -60 * DEGRE_EN_RADIAN,
+        -120 * DEGRE_EN_RADIAN
+    };
+
+    float angle_bras_correction[6] =
+    {
+        11 * DEGRE_EN_RADIAN,
+        0 * DEGRE_EN_RADIAN,
+        0 * DEGRE_EN_RADIAN,
+        0,
+        0 * DEGRE_EN_RADIAN,
+        13 * DEGRE_EN_RADIAN
+    };
+
+
+    temps_ms = Temps_get_temps_ms();
+    temps_ms_init = temps_ms;
+    static struct position_t position_pot, position_approche_pot, position_attrape_pot;
+    position_pot = groupe_pot_get_pot(GROUPE_POT_B1, POT_5);
+    position_approche_pot = Geometrie_deplace(position_pot, -DISTANCE_APPROCHE_POT_MM);
+    position_attrape_pot = Geometrie_deplace(position_pot, -DISTANCE_ATTRAPE_POT_MM);
+
+    int bras = BRAS_1;
+
+    do{
+        i2c_gestion(i2c0);
+        i2c_annexe_gestion();
+        Balise_VL53L1X_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);
+            Evitement_gestion(_step_ms);
+
+            // Routine à 2 ms
+            if(temps_ms % _step_ms_gyro == 0){
+                if(get_position_avec_gyroscope()){
+                    Gyro_Read(_step_ms_gyro);
+                }
+            }
+            switch(etat_test){
+                case TAP_CALAGE:
+                    if(Strategie_calage_debut(COULEUR_BLEU, _step_ms) == ACTION_TERMINEE){
+                        etat_test=TAP_APPROCHE_POT;
+                    }
+                    break;
+                case TAP_APPROCHE_POT:
+                    Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
+                    if(Strategie_aller_a(position_approche_pot.x_mm, position_approche_pot.y_mm, _step_ms) == ACTION_TERMINEE){
+                        etat_test=TAP_TOURNE_POT;
+                    }
+                    break;
+
+                case TAP_TOURNE_POT:
+                    if( Strategie_tourner_a(position_approche_pot.angle_radian - angle_bras[bras] + angle_bras_correction[bras], _step_ms) == ACTION_TERMINEE){
+                        etat_test=TAP_ATTRAPE_POT;
+                    }
+                    break;
+                
+                case TAP_ATTRAPE_POT:
+                    Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
+                    i2c_annexe_actionneur_pot(bras, BRAS_POT_SOL, DOIGT_TIENT);
+                    if( Strategie_tourner_et_aller_a(
+                            position_attrape_pot.x_mm, position_attrape_pot.y_mm, position_attrape_pot.angle_radian - angle_bras[bras] + angle_bras_correction[bras],
+                            SANS_EVITEMENT, _step_ms) == ACTION_TERMINEE){
+                        i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_TIENT);
+                        tempo_ms = 2000;
+                        etat_test = TAP_ATTRAPE_TEMPO;
+                    }
+                    break;
+                
+                case TAP_ATTRAPE_TEMPO:
+                    tempo_ms--;
+                    if(tempo_ms < 0){
+                        etat_action = ACTION_TERMINEE;
+                    }
+                    break;
+            }
+
+        }
+        lettre = getchar_timeout_us(0);
+    //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
+    }while(etat_action == ACTION_EN_COURS);
+    printf("STRATEGIE_LOOP_2\n");
+    printf("Lettre : %d; %c\n", lettre, lettre);
+
+    while(1){Moteur_Stop();}
+    
+    if(lettre == 'q' && lettre == 'Q'){
+        return 0;
+    }
+    return 0;
+
+}