Keuronde/RPiPico-Holonome2023
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Prépa Match 5 - retour à la maison

Browse Source
This commit is contained in:
Samuel 2023-05-19 16:43:33 +02:00
parent 0c50b051e4
commit a0f6044799
3 changed files with 155 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

149
Strategie.c
View File

@ -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){
@ -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);
}
}

13
Strategie.h
View File

@ -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

2
Strategie_deplacement.c
View File

@ -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();