Prépa Match 5 - retour à la maison

Strategie.c

@@ -19,11 +19,7 @@
 
 #define NB_OBJECTIFS 4
 
-struct objectif_t{
+struct objectif_t objectifs[NB_OBJECTIFS];
-    int priorite;
-    enum {A_FAIRE, EN_COURS, BLOQUE, FAIT} etat;
-    enum {CERISE_BAS, CERISE_HAUT, CERISE_GAUCHE, CERISE_DROITE} cible;
-} objectifs[NB_OBJECTIFS];
 struct objectif_t * objectif_courant;
 
 uint32_t Score_cerises_dans_robot=0;
@@ -56,7 +52,9 @@ enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t cou
 
 
 void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
-    
+    //const uint32_t temps_pre_fin_match = (97000 - 15000);
+    const uint32_t temps_pre_fin_match = (15000);
+    static bool pre_fin_match_active=false;
     float angle_fin;
     float recal_pos_x, recal_pos_y;
     float point_x, point_y;
@@ -78,6 +76,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
         ATTRAPER_CERISE_DROITE,
         ALLER_PANIER,
         LANCER_PANIER,
+        RETOUR_MAISON,
         DECISION_STRATEGIE,
         STRATEGIE_FIN
     }etat_strategie;
@@ -86,6 +85,13 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
         etat_strategie=STRATEGIE_FIN;
     }
 
