diff --git a/Holonome2023.c b/Holonome2023.c index f50b9a2..5f77760 100644 --- a/Holonome2023.c +++ b/Holonome2023.c @@ -108,6 +108,7 @@ int main() { uint16_t _step_ms_gyro = 2; const uint16_t _step_ms = 1; static uint32_t timer_match_ms=0; + static uint32_t timer_match_init=0; static enum couleur_t couleur; // Surveillance du temps d'execution @@ -119,7 +120,7 @@ int main() { if(temps_ms != Temps_get_temps_ms()){ - timer_match_ms = timer_match_ms + _step_ms; + timer_match_ms = (time_us_32() - timer_match_init) / 1000; temps_ms = Temps_get_temps_ms(); QEI_update(); Localisation_gestion(); @@ -135,6 +136,7 @@ int main() { switch(statu_match){ case MATCH_ATTENTE_TIRETTE: + Strategie_preparation(); if(lire_couleur() == COULEUR_VERT){ // Tout vert i2c_annexe_couleur_balise(0b00011100, 0x0FFF); @@ -144,8 +146,9 @@ int main() { } if(attente_tirette() == 0){ statu_match = MATCH_EN_COURS; + timer_match_init = time_us_32(); couleur = lire_couleur(); - timer_match_ms=0; + timer_match_ms = 0; i2c_annexe_couleur_balise(0, 0x00); score=5; i2c_annexe_envoie_score(score); @@ -164,7 +167,7 @@ int main() { commande_vitesse_stop(); i2c_annexe_active_deguisement(); Score_set_funny_action(); - if(Robot_est_dans_zone_depose()){ + if(Robot_est_dans_zone_depose(couleur)){ Score_set_pieds_dans_plat(); } diff --git a/Strategie.c b/Strategie.c index 495aef8..391fd70 100644 --- a/Strategie.c +++ b/Strategie.c @@ -49,6 +49,8 @@ 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_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t couleur, uint32_t step_ms); +enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t couleur, uint32_t step_ms); enum etat_homologation_t etat_homologation=STRATEGIE_INIT; @@ -156,7 +158,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 175, Localisation_get().angle_radian, angle_fin); - if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){ + if(parcourt_trajet_simple(trajectoire, step_ms) == TRAJET_TERMINE){ etat_strategie = ATTRAPER_CERISE_HAUT; } break; @@ -182,18 +184,13 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ Trajet_config(300, 250); angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN); if(couleur == COULEUR_BLEU){ - Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, - 740, 3000 - 550, - 510, 3000 - 1580, - 180, 1500, - Localisation_get().angle_radian, angle_fin); + if(Strategie_aller_cerises_laterales_proches(couleur, step_ms)== ACTION_TERMINEE){ + etat_strategie = ATTRAPER_CERISE_GAUCHE; + } }else{ - Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, - 1225, 3000 - 540, - 440, 3000 - 775, - 225, 3000 - (1500 - 45), - Localisation_get().angle_radian, angle_fin); - + if(Strategie_aller_cerises_laterales_opposees(couleur, step_ms)== ACTION_TERMINEE){ + etat_strategie = ATTRAPER_CERISE_GAUCHE; + } } if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ @@ -208,6 +205,32 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ etat_strategie = ALLER_PANIER; } break; + + case ALLER_CERISE_DROITE: + Trajet_config(300, 250); + angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN); + if(couleur == COULEUR_BLEU){ + if(Strategie_aller_cerises_laterales_proches(couleur, step_ms)== ACTION_TERMINEE){ + etat_strategie = ATTRAPER_CERISE_DROITE; + } + }else{ + if(Strategie_aller_cerises_laterales_opposees(couleur, step_ms)== ACTION_TERMINEE){ + etat_strategie = ATTRAPER_CERISE_DROITE; + } + } + + if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ + etat_strategie = ATTRAPER_CERISE_GAUCHE; + Trajet_config(500, 500); + } + break; + + case ATTRAPER_CERISE_DROITE: + if(cerises_attraper_cerises_droite(step_ms) == ACTION_TERMINEE){ + Strategie_set_cerise_dans_robot(10); + etat_strategie = ALLER_PANIER; + } + break; case ALLER_PANIER: if(Strategie_aller_panier(couleur, step_ms) == ACTION_TERMINEE){ @@ -263,6 +286,51 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ } } +enum etat_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t couleur, uint32_t step_ms){ + struct trajectoire_t trajectoire; + float angle_fin; + if(couleur == COULEUR_BLEU){ + angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN); + Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, + 740, 3000 - 550, + 510, 3000 - 1580, + 180, 1500, + Localisation_get().angle_radian, angle_fin); + }else{ + angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN); + Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, + 2000 - 740, 3000 - 550, + 2000 - 510, 3000 - 1580, + 2000 - 180, 1500, + Localisation_get().angle_radian, angle_fin); + + } + return parcourt_trajet_simple(trajectoire, step_ms); +} + +enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t couleur, uint32_t step_ms){ + struct trajectoire_t trajectoire; + float angle_fin; + if(couleur == COULEUR_BLEU){ + angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN); + Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, + 785, 3000 - 550, + 1550, 3000 - 785, + 1775, 1500, + Localisation_get().angle_radian, angle_fin); + }else{ + angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN); + Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, + 2000 - 785, 3000 - 550, + 2000 - 1550, 3000 - 785, + 2000 - 1775, 1500, + Localisation_get().angle_radian, angle_fin); + + } + return parcourt_trajet_simple(trajectoire, 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; @@ -451,7 +519,7 @@ enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t ste longer_direction = LONGER_VERS_C; } - if(calage_angle(longer_direction, recal_pos_x, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2.)), 120. *DEGRE_EN_RADIAN) == ACTION_TERMINEE){ + if(calage_angle(longer_direction, recal_pos_x, 3000 - (PETIT_RAYON_ROBOT_MM), 120. *DEGRE_EN_RADIAN) == ACTION_TERMINEE){ etat_lance_balles_dans_panier = RECULE_PANIER; } break; @@ -639,4 +707,48 @@ int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms){ *tempo_ms -= step_ms; return 0; } +} + +/// @brief Gère le chargement des balles au début du match +void Strategie_preparation(){ + + static enum{ + PREP_INIT_V, + PREP_INIT_B, + PREP_ASPIRE, + PREP_ARRET_ASPIRATION, + PREP_TEMPO, + } etat_prep=PREP_INIT_V; + + switch(etat_prep){ + case PREP_INIT_V: + if(lire_couleur()== COULEUR_VERT){ + etat_prep = PREP_INIT_B; + } + break; + case PREP_INIT_B: + if(lire_couleur()== COULEUR_BLEU){ + etat_prep = PREP_ASPIRE; + } + break; + + case PREP_ASPIRE: + if(lire_couleur()== COULEUR_VERT){ + i2c_annexe_ferme_porte(); + i2c_annexe_active_turbine(); + etat_prep=PREP_ARRET_ASPIRATION; + } + break; + + case PREP_ARRET_ASPIRATION: + if(lire_couleur()== COULEUR_BLEU){ + i2c_annexe_desactive_turbine(); + etat_prep=PREP_TEMPO; + } + break; + + case PREP_TEMPO: + break; + } + } \ No newline at end of file diff --git a/Strategie.h b/Strategie.h index 3678a6b..6a789f8 100644 --- a/Strategie.h +++ b/Strategie.h @@ -67,6 +67,8 @@ 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); 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); +void Strategie_preparation(void); extern float distance_obstacle; diff --git a/Strategie_prise_cerises.c b/Strategie_prise_cerises.c index cb23a05..4658788 100644 --- a/Strategie_prise_cerises.c +++ b/Strategie_prise_cerises.c @@ -35,6 +35,46 @@ float vitesse_accostage_mm_s=100; +enum etat_action_t cerises_attraper_cerises_droite(uint32_t step_ms){ + static enum { + ATTRAPE_CERISE_DEMI_BAS, + REVENIR_CENTRE, + ATTRAPE_CERISE_DEMI_HAUT, + }etat_attrappe_cerises_droite = 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_droite){ + case ATTRAPE_CERISE_DEMI_BAS: + if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_C, pos_recal_x_mm) == ACTION_TERMINEE){ + etat_attrappe_cerises_droite = REVENIR_CENTRE; + } + break; + + case REVENIR_CENTRE: + angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 30 * DEGRE_EN_RADIAN); + + Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, + 2000 - 180, 1500 - 75, Localisation_get().angle_radian, angle_fin); + + if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ + etat_attrappe_cerises_droite = ATTRAPE_CERISE_DEMI_HAUT; + } + break; + + case ATTRAPE_CERISE_DEMI_HAUT: + if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_A, pos_recal_x_mm) == ACTION_TERMINEE){ + etat_attrappe_cerises_droite = ATTRAPE_CERISE_DEMI_BAS; + return ACTION_TERMINEE; + } + break; + + } + return ACTION_EN_COURS; + +} + enum etat_action_t cerises_attraper_cerises_gauches(uint32_t step_ms){ static enum { ATTRAPE_CERISE_DEMI_BAS, diff --git a/Strategie_prise_cerises.h b/Strategie_prise_cerises.h index c7ff9f5..6cf66d1 100644 --- a/Strategie_prise_cerises.h +++ b/Strategie_prise_cerises.h @@ -1,6 +1,7 @@ #include "Strategie.h" enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, float pos_x_mm, float pos_y_mm); enum etat_action_t cerises_attraper_cerises_gauches(uint32_t step_ms); +enum etat_action_t cerises_attraper_cerises_droite(uint32_t step_ms); void commande_translation_longer_vers_A(); void commande_translation_longer_vers_C(); void commande_translation_avance_vers_trompe(); diff --git a/Test_strategie.c b/Test_strategie.c index 028cdc3..300aedf 100644 --- a/Test_strategie.c +++ b/Test_strategie.c @@ -26,7 +26,8 @@ int test_panier(void); int test_homologation(void); int test_evitement(void); int test_tirette_et_couleur(); -int test_cerise_laterales(void); +int test_cerise_laterales_droite(void); +int test_cerise_laterales_gauche(void); void affichage_test_evitement(); void affichage_test_strategie(){ @@ -98,7 +99,7 @@ int test_strategie(){ case 'd': case 'D': - while(test_cerise_laterales()); + while(test_cerise_laterales_droite()); break; case 'e': @@ -106,6 +107,11 @@ int test_strategie(){ while(test_evitement()); break; + case 'g': + case 'G': + while(test_cerise_laterales_gauche()); + break; + case 'h': case 'H': //while(test_homologation()); @@ -130,7 +136,7 @@ int test_strategie(){ } -int test_cerise_laterales(){ +int test_cerise_laterales_gauche(){ int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init; uint32_t temps_cycle_old; enum etat_action_t etat_action; @@ -217,6 +223,91 @@ int test_cerise_laterales(){ } +int test_cerise_laterales_droite(){ + int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init; + uint32_t temps_cycle_old; + enum etat_action_t etat_action; + printf("Attaper cerise latérales\n"); + + i2c_maitre_init(); + Trajet_init(); + Balise_VL53L1X_init(); + //printf("Init gyroscope\n"); + set_position_avec_gyroscope(1); + if(get_position_avec_gyroscope()){ + Gyro_Init(); + } + + stdio_flush(); + + multicore_launch_core1(affichage_test_strategie); + + temps_ms = Temps_get_temps_ms(); + temps_ms_init = temps_ms; + temps_cycle_old= time_us_32(); + + uint32_t tempo_ms=1000; + + Localisation_set(1775, 1500, 30 * DEGRE_EN_RADIAN); + do{ + etat_action = ACTION_EN_COURS; + + temps_cycle_check(); + + i2c_gestion(i2c0); + i2c_annexe_gestion(); + Balise_VL53L1X_gestion(); + + // Routines à 1 ms + + if(temps_ms != Temps_get_temps_ms()){ + static enum { + TEMPO_AVANT, + TEST, + TEMPO_APRES + }etat_test; + temps_ms = Temps_get_temps_ms(); + QEI_update(); + Localisation_gestion(); + AsserMoteur_Gestion(_step_ms); + + + // Routine à 2 ms + if(temps_ms % _step_ms_gyro == 0){ + if(get_position_avec_gyroscope()){ + Gyro_Read(_step_ms_gyro); + } + } + + switch(etat_test){ + case TEMPO_AVANT: + if(temporisation_terminee(&tempo_ms, _step_ms)){ + etat_test = TEST; + } + break; + case TEST: + if(cerises_attraper_cerises_droite(_step_ms) == ACTION_TERMINEE){ + tempo_ms = 1000; + etat_test = TEMPO_APRES; + } + break; + case TEMPO_APRES: + if(temporisation_terminee(&tempo_ms, _step_ms)){ + etat_test = TEMPO_AVANT; + etat_action = ACTION_TERMINEE; + } + break; + } + + } + //lettre = getchar_timeout_us(0); + //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0)); + }while(etat_action == ACTION_EN_COURS); + Moteur_Stop(); + return 0; + +} + int test_homologation(){ int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init; uint32_t temps_cycle[10], temps_cycle_old, index_temps_cycle=0;