diff --git a/Holonome2023.c b/Holonome2023.c index af9ef40..d6c85a7 100644 --- a/Holonome2023.c +++ b/Holonome2023.c @@ -21,6 +21,7 @@ #include "Moteurs.h" #include "QEI.h" #include "Robot_config.h" +#include "Score.h" #include "Servomoteur.h" #include "spi_nb.h" #include "Strategie.h" @@ -162,6 +163,11 @@ int main() { case MATCH_ARRET_EN_COURS: commande_vitesse_stop(); i2c_annexe_active_deguisement(); + Score_set_funny_action(); + if(Robot_est_dans_zone_depose()){ + Score_set_pieds_dans_plat(); + } + if (timer_match_ms > 100000){ statu_match = MATCH_TERMINEE; } diff --git a/Strategie.c b/Strategie.c index 3b35b1c..7bb13e2 100644 --- a/Strategie.c +++ b/Strategie.c @@ -369,6 +369,62 @@ int Robot_est_dans_zone_cerise_haut(enum couleur_t couleur){ 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. +int Robot_est_dans_zone_depose(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; + } + // Zone 2 + if(x_mm < 550 && y_mm > 800 && y_mm < 1450){ + return 1; + } + // Zone 3 + if(x_mm > 400 && x_mm < 1050 && y_mm < 550){ + return 1; + } + // Zone 4 + if(x_mm > 1450 && y_mm < 550){ + return 1; + } + // Zone 5 + if(x_mm > 1450 && y_mm > 1550 && y_mm < 2200){ + return 1; + } + return 0; + + }else{ + // VERT + // Zone 1 + if(x_mm > (2000 - 550) && y_mm > 2450){ + return 1; + } + // Zone 2 + if(x_mm > (2000 - 550) && y_mm > 800 && y_mm < 1450){ + return 1; + } + // Zone 3 + if(x_mm < (2000 - 400) && x_mm > 2000 - 1050 && y_mm < 550){ + return 1; + } + // Zone 4 + if(x_mm < (2000 - 1450) && y_mm < 550){ + return 1; + } + // Zone 5 + if(x_mm < (2000 - 1450) && y_mm > 1550 && y_mm < 2200){ + return 1; + } + return 0; + } +} + enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms, uint32_t nb_cerises){ static enum { CALAGE_PANIER_1,