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

Debut de la gestion des objectifs

Browse Source
This commit is contained in:
Samuel 2023-05-08 11:29:23 +02:00
parent e5e8c23879
commit 2a0bde7385
2 changed files with 167 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

168
Strategie.c
View File

@ -16,6 +16,17 @@
#define DISTANCE_OBSTACLE_CM 50
#define DISTANCE_PAS_OBSTACLE_MM 2000
#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 * objectif_courant;
// TODO: Peut-être à remetttre en variable locale après
float distance_obstacle;
@ -30,6 +41,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){
float angle_fin;
float recal_pos_x, recal_pos_y;
float point_x, point_y;
enum longer_direction_t longer_direction;
enum etat_action_t etat_action;
enum etat_trajet_t etat_trajet;
@ -48,15 +60,33 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){
ATTRAPER_CERISE_DROITE,
ALLER_PANIER,
LANCER_PANIER,
DECISION_STRATEGIE,
}etat_strategie;
switch(etat_strategie){
case STRATEGIE_INIT:
if(couleur == COULEUR_BLEU){
Localisation_set(775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_BAS};
struct objectif_t objectif_2 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_HAUT};
struct objectif_t objectif_3 = { .priorite = 3, .etat = A_FAIRE, .cible = CERISE_GAUCHE};
struct objectif_t objectif_4 = { .priorite = 4, .etat = A_FAIRE, .cible = CERISE_DROITE};
objectifs[0]= objectif_1;
objectifs[1]= objectif_2;
objectifs[2]= objectif_3;
objectifs[3]= objectif_4;
}else{
Localisation_set(2000 - 775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_BAS};
struct objectif_t objectif_2 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_HAUT};
struct objectif_t objectif_3 = { .priorite = 3, .etat = A_FAIRE, .cible = CERISE_DROITE};
struct objectif_t objectif_4 = { .priorite = 4, .etat = A_FAIRE, .cible = CERISE_GAUCHE};
objectifs[0]= objectif_1;
objectifs[1]= objectif_2;
objectifs[2]= objectif_3;
objectifs[3]= objectif_4;
}
objectif_courant = &objectifs[0];
etat_strategie = ALLER_CERISE_BAS;
break;
@ -64,12 +94,14 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){
if(couleur == COULEUR_BLEU){
angle_fin = 30. * DEGRE_EN_RADIAN;
point_x = 857;
}else{
angle_fin = -150. * DEGRE_EN_RADIAN;
point_x = 2000 - 857;
}
Trajet_config(250, 500);
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 857, 156,
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 156,
Localisation_get().angle_radian, angle_fin);
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){
@ -92,6 +124,41 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){
etat_strategie = ALLER_PANIER;
}
break;
case ALLER_CERISE_HAUT:
if(couleur == COULEUR_BLEU){
angle_fin = 30. * DEGRE_EN_RADIAN;
point_x = 857;
}else{
angle_fin = -150. * DEGRE_EN_RADIAN;
point_x = 2000 - 857;
}
Trajet_config(250, 500);
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 156,
Localisation_get().angle_radian, angle_fin);
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){
etat_strategie = ATTRAPER_CERISE_HAUT;
}
break;
case ATTRAPER_CERISE_HAUT:
recal_pos_y = 3000 - RAYON_ROBOT;
if(couleur == COULEUR_BLEU){
longer_direction = LONGER_VERS_C;
recal_pos_x = 1000 - 15 - PETIT_RAYON_ROBOT_MM;
}else{
longer_direction = LONGER_VERS_A;
recal_pos_x = 1000 + 15 + PETIT_RAYON_ROBOT_MM;
}
etat_action = cerise_attraper_bordure(longer_direction, step_ms, recal_pos_x, recal_pos_y);
if(etat_action == ACTION_TERMINEE){
etat_strategie = ALLER_PANIER;
}
break;
case ALLER_PANIER:
Trajet_config(500, 500);
@ -116,10 +183,42 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){
case LANCER_PANIER:
if(lance_balles_dans_panier(couleur, step_ms)== ACTION_TERMINEE){
etat_strategie = STRATEGIE_FIN;
objectif_courant->etat = FAIT;
etat_strategie = DECISION_STRATEGIE;
}
break;
case DECISION_STRATEGIE:
// Obtenir l'objectif suivant
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]);
}
}
if(objectif_courant->etat == FAIT){
etat_strategie = STRATEGIE_FIN;
}else{
switch(objectif_courant->cible){
case CERISE_BAS:
etat_strategie = ALLER_CERISE_BAS;
break;
case CERISE_HAUT:
etat_strategie = ALLER_CERISE_HAUT;
break;
case CERISE_GAUCHE:
etat_strategie = STRATEGIE_FIN;
break;
case CERISE_DROITE:
etat_strategie = STRATEGIE_FIN;
break;
}
}
break;
case STRATEGIE_FIN:
i2c_annexe_desactive_propulseur();
commande_vitesse_stop();
@ -127,6 +226,71 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){
}
}
enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step_ms){
enum etat_action_t etat_action = ACTION_EN_COURS;
struct trajectoire_t trajectoire;
Trajet_config(500, 500);
// Definition des trajectoires
if(couleur == COULEUR_BLEU){
// Si le robot est déjà dans le quart haut gauche,
// On va en ligne droite
if(Robot_est_dans_quart_haut_gauche()){
Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
465,2830,
+30. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN);
}else{
// Sinon, on a une courbe de bézier
Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
485, Localisation_get().y_mm,
465, 857,
465,2830,
+30. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN);
}
}else{
// Si le robot est déjà dans le quart haut droit,
// On va en ligne droite
if(Robot_est_dans_quart_haut_droit()){
Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
2000-465, 2830,
-150. * DEGRE_EN_RADIAN, -240. * DEGRE_EN_RADIAN);
}else{
// Sinon, on a une courbe de bézier
Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
485, Localisation_get().y_mm,
2000-465, 857,
2000-465, 2830,
-150. * DEGRE_EN_RADIAN, -240. * DEGRE_EN_RADIAN);
}
}
// parcours du trajet
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_action = ACTION_TERMINEE;
}
return etat_action;
}
/// @brief Renvoi 1 si le robot est dans le quart supérieur gauche du terrain
/// @return 1 si oui, 0 si non.
int Robot_est_dans_quart_haut_gauche(){
if(Localisation_get().x_mm < 1000 && Localisation_get().y_mm > 1500){
return 1;
}
return 0;
}
/// @brief Renvoi 1 si le robot est dans le quart supérieur droit du terrain
/// @return 1 si oui, 0 si non.
int Robot_est_dans_quart_haut_droit(){
if(Localisation_get().x_mm > 1000 && Localisation_get().y_mm > 1500){
return 1;
}
return 0;
}
void Homologation(uint32_t step_ms){

1
Strategie_prise_cerises.c
View File

@ -109,6 +109,7 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
commande_vitesse_stop();
i2c_annexe_desactive_turbine();
etat_action = ACTION_TERMINEE;
etat_attrape = ATTRAPE_INIT;
break;
}
return etat_action;