Creation de l'action pour récupérer les cerises latérales.

Holonome2023.c

@@ -82,7 +82,7 @@ int main() {
     AsserMoteur_Init();
     Localisation_init();
 
-    //while(mode_test());
+    while(mode_test());
     i2c_maitre_init();
     Trajet_init();
     Balise_VL53L1X_init();

Strategie.c

@@ -176,6 +176,38 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){
                 etat_strategie = ALLER_PANIER;
             }
             break;
+
+        case ALLER_CERISE_GAUCHE:
+            angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN);
+            if(couleur == COULEUR_BLEU){
+                Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
+                        740, 3000 - 550,
+                        510, 3000 - 1580,
+                        180, 3000 - (1500 - 45),
+                        Localisation_get().angle_radian, angle_fin);
+            }else{
+                Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
+                        1225, 3000 - 540,
+                        440, 3000 - 775,
+                        225, 3000 - (1500 - 45),
+                        Localisation_get().angle_radian, angle_fin);
+
+            }
+
+            if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
+                etat_strategie = ATTRAPER_CERISE_GAUCHE;
+            }
+            break;
+
+        case ATTRAPER_CERISE_GAUCHE:
+            // 1 accoster
+            // 2 Longer en aspirant
+            // 3 avancer en aspirant
+            // 4 Revenir au milieu
+            // 5 accoster
+            // 6 longer en aspirant
+            // 7 avancer en aspirant
+            break;
             
         case ALLER_PANIER:
             if(Strategie_aller_panier(couleur, step_ms) == ACTION_TERMINEE){

Strategie_prise_cerises.c

@@ -20,12 +20,111 @@
 
 void commande_rotation_contacteur_longer_A();
 void commande_rotation_contacteur_longer_C();
+enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum longer_direction_t longer_direction);
+enum etat_action_t demarre_turbine(uint32_t step_ms);
 
 enum longer_direction_t inverser_longe_direction(enum longer_direction_t direction);
 
 
 float vitesse_accostage_mm_s=100;
 
+
+
+
+enum etat_action_t cerises_attraper_cerises_gauches(uint32_t step_ms){
+    static enum {
+        ATTRAPE_CERISE_DEMI_BAS,
+        REVENIR_CENTRE,
+        ATTRAPE_CERISE_DEMI_HAUT,
+    }etat_attrappe_cerises_gauche = ATTRAPE_CERISE_DEMI_BAS;
+    struct trajectoire_t trajectoire;
+    float angle_fin;
+
+    switch (etat_attrappe_cerises_gauche){
+        case ATTRAPE_CERISE_DEMI_BAS:
+            if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_A) == ACTION_TERMINEE){
+                etat_attrappe_cerises_gauche = REVENIR_CENTRE;
+            }
+            break;
+
+        case REVENIR_CENTRE:
+            angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150 * DEGRE_EN_RADIAN);
+
+            Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
+                    180, 1500 - 75, Localisation_get().angle_radian, angle_fin);
+                    
+            if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
+                etat_attrappe_cerises_gauche = ATTRAPE_CERISE_DEMI_HAUT;
+            }
+            break;
+        
+        case ATTRAPE_CERISE_DEMI_HAUT:
+            if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_C) == ACTION_TERMINEE){
+                etat_attrappe_cerises_gauche = ATTRAPE_CERISE_DEMI_BAS;
+                return ACTION_TERMINEE;
+            }
+            break;
+
+    }
+    return ACTION_EN_COURS;
+    
+}
+
+enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum longer_direction_t longer_direction){
+    // 1 accoster
+    // Demarrer la turbine
+    // 2 Longer en aspirant
+    // 3 avancer en aspirant
+    static enum {
+        ACCOSTAGE,
+        DEMARRE_TURBINE,
+        LONGE,
+        AVANCE,
+    }etat_attrappe_demi_cerise=ACCOSTAGE;
+
+    switch(etat_attrappe_demi_cerise){
+        case ACCOSTAGE:
+            if(cerise_accostage() == ACTION_TERMINEE){
+                etat_attrappe_demi_cerise = DEMARRE_TURBINE;
+            }
+            break;
+
+        case DEMARRE_TURBINE:
+            if(demarre_turbine(step_ms) == ACTION_TERMINEE){
+                etat_attrappe_demi_cerise = LONGE;
+            }
+            break;
+
+        case LONGE:
+            avance_puis_longe_bordure(longer_direction);
+            // La fonction cerise_longer_bord n'est efficace que tant que le robot a ses deux contacteur sur le support
+            // Le robot n'a les deux contacteurs sur le support que tant qu'il est à moins de 240mm (MAX_LONGE_MM) de la bordure
+            // ou 120 (MAX_LONGE_MM/2) du milieu de la bordure
+            // En fonction du demi-terrain sur lequel se trouve le robot, on surveille la position en Y pour respecter cette condition
+            if( (Localisation_get().y_mm > 1500 + MAX_LONGE_MM/2 ) || (Localisation_get().y_mm < 1500 - MAX_LONGE_MM/2 )){
+                etat_attrappe_demi_cerise = AVANCE;
+            }
+            break;
+
+        case AVANCE:
+            if(longer_direction == LONGER_VERS_A){
+                commande_translation_longer_vers_A();
+            }else{
+                commande_translation_longer_vers_C();
+            }
+
+            if( (Localisation_get().y_mm > 1500 + MAX_ASPIRE_CERISE_MM/2 ) || (Localisation_get().y_mm < 1500 - MAX_ASPIRE_CERISE_MM/2 )){
+                etat_attrappe_demi_cerise = ACCOSTAGE;
+                i2c_annexe_desactive_turbine();
+                commande_vitesse_stop();
+                return ACTION_TERMINEE;
+            }
+            break;
+    }
+    return ACTION_EN_COURS;
+    
+}
+
 /// @brief Fonction pour attraper les cerises sur les supports perpendiculaires à la bordure.
 /// Le robot accoste, longe le support cerise vers la bordure, active la turbine, puis longe le support cerise jusqu'à son bout.
 /// @param longer_direction : direction dans laquelle se trouve la bordure
