diff --git a/Holonome2023.c b/Holonome2023.c index 8c80ee0..69a154a 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(); diff --git a/Strategie.c b/Strategie.c index cce6bb5..a1b9df5 100644 --- a/Strategie.c +++ b/Strategie.c @@ -176,6 +176,38 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){ etat_strategie = ALLER_PANIER; } break; + + case ALLER_CERISE_GAUCHE: + 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, 3000 - (1500 - 45), + Localisation_get().angle_radian, angle_fin); + }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(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ + etat_strategie = ATTRAPER_CERISE_GAUCHE; + } + break; + + case ATTRAPER_CERISE_GAUCHE: + // 1 accoster + // 2 Longer en aspirant + // 3 avancer en aspirant + // 4 Revenir au milieu + // 5 accoster + // 6 longer en aspirant + // 7 avancer en aspirant + break; case ALLER_PANIER: if(Strategie_aller_panier(couleur, step_ms) == ACTION_TERMINEE){ diff --git a/Strategie_prise_cerises.c b/Strategie_prise_cerises.c index 02e445d..942dde2 100644 --- a/Strategie_prise_cerises.c +++ b/Strategie_prise_cerises.c @@ -20,12 +20,111 @@ 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 demarre_turbine(uint32_t step_ms); enum longer_direction_t inverser_longe_direction(enum longer_direction_t direction); float vitesse_accostage_mm_s=100; + + + +enum etat_action_t cerises_attraper_cerises_gauches(uint32_t step_ms){ + static enum { + ATTRAPE_CERISE_DEMI_BAS, + REVENIR_CENTRE, + ATTRAPE_CERISE_DEMI_HAUT, + }etat_attrappe_cerises_gauche = ATTRAPE_CERISE_DEMI_BAS; + struct trajectoire_t trajectoire; + float angle_fin; + + switch (etat_attrappe_cerises_gauche){ + case ATTRAPE_CERISE_DEMI_BAS: + if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_A) == ACTION_TERMINEE){ + etat_attrappe_cerises_gauche = REVENIR_CENTRE; + } + break; + + case REVENIR_CENTRE: + angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150 * DEGRE_EN_RADIAN); + + Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, + 180, 1500 - 75, Localisation_get().angle_radian, angle_fin); + + if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ + etat_attrappe_cerises_gauche = ATTRAPE_CERISE_DEMI_HAUT; + } + break; + + case ATTRAPE_CERISE_DEMI_HAUT: + if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_C) == ACTION_TERMINEE){ + etat_attrappe_cerises_gauche = ATTRAPE_CERISE_DEMI_BAS; + return ACTION_TERMINEE; + } + break; + + } + return ACTION_EN_COURS; + +} + +enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum longer_direction_t longer_direction){ + // 1 accoster + // Demarrer la turbine + // 2 Longer en aspirant + // 3 avancer en aspirant + static enum { + ACCOSTAGE, + DEMARRE_TURBINE, + LONGE, + AVANCE, + }etat_attrappe_demi_cerise=ACCOSTAGE; + + switch(etat_attrappe_demi_cerise){ + case ACCOSTAGE: + if(cerise_accostage() == ACTION_TERMINEE){ + etat_attrappe_demi_cerise = DEMARRE_TURBINE; + } + break; + + case DEMARRE_TURBINE: + if(demarre_turbine(step_ms) == ACTION_TERMINEE){ + etat_attrappe_demi_cerise = LONGE; + } + break; + + case LONGE: + avance_puis_longe_bordure(longer_direction); + // La fonction cerise_longer_bord n'est efficace que tant que le robot a ses deux contacteur sur le support + // 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 )){ + etat_attrappe_demi_cerise = AVANCE; + } + break; + + case AVANCE: + if(longer_direction == LONGER_VERS_A){ + commande_translation_longer_vers_A(); + }else{ + 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 )){ + etat_attrappe_demi_cerise = ACCOSTAGE; + i2c_annexe_desactive_turbine(); + commande_vitesse_stop(); + return ACTION_TERMINEE; + } + break; + } + return ACTION_EN_COURS; + +} + /// @brief Fonction pour attraper les cerises sur les supports perpendiculaires à la bordure. /// Le robot accoste, longe le support cerise vers la bordure, active la turbine, puis longe le support cerise jusqu'à son bout. /// @param longer_direction : direction dans laquelle se trouve la bordure @@ -84,7 +183,7 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct avance_puis_longe_bordure(longer_direction_aspire); // La fonction cerise_longer_bord n'est efficace que tant que le robot a ses deux contacteur sur le support // Le robot n'a les deux contacteurs sur le support que tant qu'il est à moins de 240mm (MAX_LONGE_MM) de la bordure - // En fonction du demi-terrain sur lequel se trouve le robot, on surveille la position en Z pour respecter cette condition + // 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) && (Localisation_get().y_mm < (3000 - MAX_LONGE_MM) )) || ((Localisation_get().y_mm < 1500) && (Localisation_get().y_mm > (MAX_LONGE_MM))) ){ etat_attrape = ASPIRE_LIBRE; @@ -114,6 +213,37 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct return etat_action; } +/// @brief Envoie l'ordre de démarrer la turbine puis attends 1 seconde +/// @param step_ms +/// @return ACTION_EN_COURS ou ACTION_TERMINEE +enum etat_action_t demarre_turbine(uint32_t step_ms){ + static enum { + TURBINE_DEMARRAGE, + TURBINE_DEMARRAGE_TEMPO, + } etat_demarrage_turbine=TURBINE_DEMARRAGE; + static uint32_t tempo_ms; + + switch(etat_demarrage_turbine){ + case TURBINE_DEMARRAGE: + i2c_annexe_ferme_porte(); + i2c_annexe_active_turbine(); + commande_vitesse_stop(); + tempo_ms = 1000; + etat_demarrage_turbine = TURBINE_DEMARRAGE_TEMPO; + + break; + + case TURBINE_DEMARRAGE_TEMPO: + if(temporisation_terminee(&tempo_ms, step_ms)){ + etat_demarrage_turbine = TURBINE_DEMARRAGE; + return ACTION_TERMINEE; + } + + break; + } + return ACTION_EN_COURS; + +} /// @brief Fonction pour accoster et longer une bordure /// @param longer_direction : direction dans laquelle le robot va aller une fois le long de la bordure @@ -159,7 +289,7 @@ enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_dire } -/// @brief Viens position le robot contre une bordure ou un support cerise devant lui. +/// @brief Viens positionner le robot contre une bordure ou un support cerise devant lui. enum etat_action_t cerise_accostage(void){ enum etat_action_t etat_action = ACTION_EN_COURS; float rotation; diff --git a/Strategie_prise_cerises.h b/Strategie_prise_cerises.h index f343720..c7ff9f5 100644 --- a/Strategie_prise_cerises.h +++ b/Strategie_prise_cerises.h @@ -1,5 +1,6 @@ #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); 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 f7f8544..8128052 100644 --- a/Test_strategie.c +++ b/Test_strategie.c @@ -13,6 +13,7 @@ #include "QEI.h" #include "Robot_config.h" #include "Strategie.h" +#include "Strategie_prise_cerises.h" #include "Temps.h" #include "Trajet.h" #include "Trajectoire.h" @@ -24,6 +25,7 @@ int test_panier(void); int test_homologation(void); int test_evitement(void); int test_tirette_et_couleur(); +int test_cerise_laterales(void); void affichage_test_evitement(); void affichage_test_strategie(){ @@ -72,6 +74,7 @@ void affichage_test_strategie(){ int test_strategie(){ printf("A - Accoster.\n"); printf("C - Couleur et tirette.\n"); + printf("D - Attraper cerises laterales.\n"); printf("E - Evitement\n"); printf("H - Homologation.\n"); printf("L - Longer.\n"); @@ -92,6 +95,11 @@ int test_strategie(){ while(test_tirette_et_couleur()); break; + case 'd': + case 'D': + while(test_cerise_laterales()); + break; + case 'e': case 'E': while(test_evitement()); @@ -99,7 +107,7 @@ int test_strategie(){ case 'h': case 'H': - while(test_homologation()); + //while(test_homologation()); break; case 'l': @@ -121,6 +129,93 @@ int test_strategie(){ } +int test_cerise_laterales(){ + 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(0); + 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(250, 1500, -150 * 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_gauches(_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; @@ -174,7 +269,7 @@ int test_homologation(){ } } - Homologation(_step_ms); + //Homologation(_step_ms); } //lettre = getchar_timeout_us(0); diff --git a/i2c_annexe.c b/i2c_annexe.c index 5ccf066..798e596 100644 --- a/i2c_annexe.c +++ b/i2c_annexe.c @@ -73,10 +73,8 @@ void i2c_annexe_couleur_balise(uint8_t couleur, uint16_t masque_led){ } void i2c_annexe_active_turbine(void){ - /* donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x01; donnees_a_envoyer=1; - */ } void i2c_annexe_desactive_turbine(void){ donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xFE;