diff --git a/CMakeLists.txt b/CMakeLists.txt index ce9f5f1..0f49776 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,6 +25,7 @@ Localisation.c Moteurs.c Monitoring.c Robot_config.c +Score.c Servomoteur.c Strategie.c Strategie_prise_cerises.c diff --git a/Holonome2023.c b/Holonome2023.c index 8c80ee0..abdc460 100644 --- a/Holonome2023.c +++ b/Holonome2023.c @@ -82,7 +82,7 @@ int main() { AsserMoteur_Init(); Localisation_init(); - //while(mode_test()); + while(mode_test()); i2c_maitre_init(); Trajet_init(); Balise_VL53L1X_init(); @@ -154,7 +154,7 @@ int main() { printf("MATCH_ARRET_EN_COURS\n"); statu_match = MATCH_ARRET_EN_COURS; } - Strategie(couleur, _step_ms); + Strategie(couleur, _step_ms, timer_match_ms); break; case MATCH_ARRET_EN_COURS: diff --git a/Score.c b/Score.c new file mode 100644 index 0000000..4509618 --- /dev/null +++ b/Score.c @@ -0,0 +1,40 @@ +#include "pico/stdlib.h" +#include "i2c_annexe.h" +#include "Score.h" + +#define SCORE_INITIAL 5 + +uint32_t Score_nb_cerises=0; +uint32_t Score_nb_points=0; +uint32_t Score_funny_action=0; +uint32_t Score_pieds_dans_plat=0; + +void Score_recalcule(){ + Score_nb_points = SCORE_INITIAL; + if(Score_nb_cerises > 0){ + Score_nb_points += Score_nb_cerises + 5; + } + Score_nb_points += Score_funny_action; + Score_nb_points += Score_pieds_dans_plat; + +} + +void Score_actualise(){ + Score_recalcule(); + i2c_annexe_envoie_score(Score_nb_points); +} + +void Score_set_funny_action(){ + Score_funny_action = 5; + Score_actualise(); +} + +void Score_set_pieds_dans_plat(){ + Score_pieds_dans_plat = 15; + Score_actualise(); +} + +void Score_ajout_cerise(uint32_t nb_cerises){ + Score_nb_cerises += nb_cerises; + Score_actualise(); +} diff --git a/Score.h b/Score.h new file mode 100644 index 0000000..01c1d50 --- /dev/null +++ b/Score.h @@ -0,0 +1,5 @@ +#include "pico/stdlib.h" + +void Score_set_funny_action(); +void Score_set_pieds_dans_plat(); +void Score_ajout_cerise(uint32_t nb_cerises); \ No newline at end of file diff --git a/Strategie.c b/Strategie.c index f05e53a..51728c4 100644 --- a/Strategie.c +++ b/Strategie.c @@ -6,6 +6,7 @@ #include "Geometrie_robot.h" #include "Localisation.h" #include "Moteurs.h" +#include "Score.h" #include "Strategie_prise_cerises.h" #include "Strategie.h" #include "Trajet.h" @@ -25,14 +26,13 @@ struct objectif_t{ } objectifs[NB_OBJECTIFS]; struct objectif_t * objectif_courant; -uint32_t Score_nb_cerises = 0; uint32_t Score_cerises_dans_robot=0; -void Score_set_cerise_dans_robot(uint32_t nb_cerises){ +void Strategie_set_cerise_dans_robot(uint32_t nb_cerises){ Score_cerises_dans_robot = nb_cerises; } -uint32_t Score_get_cerise_dans_robot(void){ +uint32_t Strategie_get_cerise_dans_robot(void){ return Score_cerises_dans_robot; } @@ -48,11 +48,13 @@ enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float int Robot_est_dans_quart_haut_gauche(); int Robot_est_dans_quart_haut_droit(); +int Robot_est_dans_zone_cerise_haut(enum couleur_t couleur); +int Robot_est_dans_zone_cerise_gauche(); enum etat_homologation_t etat_homologation=STRATEGIE_INIT; -void Strategie(enum couleur_t couleur, uint32_t step_ms){ +void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ float angle_fin; float recal_pos_x, recal_pos_y; @@ -136,7 +138,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){ etat_action = cerise_attraper_bordure(longer_direction, step_ms, recal_pos_x, recal_pos_y); if(etat_action == ACTION_TERMINEE){ - Score_set_cerise_dans_robot(6); + Strategie_set_cerise_dans_robot(6); etat_strategie = ALLER_PANIER; } break; @@ -157,7 +159,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){ if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){ etat_strategie = ATTRAPER_CERISE_HAUT; - Score_set_cerise_dans_robot(6); + Strategie_set_cerise_dans_robot(6); } break; @@ -214,7 +216,9 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){ break; case LANCER_PANIER: - if(lance_balles_dans_panier(couleur, step_ms, Score_get_cerise_dans_robot())== ACTION_TERMINEE){ + if(lance_balles_dans_panier(couleur, step_ms, Strategie_get_cerise_dans_robot())== ACTION_TERMINEE){ + Score_ajout_cerise(Strategie_get_cerise_dans_robot()); + Strategie_set_cerise_dans_robot(0); objectif_courant->etat = FAIT; etat_strategie = DECISION_STRATEGIE; } @@ -265,13 +269,16 @@ enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step // Definition des trajectoires if(couleur == COULEUR_BLEU){ - // Si le robot est déjà dans le quart haut gauche, + // Si le robot est déjà dans la zone cerise haut // On va en ligne droite - if(Robot_est_dans_quart_haut_gauche()){ + if(Robot_est_dans_zone_cerise_haut(couleur)){ Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 465,2830, Localisation_get().angle_radian, Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN)); + }else if (Robot_est_dans_zone_cerise_gauche()){ + + /// !!! TODO !!! }else{ // Sinon, on a une courbe de bézier Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, @@ -307,6 +314,8 @@ enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step } +/// Fonction de localisation aproximatives pour la stratégie. + /// @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(){ @@ -325,6 +334,28 @@ int Robot_est_dans_quart_haut_droit(){ return 0; } +int Robot_est_dans_zone_cerise_gauche(){ + if(Localisation_get().x_mm < 630 && + Localisation_get().y_mm >1000 && Localisation_get().y_mm < 2000){ + return 1; + } + return 0; + +} + +int Robot_est_dans_zone_cerise_haut(enum couleur_t couleur){ + if(couleur == COULEUR_BLEU){ + if(Localisation_get().y_mm > 2500 && Localisation_get().x_mm < 1000){ + return 1; + } + }else{ + if(Localisation_get().y_mm > 2500 && Localisation_get().x_mm > 1000){ + 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, diff --git a/Strategie.h b/Strategie.h index 2e82a73..3393595 100644 --- a/Strategie.h +++ b/Strategie.h @@ -54,7 +54,7 @@ enum couleur_t lire_couleur(void); uint attente_tirette(void); enum etat_action_t lance_balles(uint32_t step_ms, uint32_t nb_cerises); int test_panier(void); -void Strategie(enum couleur_t couleur, uint32_t step_ms); +void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms); 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); diff --git a/Strategie_prise_cerises.c b/Strategie_prise_cerises.c index 942dde2..cb23a05 100644 --- a/Strategie_prise_cerises.c +++ b/Strategie_prise_cerises.c @@ -7,6 +7,7 @@ #include "Localisation.h" #include "math.h" #include "i2c_annexe.h" +#include "Trajet.h" // Rotation en rad/s pour accoster les cerises #define ROTATION_CERISE 0.5f @@ -18,9 +19,12 @@ // Translation en mm/s pour aspirer les cerises #define TRANSLATION_CERISE 70 +// Seuil angulaire pour la détection de la fin de la barrette de cerises +#define SEUIL_ANGLE_CERISE (2. * DEGRE_EN_RADIAN) + void commande_rotation_contacteur_longer_A(); void commande_rotation_contacteur_longer_C(); -enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum longer_direction_t longer_direction); +enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum longer_direction_t longer_direction, float pos_recal_x_mm); enum etat_action_t demarre_turbine(uint32_t step_ms); enum longer_direction_t inverser_longe_direction(enum longer_direction_t direction); @@ -39,10 +43,11 @@ enum etat_action_t cerises_attraper_cerises_gauches(uint32_t step_ms){ }etat_attrappe_cerises_gauche = ATTRAPE_CERISE_DEMI_BAS; struct trajectoire_t trajectoire; float angle_fin; + float pos_recal_x_mm = PETIT_RAYON_ROBOT_MM + 30; switch (etat_attrappe_cerises_gauche){ case ATTRAPE_CERISE_DEMI_BAS: - if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_A) == ACTION_TERMINEE){ + if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_A, pos_recal_x_mm) == ACTION_TERMINEE){ etat_attrappe_cerises_gauche = REVENIR_CENTRE; } break; @@ -59,7 +64,7 @@ enum etat_action_t cerises_attraper_cerises_gauches(uint32_t step_ms){ break; case ATTRAPE_CERISE_DEMI_HAUT: - if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_C) == ACTION_TERMINEE){ + if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_C, pos_recal_x_mm) == ACTION_TERMINEE){ etat_attrappe_cerises_gauche = ATTRAPE_CERISE_DEMI_BAS; return ACTION_TERMINEE; } @@ -70,7 +75,7 @@ enum etat_action_t cerises_attraper_cerises_gauches(uint32_t step_ms){ } -enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum longer_direction_t longer_direction){ +enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum longer_direction_t longer_direction, float pos_recal_x_mm){ // 1 accoster // Demarrer la turbine // 2 Longer en aspirant @@ -79,12 +84,19 @@ enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum ACCOSTAGE, DEMARRE_TURBINE, LONGE, + TOURNE, AVANCE, }etat_attrappe_demi_cerise=ACCOSTAGE; + const float continu_avance_mm = 60; + static float orientation_ref; + static float pos_y_ref; + struct trajectoire_t trajectoire; switch(etat_attrappe_demi_cerise){ case ACCOSTAGE: if(cerise_accostage() == ACTION_TERMINEE){ + Localisation_set_x(pos_recal_x_mm); + orientation_ref = Localisation_get().angle_radian; etat_attrappe_demi_cerise = DEMARRE_TURBINE; } break; @@ -101,7 +113,18 @@ enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum // Le robot n'a les deux contacteurs sur le support que tant qu'il est à moins de 240mm (MAX_LONGE_MM) de la bordure // ou 120 (MAX_LONGE_MM/2) du milieu de la bordure // En fonction du demi-terrain sur lequel se trouve le robot, on surveille la position en Y pour respecter cette condition - if( (Localisation_get().y_mm > 1500 + MAX_LONGE_MM/2 ) || (Localisation_get().y_mm < 1500 - MAX_LONGE_MM/2 )){ + /*if( (Localisation_get().y_mm > 1500 + MAX_LONGE_MM/2 ) || (Localisation_get().y_mm < 1500 - MAX_LONGE_MM/2 )){ + etat_attrappe_demi_cerise = AVANCE; + }*/ + if(fabs(orientation_ref - Localisation_get().angle_radian) > SEUIL_ANGLE_CERISE){ + etat_attrappe_demi_cerise = TOURNE; + pos_y_ref = Localisation_get().y_mm; + } + break; + + case TOURNE: + Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian, orientation_ref); + if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){ etat_attrappe_demi_cerise = AVANCE; } break; @@ -113,7 +136,7 @@ enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum commande_translation_longer_vers_C(); } - if( (Localisation_get().y_mm > 1500 + MAX_ASPIRE_CERISE_MM/2 ) || (Localisation_get().y_mm < 1500 - MAX_ASPIRE_CERISE_MM/2 )){ + if( (Localisation_get().y_mm > pos_y_ref + continu_avance_mm ) || (Localisation_get().y_mm < pos_y_ref - continu_avance_mm )){ etat_attrappe_demi_cerise = ACCOSTAGE; i2c_annexe_desactive_turbine(); commande_vitesse_stop(); diff --git a/Test_strategie.c b/Test_strategie.c index 8128052..487623c 100644 --- a/Test_strategie.c +++ b/Test_strategie.c @@ -139,7 +139,7 @@ int test_cerise_laterales(){ Trajet_init(); Balise_VL53L1X_init(); //printf("Init gyroscope\n"); - set_position_avec_gyroscope(0); + set_position_avec_gyroscope(1); if(get_position_avec_gyroscope()){ Gyro_Init(); }