@@ -84,7 +183,7 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
             avance_puis_longe_bordure(longer_direction_aspire);
             // La fonction cerise_longer_bord n'est efficace que tant que le robot a ses deux contacteur sur le support
             // Le robot n'a les deux contacteurs sur le support que tant qu'il est à moins de 240mm (MAX_LONGE_MM) de la bordure
-            // En fonction du demi-terrain sur lequel se trouve le robot, on surveille la position en Z pour respecter cette condition
+            // En fonction du demi-terrain sur lequel se trouve le robot, on surveille la position en Y pour respecter cette condition
             if( ((Localisation_get().y_mm > 1500) && (Localisation_get().y_mm < (3000 - MAX_LONGE_MM) )) ||
                 ((Localisation_get().y_mm < 1500) && (Localisation_get().y_mm > (MAX_LONGE_MM))) ){
                 etat_attrape = ASPIRE_LIBRE;
@@ -114,6 +213,37 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
     return etat_action;
 }
 
+/// @brief Envoie l'ordre de démarrer la turbine puis attends 1 seconde
+/// @param step_ms 
+/// @return ACTION_EN_COURS ou ACTION_TERMINEE
+enum etat_action_t demarre_turbine(uint32_t step_ms){
+    static enum {
+        TURBINE_DEMARRAGE,
+        TURBINE_DEMARRAGE_TEMPO,
+    } etat_demarrage_turbine=TURBINE_DEMARRAGE;
+    static uint32_t tempo_ms;
+
+    switch(etat_demarrage_turbine){
+        case TURBINE_DEMARRAGE:
+            i2c_annexe_ferme_porte();
+            i2c_annexe_active_turbine();
+            commande_vitesse_stop();
+            tempo_ms = 1000;
+            etat_demarrage_turbine = TURBINE_DEMARRAGE_TEMPO;
+
+            break;
+
+        case TURBINE_DEMARRAGE_TEMPO:
+            if(temporisation_terminee(&tempo_ms, step_ms)){
+                etat_demarrage_turbine = TURBINE_DEMARRAGE;
+                return ACTION_TERMINEE;
+            }
+            
+            break;
+    }
+    return ACTION_EN_COURS;
+
+}
 
 /// @brief Fonction pour accoster et longer une bordure
 /// @param longer_direction : direction dans laquelle le robot va aller une fois le long de la bordure
@@ -159,7 +289,7 @@ enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_dire
 
 }
 
-/// @brief Viens position le robot contre une bordure ou un support cerise devant lui.
+/// @brief Viens positionner le robot contre une bordure ou un support cerise devant lui.
 enum etat_action_t cerise_accostage(void){
     enum etat_action_t etat_action = ACTION_EN_COURS;
     float rotation;

Strategie_prise_cerises.h

@@ -1,5 +1,6 @@
 #include "Strategie.h"
 enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, float pos_x_mm, float pos_y_mm);
+enum etat_action_t cerises_attraper_cerises_gauches(uint32_t step_ms);
 void commande_translation_longer_vers_A();
 void commande_translation_longer_vers_C();
 void commande_translation_avance_vers_trompe();

