diff --git a/Strategie.c b/Strategie.c index c74f7ff..053db10 100644 --- a/Strategie.c +++ b/Strategie.c @@ -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){ diff --git a/Strategie_prise_cerises.c b/Strategie_prise_cerises.c index 5834e46..b2b2c80 100644 --- a/Strategie_prise_cerises.c +++ b/Strategie_prise_cerises.c @@ -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;