From a0f60447998b6a0e313e4cb93314c691895398e3 Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Fri, 19 May 2023 16:43:33 +0200
Subject: [PATCH] =?UTF-8?q?Pr=C3=A9pa=20Match=205=20-=20retour=20=C3=A0=20?=
=?UTF-8?q?la=20maison?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
Strategie.c | 151 +++++++++++++++++++++++++++++++++++++---
Strategie.h | 15 +++-
Strategie_deplacement.c | 2 +-
3 files changed, 155 insertions(+), 13 deletions(-)
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();