diff --git a/Monitoring.c b/Monitoring.c index e6f7797..c2d3e40 100644 --- a/Monitoring.c +++ b/Monitoring.c @@ -5,6 +5,7 @@ #include "Trajet.h" #include "Asser_Moteurs.h" #include "i2c_annexe.h" +#include "Evitement.h" #define LED_PIN_ROUGE 28 @@ -45,6 +46,7 @@ void Monitoring_display(){ printf(">V_bat:%ld:%2.1f\n", temps, (float) (i2c_annexe_get_tension_batterie() / 10.)); printf(">DebugVar:%ld:%d\n", temps, debug_var); printf(">Distance_Obstacle:%ld:%f\n", temps, Trajet_get_obstacle_mm()); + printf(">Etat_Evitement:%ld:%d\n", temps, Evitement_get_statu()); printf(">DebugVarf(abscisse):%ld:%f\n", temps, debug_varf); struct position_t position = Localisation_get(); printf(">pos_x:%ld:%f\n", temps, position.x_mm); diff --git a/Strategie.c b/Strategie.c index 9ed7e85..05eefbc 100644 --- a/Strategie.c +++ b/Strategie.c @@ -52,8 +52,7 @@ 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); + const uint32_t temps_pre_fin_match = (97000 - 15000); static bool pre_fin_match_active=false; float angle_fin; float recal_pos_x, recal_pos_y; @@ -85,7 +84,7 @@ 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(temps_ms > temps_pre_fin_match && pre_fin_match_active == false){ if(etat_strategie != LANCER_PANIER){ etat_strategie = RETOUR_MAISON; } @@ -98,7 +97,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ Localisation_set(225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN); struct objectif_t objectif_1 = { .priorite = 10, .etat = FAIT, .cible = CERISE_BAS}; struct objectif_t objectif_2 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT}; - struct objectif_t objectif_3 = { .priorite = 2, .etat = FAIT, .cible = CERISE_GAUCHE}; + struct objectif_t objectif_3 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_GAUCHE}; struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_DROITE}; objectifs[0]= objectif_1; objectifs[1]= objectif_2; @@ -108,7 +107,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ Localisation_set(2000 - 225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN); struct objectif_t objectif_1 = { .priorite = 10, .etat = FAIT, .cible = CERISE_BAS}; struct objectif_t objectif_2 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT}; - struct objectif_t objectif_3 = { .priorite = 2, .etat = FAIT, .cible = CERISE_DROITE}; + struct objectif_t objectif_3 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_DROITE}; struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_GAUCHE}; objectifs[0]= objectif_1; objectifs[1]= objectif_2; @@ -291,8 +290,22 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ } break; + + case RETOUR_MAISON: + if(Strategie_pieds_dans_plat(couleur, step_ms) == ACTION_TERMINEE){ + // Si le robot est dans la zone du panier, jeter les cerises s'il en a + if(Strategie_get_cerise_dans_robot() > 0 && Robot_est_dans_zone_depose_panier(couleur)){ + //etat_strategie=LANCER_PANIER; // Il faut orienter le robot face au panier + etat_strategie=STRATEGIE_FIN; + }else{ + etat_strategie=STRATEGIE_FIN; + } + } + break; + case STRATEGIE_FIN: + pre_fin_match_active=true; i2c_annexe_desactive_propulseur(); commande_vitesse_stop(); i2c_annexe_couleur_balise(0xE0, 0x0FFF); @@ -459,6 +472,25 @@ int Robot_est_dans_zone_cerise_haut(enum couleur_t couleur){ return 0; } + +int Robot_est_dans_zone_depose_panier(enum couleur_t couleur){ + float x_mm, y_mm; + x_mm = Localisation_get().x_mm; + y_mm = Localisation_get().y_mm; + if(couleur == COULEUR_BLEU){ + // Zone 1 + if(x_mm < 550 && y_mm > 2450){ + return 1; + } + }else{ + // Zone 1 + if(x_mm > 2000 - 550 && y_mm > 2450){ + return 1; + } + } + return 0; +} + /// @brief Renvoi 1 si le robot intersect une zone de dépose /// @param couleur du robot /// @return 1 si oui, 0 si non. @@ -787,20 +819,23 @@ enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, uint32_t step_ms) 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]; + static struct objectif_t objectifs_plats[5], *objectif_plat_courant=NULL; enum etat_action_t etat_action; + //Trajet_config(500,500); //750, 500 => Ne marche pas - - 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; + if(objectif_plat_courant==NULL){ + objectif_plat_courant = &objectifs_plats[0]; + struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = ZONE_1}; + struct objectif_t objectif_2 = { .priorite = 3, .etat = A_FAIRE, .cible = ZONE_2}; + struct objectif_t objectif_3 = { .priorite = 5, .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 = 4, .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){ @@ -829,7 +864,7 @@ enum etat_action_t Strategie_pieds_dans_plat(enum couleur_t couleur, uint32_t st // 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){ + if(objectif_plat_courant->etat == BLOQUE && 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){ @@ -838,7 +873,7 @@ enum etat_action_t Strategie_pieds_dans_plat(enum couleur_t couleur, uint32_t st } } - + return ACTION_EN_COURS; } diff --git a/Strategie.h b/Strategie.h index 6e901a2..8b23344 100644 --- a/Strategie.h +++ b/Strategie.h @@ -57,6 +57,7 @@ int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms); enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step_ms); enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, uint32_t step_ms, enum evitement_t evitement); int Robot_est_dans_zone_depose(enum couleur_t couleur); +int Robot_est_dans_zone_depose_panier(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); diff --git a/Strategie_deplacement.c b/Strategie_deplacement.c index f841151..a3c5317 100644 --- a/Strategie_deplacement.c +++ b/Strategie_deplacement.c @@ -27,7 +27,7 @@ enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, Trajet_set_obstacle_mm(distance_obstacle); } - if(Evitement_get_statu() == ARRET_DEVANT_OBSTACLE){ + if(Evitement_get_statu() == OBSTACLE_CONFIRME || Evitement_get_statu() == OBSTACLE_NON_CONFIRME){ switch(evitement){ case SANS_EVITEMENT: //printf("Evitement lors trajet SANS_EVITEMENT: ERREUR\n");