diff --git a/Strategie.c b/Strategie.c index e8cb57d..9ed7e85 100644 --- a/Strategie.c +++ b/Strategie.c @@ -19,11 +19,7 @@ #define NB_OBJECTIFS 4 -struct objectif_t{ - 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 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){ - objectif_courant = &(objectifs[m_objectif]); + }else if(objectif_courant->etat == A_FAIRE && objectifs[m_objectif].etat == A_FAIRE){ + 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){ @@ -765,4 +773,127 @@ enum etat_action_t Strategie_preparation(){ } return ACTION_EN_COURS; -} \ No newline at end of file +} + +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); + } +} + diff --git a/Strategie.h b/Strategie.h index 2a7d203..6e901a2 100644 --- a/Strategie.h +++ b/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,7 +59,10 @@ 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 -#endif \ No newline at end of file +#endif diff --git a/Strategie_deplacement.c b/Strategie_deplacement.c index 83130d4..f841151 100644 --- a/Strategie_deplacement.c +++ b/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();