+    if(temps_ms > temps_pre_fin_match){
+        if(etat_strategie != LANCER_PANIER){
+            etat_strategie = RETOUR_MAISON;
+        }
+        pre_fin_match_active=true;
+    }
+
     switch(etat_strategie){
         case STRATEGIE_INIT:
             if(couleur == COULEUR_BLEU){
@@ -258,12 +264,14 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
             for(uint m_objectif=0; m_objectif < NB_OBJECTIFS; m_objectif++){
                 if(objectif_courant->etat == FAIT && objectifs[m_objectif].etat == A_FAIRE){
                     objectif_courant = &(objectifs[m_objectif]);
-                }else if(objectif_courant->priorite > objectifs[m_objectif].priorite){
+                }else if(objectif_courant->etat == A_FAIRE && objectifs[m_objectif].etat == A_FAIRE){
-                    objectif_courant = &(objectifs[m_objectif]);
+                    if(objectif_courant->priorite > objectifs[m_objectif].priorite){
+                        objectif_courant = &(objectifs[m_objectif]);
+                    }
                 }
             }
 
-            if(objectif_courant->etat == FAIT){
+            if(objectif_courant->etat == FAIT || pre_fin_match_active){
                 etat_strategie = STRATEGIE_FIN;
             }else{
                 switch(objectif_courant->cible){
@@ -766,3 +774,126 @@ enum etat_action_t Strategie_preparation(){
     return ACTION_EN_COURS;
 
 }
+
+enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, uint32_t step_ms){
+    struct trajectoire_t trajectoire;
+
+    Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
+        pos_x, pos_y,
+        Localisation_get().angle_radian, Localisation_get().angle_radian);
+
+    return Strategie_parcourir_trajet(trajectoire, step_ms, ARRET_DEVANT_OBSTACLE);
+}
+
+
+enum etat_action_t Strategie_pieds_dans_plat(enum couleur_t couleur, uint32_t step_ms){
+    static struct objectif_t objectifs_plats[5], *objectif_plat_courant = &objectifs_plats[0];
+    enum etat_action_t etat_action;
+
+
+    struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = ZONE_1};
+    struct objectif_t objectif_2 = { .priorite = 4, .etat = A_FAIRE, .cible = ZONE_2};
+    struct objectif_t objectif_3 = { .priorite = 3, .etat = A_FAIRE, .cible = ZONE_3};
+    struct objectif_t objectif_4 = { .priorite = 2, .etat = A_FAIRE, .cible = ZONE_4};
+    struct objectif_t objectif_5 = { .priorite = 5, .etat = A_FAIRE, .cible = ZONE_5};
+    objectifs_plats[0] = objectif_1;
+    objectifs_plats[1] = objectif_2;
+    objectifs_plats[2] = objectif_3;
+    objectifs_plats[3] = objectif_4;
+    objectifs_plats[4] = objectif_5;
+
+    etat_action = Strategie_pieds_dans_plat_trajet(objectif_plat_courant, couleur, step_ms);
+    switch(etat_action){
+        case ACTION_TERMINEE:
+            return ACTION_TERMINEE;
+        case ACTION_EN_COURS:
+            return ACTION_EN_COURS;
+        case ACTION_ECHEC:
+            // 1. Marquer comme bloqué l'objectif en cours
+            objectif_plat_courant->etat = BLOQUE;
+            // 2. Si tous les objectifs sont bloqués, les marquer tous comme A_FAIRE
+            uint8_t non_bloque = 0;
+            for(uint i=0 ; i<5 ; i++){
+                if(objectifs_plats[i].etat == A_FAIRE){
+                    non_bloque=1;
+                }
+            }
+            if(!non_bloque){
+                for(uint i=0 ; i<5 ; i++){
+                    if (objectifs_plats[i].etat == BLOQUE){
+                        objectifs_plats[i].etat = A_FAIRE;
+                    }
+                }
+            }
+            // 3. Trouver le prochain objectif (priorité la plus basse + etat A_FAIRE)
+            // Si notre objectif est FAIT, on prend le premier objectif "A_FAIRE"
+            // Si notre objectif est A_FAIRE, on prend le nouvel objectif que si sa priorité est plus basse.
+            for(uint i=0; i < 5; i++){
+                if(objectif_plat_courant->etat == FAIT && objectifs_plats[i].etat == A_FAIRE){
+                    objectif_plat_courant = &(objectifs_plats[i]);
+                }else if(objectif_plat_courant->etat == A_FAIRE && objectifs_plats[i].etat == A_FAIRE){
+                    if(objectif_plat_courant->priorite > objectifs_plats[i].priorite){
+                        objectif_plat_courant = &(objectifs_plats[i]);
+                    }
+
+                } 
+            }
+
+    }
+
+
+
+}
+
+enum etat_action_t Strategie_pieds_dans_plat_trajet(struct objectif_t *objectif_plat_courant, enum couleur_t couleur, uint32_t step_ms){
+    float pos_x;
+    float pos_y;
+
+    switch (objectif_plat_courant->cible){
+        case ZONE_1:
+            pos_y = 2775;
+            if (couleur == COULEUR_BLEU){
+                pos_x = 250;
+            }else{
+                pos_x = 2000 - 250;
+            }
+            return Strategie_aller_a(pos_x, pos_y, step_ms);
+
+        case ZONE_2:
+            pos_y = 1125;
+            if (couleur == COULEUR_BLEU){
+                pos_x = 250;
+            }else{
+                pos_x = 2000 - 250;
+            }
+            return Strategie_aller_a(pos_x, pos_y, step_ms);
+
+        case ZONE_3:
+            pos_y = 250;
+            if (couleur == COULEUR_BLEU){
+                pos_x = 725;
+            }else{
+                pos_x = 2000 - 725;
+            }
+            return Strategie_aller_a(pos_x, pos_y, step_ms);
+
+        case ZONE_4:
+            pos_y = 250;
+            if (couleur == COULEUR_BLEU){
+                pos_x = 2000 - 250;
+            }else{
+                pos_x = 250;
+            }
+            return Strategie_aller_a(pos_x, pos_y, step_ms);
+
+        case ZONE_5:
+            pos_y = 1850;
+            if (couleur == COULEUR_BLEU){
+                pos_x = 2000 - 250;
+            }else{
+                pos_x = 250;
+            }
+            return Strategie_aller_a(pos_x, pos_y, step_ms);
+    }
+}
+

Strategie.h

@@ -11,7 +11,8 @@
 
 enum etat_action_t{
     ACTION_EN_COURS,
-    ACTION_TERMINEE
+    ACTION_TERMINEE,
+    ACTION_ECHEC
 };
 
 enum longer_direction_t{
@@ -33,6 +34,13 @@ enum evitement_t{
     CONTOURNEMENT
 };
 
+struct objectif_t{
+    int priorite;
+    enum {A_FAIRE, EN_COURS, BLOQUE, FAIT} etat;
+    enum {CERISE_BAS, CERISE_HAUT, CERISE_GAUCHE, CERISE_DROITE, 
+        ZONE_1, ZONE_2, ZONE_3, ZONE_4, ZONE_5} cible;
+};
+
 enum etat_action_t cerise_accostage(void);
 enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_direction);
 enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms);
@@ -51,6 +59,9 @@ enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire,
 int Robot_est_dans_zone_depose(enum couleur_t couleur);
 enum etat_action_t Strategie_preparation();
 
+enum etat_action_t Strategie_pieds_dans_plat_trajet(struct objectif_t *objectif_plat_courant, enum couleur_t couleur, uint32_t step_ms);
+enum etat_action_t Strategie_pieds_dans_plat(enum couleur_t couleur, uint32_t step_ms);
+
 extern float distance_obstacle;
 
 // STRATEGIE_H

Strategie_deplacement.c

@@ -38,7 +38,7 @@ enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire,
                     
                     case ARRET_DEVANT_OBSTACLE:
                         etat_parcourt = PARCOURS_INIT;
-                        return ACTION_TERMINEE;
+                        return ACTION_ECHEC;
 
                     case RETOUR_SI_OBSTABLE:
                         Trajet_inverse();