Test_strategie.c

@@ -13,6 +13,7 @@
 #include "QEI.h"
 #include "Robot_config.h"
 #include "Strategie.h"
+#include "Strategie_prise_cerises.h"
 #include "Temps.h"
 #include "Trajet.h"
 #include "Trajectoire.h"
@@ -24,6 +25,7 @@ int test_panier(void);
 int test_homologation(void);
 int test_evitement(void);
 int test_tirette_et_couleur();
+int test_cerise_laterales(void);
 void affichage_test_evitement();
 
 void affichage_test_strategie(){
@@ -72,6 +74,7 @@ void affichage_test_strategie(){
 int test_strategie(){
     printf("A - Accoster.\n");
     printf("C - Couleur et tirette.\n");
+    printf("D - Attraper cerises laterales.\n");
     printf("E - Evitement\n");
     printf("H - Homologation.\n");
     printf("L - Longer.\n");
@@ -92,6 +95,11 @@ int test_strategie(){
             while(test_tirette_et_couleur());
             break;
 
+        case 'd':
+        case 'D':
+            while(test_cerise_laterales());
+            break;
+
         case 'e':
         case 'E':
             while(test_evitement());
@@ -99,7 +107,7 @@ int test_strategie(){
 
         case 'h':
         case 'H':
-            while(test_homologation());
+            //while(test_homologation());
             break;
 
         case 'l':
@@ -121,6 +129,93 @@ int test_strategie(){
 }
 
 
+int test_cerise_laterales(){
+    int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
+    uint32_t temps_cycle_old;
+    enum etat_action_t etat_action;
+    printf("Attaper cerise latérales\n");
+
+    i2c_maitre_init();
+    Trajet_init();
+    Balise_VL53L1X_init();
+    //printf("Init gyroscope\n");
+    set_position_avec_gyroscope(0);
+    if(get_position_avec_gyroscope()){
+        Gyro_Init();
+    }
+
+    stdio_flush();
+
+    multicore_launch_core1(affichage_test_strategie);
+
+    temps_ms = Temps_get_temps_ms();
+    temps_ms_init = temps_ms;
+    temps_cycle_old= time_us_32();
+
+    uint32_t tempo_ms=1000;
+
+    Localisation_set(250, 1500, -150 * DEGRE_EN_RADIAN);
+    do{
+        etat_action = ACTION_EN_COURS;
+        
+        temps_cycle_check();
+        
+        i2c_gestion(i2c0);
+        i2c_annexe_gestion();
+        Balise_VL53L1X_gestion();
+        
+        // Routines à 1 ms
+        
+        if(temps_ms != Temps_get_temps_ms()){
+            static enum {
+                TEMPO_AVANT,
+                TEST,
+                TEMPO_APRES
+            }etat_test;
+            temps_ms = Temps_get_temps_ms();
+            QEI_update();
+            Localisation_gestion();
+            AsserMoteur_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 TEMPO_AVANT:
+                    if(temporisation_terminee(&tempo_ms, _step_ms)){
+                        etat_test = TEST;
+                    }
+                    break;
+                case TEST:
+                    if(cerises_attraper_cerises_gauches(_step_ms) == ACTION_TERMINEE){
+                        tempo_ms = 1000;
+                        etat_test = TEMPO_APRES;
+                    }
+                    break;
+                case TEMPO_APRES:
+                    if(temporisation_terminee(&tempo_ms, _step_ms)){
+                        etat_test = TEMPO_AVANT;
+                        etat_action = ACTION_TERMINEE;
+                    }
+                    break;
+            }
+
+            
+
+        }
+        //lettre = getchar_timeout_us(0);
+    //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
+    }while(etat_action == ACTION_EN_COURS);
+    Moteur_Stop();
+    return 0;
+
+}
+
 int test_homologation(){
     int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
     uint32_t temps_cycle[10], temps_cycle_old, index_temps_cycle=0;
@@ -174,7 +269,7 @@ int test_homologation(){
                 }
             }
 
-            Homologation(_step_ms);
+            //Homologation(_step_ms);
 
         }
         //lettre = getchar_timeout_us(0);

i2c_annexe.c

@@ -73,10 +73,8 @@ void i2c_annexe_couleur_balise(uint8_t couleur, uint16_t masque_led){
 }
 
 void i2c_annexe_active_turbine(void){
-    /*
     donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x01;
     donnees_a_envoyer=1;
-    */
 }
 void i2c_annexe_desactive_turbine(void){
     donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xFE;